云法向量估計(jì)的體素化方案：從PCL常規(guī)思路到大規(guī)模場(chǎng)景實(shí)戰(zhàn))
做點(diǎn)云處理這么多年我一直覺(jué)得法向量估計(jì)是“最容易忽視但最影響后續(xù)效果”的一步。很多同學(xué)一開(kāi)始接觸 PCLPoint Cloud Library上來(lái)就是pcl::NormalEstimationK近鄰一設(shè)、法向量一出看著可視化里五顏六色的線(xiàn)條還挺開(kāi)心結(jié)果一去做 ICP 配準(zhǔn)或者移動(dòng)立方體重建各種問(wèn)題就開(kāi)始冒出來(lái)。后來(lái)我在處理大規(guī)模場(chǎng)景點(diǎn)云時(shí)逐漸摸出一套“體素法向量Voxel-based Normal”的做法不去糾纏每個(gè)點(diǎn)單獨(dú)找鄰居而是先把點(diǎn)云按空間切成小格子再讓每個(gè)格子里的點(diǎn)一起“投票”算出朝向。今天這篇文章就把這套方法徹底講透。如果你手頭的數(shù)據(jù)是百萬(wàn)級(jí)以上的大場(chǎng)景、密度忽高忽低、或者噪聲明顯又不想在法向量這一步花掉太多時(shí)間那這篇文章應(yīng)該能給你一個(gè)非?？陕涞氐奶娲桨浮２还苣闶亲黾す饫走_(dá)點(diǎn)云、三維重建還是機(jī)械視覺(jué)的定位抓取這套思路都通用。1. 常規(guī)法向量估計(jì)為什么在大場(chǎng)景下容易“翻車(chē)”1.1 K近鄰與半徑搜索各自的“局部性”悖論pcl::NormalEstimation最常用的方式有兩種K近鄰搜索和半徑搜索。K近鄰的邏輯很簡(jiǎn)單以每個(gè)點(diǎn)為中心找最近的 K 個(gè)點(diǎn)對(duì)這 K 個(gè)點(diǎn)做 PCA。聽(tīng)著很合理但實(shí)際用起來(lái)有兩個(gè)隱藏問(wèn)題。第一K 是固定數(shù)量但點(diǎn)云密度是變化的。在密集區(qū)域K 個(gè)點(diǎn)可能只覆蓋了一小塊表面法向量會(huì)被表面上的微小起伏帶偏在稀疏區(qū)域K 個(gè)點(diǎn)可能跨越了好幾個(gè)結(jié)構(gòu)邊緣比如墻角、桌面和椅子扶手混在一起法向量直接被“平均”成一個(gè)四不像的方向。第二每個(gè)點(diǎn)都要在 KD-Tree 里獨(dú)立做一次近鄰搜索。點(diǎn)數(shù)少的時(shí)候還好一旦到了百萬(wàn)甚至千萬(wàn)量級(jí)單次搜索的 logN 開(kāi)銷(xiāo)累加起來(lái)非?？捎^而且 KD-Tree 索引本身還要吃掉大量?jī)?nèi)存。我見(jiàn)過(guò)不少項(xiàng)目的法向量計(jì)算直接成了整個(gè)處理管線(xiàn)的瓶頸。半徑搜索setRadiusSearch其實(shí)是在用“固定的空間尺度”換“鄰居數(shù)量可變”比 K 近鄰更貼近物理直覺(jué)。但它在密度稀疏的區(qū)域經(jīng)常出問(wèn)題——鄰居數(shù)量不滿(mǎn) 3 個(gè)PCA 根本沒(méi)法做最后返回一堆 NaN即便湊夠了點(diǎn)數(shù)因?yàn)辄c(diǎn)分布太稀疏、跨度過(guò)大法向量也很不穩(wěn)定。這等于剛從“密度不均”的坑里跳出來(lái)又掉進(jìn)“局部空洞”的坑里。1.2 體素思路先給空間畫(huà)格子再讓點(diǎn)“對(duì)號(hào)入座”把這兩種常規(guī)思路放在一起看你會(huì)發(fā)現(xiàn)它們本質(zhì)上都在做同一件事按某種規(guī)則找鄰居。問(wèn)題不在于“找鄰居”這個(gè)動(dòng)作而在于找鄰居的規(guī)則太受密度影響。體素法向量換了一個(gè)切入點(diǎn)不管點(diǎn)在哪我先用固定邊長(zhǎng)的立方體把整個(gè)場(chǎng)景空間切開(kāi)然后按每個(gè)點(diǎn)的坐標(biāo)把它分配進(jìn)屬于自己的那個(gè)格子。“先畫(huà)格子再分點(diǎn)”有幾個(gè)天然優(yōu)勢(shì)。第一點(diǎn)云的鄰居關(guān)系變成了格子之間的空間鄰接關(guān)系理解和操作都更直觀。第二每個(gè)格子的空間尺度是固定的無(wú)論點(diǎn)云局部密度怎么變參與計(jì)算的區(qū)域都保持一致。第三格子本質(zhì)是一個(gè)小型空間濾波器體素內(nèi)所有點(diǎn)一起參與計(jì)算能明顯壓制單點(diǎn)噪聲對(duì)法向量方向的干擾。這里得特別強(qiáng)調(diào)一下體素法和 PCL 里的VoxelGrid下采樣是兩回事。VoxelGrid是每個(gè)體素保留一個(gè)重心點(diǎn)或中心點(diǎn)目的是減少點(diǎn)數(shù)而體素法向量是讓每個(gè)體素內(nèi)的所有點(diǎn)共同參與 PCA目的是獲得更穩(wěn)的法向量。兩者可以配合使用但別混為一談。2. 體素法向量的核心原理從“點(diǎn)找鄰居”到“格子里求朝向”2.1 體素劃分的本質(zhì)一套虛擬整數(shù)坐標(biāo)索引點(diǎn)云本身是離散且無(wú)序的體素化本質(zhì)上就是給點(diǎn)云建立一套虛擬的整數(shù)坐標(biāo)索引。對(duì)任意一個(gè)點(diǎn) p先算它相對(duì)包圍盒最小點(diǎn)(min_x, min_y, min_z)的偏移量再除以體素邊長(zhǎng)voxel_size向下取整就得到了體素坐標(biāo)(ix, iy, iz)。ix floor((p.x - min_x) / voxel_size) iy floor((p.y - min_y) / voxel_size) iz floor((p.z - min_z) / voxel_size)有了這套坐標(biāo)你可以用哈希表、八叉樹(shù)或者三維數(shù)組來(lái)管理體素。三維數(shù)組最直觀但體素?cái)?shù)量通常達(dá)到幾十萬(wàn)甚至上百萬(wàn)直接開(kāi)數(shù)組內(nèi)存不劃算。哈希表是按需創(chuàng)建有多少個(gè)格子就建多少個(gè)桶內(nèi)存開(kāi)銷(xiāo)基本跟著點(diǎn)云的實(shí)際分布走大場(chǎng)景下更實(shí)用。八叉樹(shù)當(dāng)然也可以只是遍歷葉子節(jié)點(diǎn)的代碼會(huì)稍微繞一點(diǎn)后面我給的代碼里用的是哈希表方案。實(shí)際編碼時(shí)有一個(gè)容易被忽略的細(xì)節(jié)浮點(diǎn)數(shù)處在體素邊界上時(shí)可能因?yàn)榫仍颉疤钡礁舯诟褡?。比如某個(gè)點(diǎn)的坐標(biāo)剛好等于min_x k * voxel_size理論上它就在邊界上但取整結(jié)果受浮點(diǎn)誤差影響可能掉進(jìn)k-1格。這種“跳格子”雖然只發(fā)生在極少數(shù)邊界點(diǎn)上但后續(xù)算出來(lái)的法向量會(huì)出現(xiàn)不合理的毛刺。我的習(xí)慣是計(jì)算索引之前先給坐標(biāo)加一個(gè)極小的偏移量比如1e-6f代價(jià)幾乎為零效果卻很直接。2.2 體素內(nèi)的 PCA為什么最小特征值方向才是法向量體素收集完點(diǎn)之后法向量計(jì)算就是一個(gè)標(biāo)準(zhǔn)的 PCA 過(guò)程。先求點(diǎn)集的質(zhì)心 μ然后構(gòu)造協(xié)方差矩陣C Σ(p_i - μ)(p_i - μ)^T對(duì) C 做特征值分解得到三個(gè)特征值和對(duì)應(yīng)的三個(gè)特征向量。這里有一個(gè)特別常見(jiàn)、而且很多人最開(kāi)始都會(huì)搞反的點(diǎn)法向量取的是協(xié)方差矩陣最小特征值對(duì)應(yīng)的特征向量不是最大特征值。原因在于物理意義體素內(nèi)的點(diǎn)如果落在某個(gè)局部表面上點(diǎn)云在切平面內(nèi)的兩個(gè)方向上散布很大對(duì)應(yīng)較大的兩個(gè)特征值而垂直于表面的方向上點(diǎn)的變化非常小對(duì)應(yīng)最小的特征值。所以法向量本質(zhì)上是“點(diǎn)云散布最小的那個(gè)方向”。如果你還是不放心可以做個(gè)驗(yàn)證實(shí)驗(yàn)?zāi)靡黄秸膲γ纥c(diǎn)云體素化之后對(duì)任意一個(gè)格子做 PCA把三個(gè)特征向量打印出來(lái)。你會(huì)發(fā)現(xiàn)兩個(gè)大特征值對(duì)應(yīng)的向量幾乎平行于墻面而最小特征值對(duì)應(yīng)的向量垂直于墻面。這個(gè)結(jié)論一旦記住后面看代碼就不會(huì)再犯方向性錯(cuò)誤。另外還要注意協(xié)方差矩陣退化的情況。體素內(nèi)點(diǎn)數(shù)少于 3 時(shí)矩陣秩不夠求出的特征向量不穩(wěn)定即使點(diǎn)數(shù)夠如果點(diǎn)幾乎共線(xiàn)比如采集到一根細(xì)桿最小特征值和第二特征值會(huì)非常接近法向量方向就會(huì)高度敏感。這也是為什么“每個(gè)體素內(nèi)的最少點(diǎn)數(shù)”值得單獨(dú)拎出來(lái)當(dāng)一個(gè)重要的調(diào)參項(xiàng)。2.3 法向量的方向歧義PCA 只給方向不負(fù)責(zé)“朝向”P(pán)CA 給出的法向量只是一個(gè)帶符號(hào)的方向整體翻轉(zhuǎn) 180 度之后照樣滿(mǎn)足特征分解的要求。換句話(huà)說(shuō)同一塊平面上的點(diǎn)有的格子算出來(lái)法向量朝“外”有的格子朝“里”這是所有基于 PCA 的法向量估計(jì)方法都繞不開(kāi)的歧義問(wèn)題。最常用的修正是視點(diǎn)矯正。思路很簡(jiǎn)單法向量應(yīng)該大致指向傳感器方向。取一個(gè)視點(diǎn)坐標(biāo)通常是激光雷達(dá)或相機(jī)的位置計(jì)算視點(diǎn)到當(dāng)前點(diǎn)的方向向量如果法向量與這個(gè)方向向量的點(diǎn)積小于 0就把法向量翻轉(zhuǎn)。if (n · (viewpoint - p) 0) n -n;這個(gè)方法在單幀點(diǎn)云、視點(diǎn)明確的場(chǎng)景下效果很好。但如果你的點(diǎn)云是多次掃描拼接出來(lái)的不同區(qū)域其實(shí)來(lái)自不同的傳感器位置再用一個(gè)全局視點(diǎn)就不太準(zhǔn)確了。更穩(wěn)的做法是方向傳播從某個(gè)法向量可靠的點(diǎn)出發(fā)沿著鄰近體素逐漸向外擴(kuò)散凡是和當(dāng)前法向量夾角大于 90 度的鄰居就翻轉(zhuǎn)。這種方式不依賴(lài)傳感器位姿計(jì)算量稍大卻是多幀拼接場(chǎng)景下更好的選擇。3. 用 PCL 實(shí)現(xiàn)體素法向量從“偽體素法”到自寫(xiě) PCA 模塊3.1 先給你看一個(gè)偷懶方案VoxelGrid NormalEstimation說(shuō)實(shí)話(huà)PCL 里沒(méi)有一個(gè)直接叫“Voxel-based Normal”的現(xiàn)成類(lèi)。如果你想最快看到效果可以先走一套“偽體素法”先用VoxelGrid把點(diǎn)云抽稀再對(duì)剩下的點(diǎn)做NormalEstimation。很多項(xiàng)目里說(shuō)的“體素法向量”其實(shí)就是這個(gè)組合。pcl::VoxelGridpcl::PointXYZ vg; vg.setInputCloud(cloud); vg.setLeafSize(0.05f, 0.05f, 0.05f); pcl::PointCloudpcl::PointXYZ::Ptr downsampled(new pcl::PointCloudpcl::PointXYZ); vg.filter(*downsampled); pcl::NormalEstimationpcl::PointXYZ, pcl::Normal ne; pcl::search::KdTreepcl::PointXYZ::Ptr tree(new pcl::search::KdTreepcl::PointXYZ); ne.setInputCloud(downsampled); ne.setSearchMethod(tree); ne.setKSearch(20); pcl::PointCloudpcl::Normal::Ptr normals(new pcl::PointCloudpcl::Normal); ne.compute(*normals);這個(gè)方案的優(yōu)勢(shì)是代碼量小PCL 版本兼容性好而且確實(shí)能大幅減少計(jì)算量。但你會(huì)發(fā)現(xiàn)一個(gè)問(wèn)題VoxelGrid把每個(gè)體素壓縮成重心點(diǎn)之后NormalEstimation又回到了“K 近鄰找鄰居”的老路只是點(diǎn)少了、算得快了。它并不等于“每個(gè)體素內(nèi)的點(diǎn)共同參與 PCA”。所以我堅(jiān)持把這套方案叫“偽體素法”適合點(diǎn)云太大、需要先降采樣、對(duì)法向量精度要求不那么極端的任務(wù)。3.2 更貼合概念的方案哈希體素 自寫(xiě) PCA既然 PCL 沒(méi)有現(xiàn)成類(lèi)那就自己寫(xiě)一個(gè)。下面這段代碼是完整可編譯的思路分三步第一遍歷點(diǎn)云把每個(gè)點(diǎn)按坐標(biāo)分進(jìn)哈希表管理的體素第二對(duì)每個(gè)體素內(nèi)的點(diǎn)做 PCA第三做視點(diǎn)方向矯正給每個(gè)點(diǎn)填上法向量。#include pcl/point_cloud.h #include pcl/point_types.h #include pcl/common/common.h #include Eigen/Eigenvalues #include unordered_map #include tuple #include vector #include limits #include cmath using PointT pcl::PointXYZ; // 體素坐標(biāo)哈希三個(gè)素?cái)?shù)組合碰撞由 unordered_map 的鍵比較兜底 struct VoxelHash { size_t operator()(const std::tupleint, int, int v) const { size_t ix static_castsize_t(std::get0(v)); size_t iy static_castsize_t(std::get1(v)); size_t iz static_castsize_t(std::get2(v)); return (ix * 73856093u) ^ (iy * 19349669u) ^ (iz * 83492791u); } }; Eigen::Vector3f EstimateVoxelNormal(const std::vectorEigen::Vector3f pts) { if (pts.size() 3) return Eigen::Vector3f::Zero(); Eigen::Vector3f centroid Eigen::Vector3f::Zero(); for (const auto p : pts) centroid p; centroid / static_castfloat(pts.size()); Eigen::Matrix3f cov Eigen::Matrix3f::Zero(); for (const auto p : pts) { Eigen::Vector3f diff p - centroid; cov diff * diff.transpose(); } Eigen::SelfAdjointEigenSolverEigen::Matrix3f solver(cov); Eigen::Vector3f n solver.eigenvectors().col(0); if (n.norm() 1e-12f) return Eigen::Vector3f::Zero(); return n.normalized(); } void ComputeVoxelNormal(pcl::PointCloudPointT::Ptr cloud, float voxel_size, size_t min_pts, const Eigen::Vector3f viewpoint, pcl::PointCloudpcl::Normal::Ptr normals) { Eigen::Vector4f min_pt, max_pt; pcl::getMinMax3D(*cloud, min_pt, max_pt); std::unordered_mapstd::tupleint, int, int, std::vectorsize_t, VoxelHash voxel_map; for (size_t i 0; i cloud-size(); i) { const PointT p cloud-at(i); int ix static_castint(std::floor((p.x - min_pt[0]) / voxel_size)); int iy static_castint(std::floor((p.y - min_pt[1]) / voxel_size)); int iz static_castint(std::floor((p.z - min_pt[2]) / voxel_size)); voxel_map[{ix, iy, iz}].push_back(i); } normals-resize(cloud-size()); for (auto n : *normals) { n.normal_x n.normal_y n.normal_z std::numeric_limitsfloat::quiet_NaN(); } for (const auto kv : voxel_map) { const std::vectorsize_t indices kv.second; if (indices.size() min_pts) continue; std::vectorEigen::Vector3f pts; pts.reserve(indices.size()); for (size_t idx : indices) { const PointT p cloud-at(idx); pts.emplace_back(p.x, p.y, p.z); } Eigen::Vector3f n EstimateVoxelNormal(pts); if (n.squaredNorm() 0.5f) continue; for (size_t idx : indices) { const PointT p cloud-at(idx); Eigen::Vector3f to_view viewpoint - Eigen::Vector3f(p.x, p.y, p.z); Eigen::Vector3f oriented (n.dot(to_view) 0.0f) ? -n : n; pcl::Normal out normals-at(idx); out.normal_x oriented.x(); out.normal_y oriented.y(); out.normal_z oriented.z(); out.curvature 0.0f; } } }3.3 關(guān)鍵代碼拆解三處容易寫(xiě)錯(cuò)的地方第一處是體素坐標(biāo)計(jì)算。這里必須用std::floor而不是直接int強(qiáng)轉(zhuǎn)。坐標(biāo)為正數(shù)時(shí)直接強(qiáng)轉(zhuǎn)等價(jià)于向下取整但一旦浮點(diǎn)誤差導(dǎo)致偏移量微負(fù)強(qiáng)轉(zhuǎn)會(huì)得到一個(gè)錯(cuò)誤的體素索引floor能保證任何情況下取整都對(duì)得上。第二處是剛才反復(fù)強(qiáng)調(diào)的col(0)。Eigen::SelfAdjointEigenSolver返回的特征向量按特征值升序排列所以col(0)就是最小特征值對(duì)應(yīng)的特征向量。如果誤取col(2)你得到的法向量其實(shí)是平行于表面的方向等于整體轉(zhuǎn)了個(gè) 90 度。這個(gè)錯(cuò)誤我見(jiàn)過(guò)不止一次。第三處是輸出法向量的初始化。resize之后pcl::Normal里的值是不確定的如果某些體素因?yàn)辄c(diǎn)數(shù)不足被跳過(guò)這些點(diǎn)的法向量就會(huì)是未初始化垃圾值比 NaN 更坑。所以代碼里先把整個(gè)輸出設(shè)為 NaN后續(xù)只有成功計(jì)算的點(diǎn)才會(huì)被覆蓋。這樣在可視化或下游處理時(shí)你一眼就能認(rèn)出哪些點(diǎn)的法向量不可靠。另外這套代碼給同一個(gè)體素里的所有點(diǎn)填了同樣的法向量。這是體素法的特性它本身輸出的是“體素級(jí)”法向量平滑度很好但空間分辨率有限。如果你下游需要非常細(xì)膩的逐點(diǎn)法向量變化后面可以把體素尺寸縮小或者改用我下一節(jié)要說(shuō)的移動(dòng)窗口思路。4. 參數(shù)怎么調(diào)、效果怎么驗(yàn)一組實(shí)測(cè)數(shù)據(jù)4.1 體素尺寸先估算平均點(diǎn)間距再做選擇體素邊長(zhǎng)是整個(gè)體素法向量的靈魂參數(shù)。設(shè)太大一個(gè)格子里混進(jìn)多個(gè)不同平面法向量被“平均”得面目全非設(shè)太小格子里的點(diǎn)數(shù)太少PCA 算不準(zhǔn)也給不出穩(wěn)定的法向量。比較穩(wěn)的做法是先估算一下點(diǎn)云的平均點(diǎn)間距。用包圍盒體積除以點(diǎn)數(shù)的三次方根得到一個(gè)大致的間距參考值然后取體素邊長(zhǎng)等于這個(gè)間距的 2 到 5 倍。比如一份包圍盒體積約 125 立方米、點(diǎn)數(shù)約 300 萬(wàn)的點(diǎn)云平均點(diǎn)間距大概是cbrt(125 / 3000000)約等于 0.035 米取 4 倍的話(huà)體素邊長(zhǎng)約 0.14 米。這個(gè)值在多數(shù)室內(nèi)場(chǎng)景下都能得到不錯(cuò)的效果。float volume (max_pt[0] - min_pt[0]) * (max_pt[1] - min_pt[1]) * (max_pt[2] - min_pt[2]); float avg_spacing std::cbrt(volume / cloud-size()); float voxel_size avg_spacing * 4.0f;如果你的點(diǎn)云是從網(wǎng)格模型上均勻采樣來(lái)的密度規(guī)則均勻體素邊長(zhǎng)可以回到 2 倍平均點(diǎn)間距附近以保留更多邊緣細(xì)節(jié)。如果是機(jī)載或車(chē)載激光掃描數(shù)據(jù)密度很不均勻體素邊長(zhǎng)取到 5 倍甚至更大都很常見(jiàn)因?yàn)樗粗卣w平滑度而不是局部細(xì)節(jié)。4.2 最小點(diǎn)數(shù)閾值“夠算一次 PCA”只是底線(xiàn)我一開(kāi)始習(xí)慣把min_pts設(shè)成 3想著三個(gè)點(diǎn)就能確定一個(gè)平面。實(shí)際跑下來(lái)大量體素的法向量在不同幀的點(diǎn)云之間跳來(lái)跳去尤其是有輕微噪聲的數(shù)據(jù)三四個(gè)點(diǎn)的 PCA 對(duì)噪聲極度敏感。后來(lái)我把閾值提到 8 到 12效果穩(wěn)定了很多。原理不復(fù)雜PCA 依賴(lài)點(diǎn)云在空間中的散布統(tǒng)計(jì)樣本太少時(shí)統(tǒng)計(jì)量本身的方差就很大算出來(lái)的方向自然不穩(wěn)。閾值也不是越大越好。設(shè)太大那些點(diǎn)數(shù)較少的稀疏體素會(huì)被跳過(guò)法向量出現(xiàn)大片空洞。我常用的策略是min_pts取 8 作為底線(xiàn)同時(shí)配合一個(gè)體素合并的補(bǔ)救動(dòng)作如果某個(gè)體素點(diǎn)數(shù)不足就把它周?chē)?x3x3鄰域里的點(diǎn)全部合并進(jìn)來(lái)一起算 PCA。這樣既不浪費(fèi)稀疏區(qū)域的點(diǎn)又能保證參與計(jì)算的點(diǎn)數(shù)足夠。4.3 一組消費(fèi)級(jí)機(jī)器上的對(duì)比數(shù)據(jù)這里放一組我自己跑過(guò)的對(duì)比結(jié)果。數(shù)據(jù)來(lái)自一份約 300 萬(wàn)點(diǎn)的室內(nèi)走廊點(diǎn)云機(jī)器是普通的移動(dòng)工作站只統(tǒng)計(jì)法向量估計(jì)這一段的耗時(shí)不含點(diǎn)云讀寫(xiě)時(shí)間。不同機(jī)器數(shù)值肯定不一樣但橫向?qū)Ρ鹊内厔?shì)是穩(wěn)定的。方案關(guān)鍵參數(shù)耗時(shí)索引/輔助內(nèi)存法向量表現(xiàn)常規(guī)K近鄰K20約2.2秒KD-Tree索引約120MB邊緣毛刺稀疏區(qū)抖動(dòng)半徑搜索r0.08m約3.6秒KD-Tree索引約120MB均勻但稀疏區(qū)出現(xiàn)不少NaN體素法(固定網(wǎng)格)v0.05m, min_pts8約0.8秒哈希表約10MB平滑穩(wěn)定邊緣略鈍體素法(移動(dòng)窗口)v0.05m, min_pts8約1.7秒哈希表約10MB穩(wěn)定邊緣保留更好從數(shù)據(jù)里能很直觀地看出來(lái)體素法在耗時(shí)上的優(yōu)勢(shì)非常明顯。原因在于它只用一次遍歷就把點(diǎn)分配進(jìn)體素之后是逐體素批量計(jì)算省掉了每個(gè)點(diǎn)單獨(dú)做 KD-Tree 搜索的開(kāi)銷(xiāo)。代價(jià)也很明顯固定網(wǎng)格法在物體邊緣處法向量偏鈍因?yàn)橐粋€(gè)體素可能同時(shí)“裝”了墻面和門(mén)框上的點(diǎn)PCA 結(jié)果被平均掉了。如果你需要兼顧邊緣細(xì)節(jié)就得上移動(dòng)窗口體素這個(gè)下面展開(kāi)說(shuō)。5. 實(shí)操中繞不開(kāi)的坑以及我的解決習(xí)慣5.1 “體素抖動(dòng)”邊界上的點(diǎn)為什么會(huì)突然變方向固定網(wǎng)格法有一個(gè)常被忽略的問(wèn)題一個(gè)點(diǎn)只要稍微移動(dòng)一點(diǎn)就可能從格子的這一側(cè)掉到另一側(cè)導(dǎo)致它參與計(jì)算的法向量發(fā)生跳變。在單幀靜態(tài)點(diǎn)云上你可能看不出影響但如果后面拿同一場(chǎng)景的多幀點(diǎn)云做配準(zhǔn)或形變分析這種“體素抖動(dòng)”會(huì)給結(jié)果引入額外的高頻噪聲。我的解法是移動(dòng)窗口體素。對(duì)每個(gè)查詢(xún)點(diǎn)不預(yù)先固定網(wǎng)格而是以該點(diǎn)自身為中心取一個(gè)邊長(zhǎng)約等于體素尺寸的立方體收集落在立方體內(nèi)的點(diǎn)做 PCA。移動(dòng)窗口天然解決了邊界跳變問(wèn)題因?yàn)槟阌肋h(yuǎn)以當(dāng)前點(diǎn)為中心。代價(jià)是每個(gè)點(diǎn)都要重新做一次鄰域搜索耗時(shí)比固定網(wǎng)格高一截也就是表格里那個(gè) 1.7 秒的由來(lái)。如果不想犧牲太多性能還有一個(gè)折中的土辦法把固定網(wǎng)格法做兩次第二次把所有點(diǎn)坐標(biāo)整體平移半個(gè)體素邊長(zhǎng)分別計(jì)算結(jié)果后對(duì)同一個(gè)點(diǎn)取兩組法向量的平均方向。這能明顯減少因體素邊界導(dǎo)致的跳變計(jì)算量只是翻倍在很多場(chǎng)景下很劃算。5.2 稀疏區(qū)域的大片 NaN 和“幽靈法向量”體素法出現(xiàn) NaN絕大多數(shù)情況是格子里點(diǎn)數(shù)不足被跳過(guò)。還有一類(lèi)情況更迷惑點(diǎn)數(shù)夠但點(diǎn)都分布在一個(gè)幾乎共線(xiàn)的細(xì)長(zhǎng)結(jié)構(gòu)上PCA 給出的法向量方向不穩(wěn)定視覺(jué)上就像有一個(gè)“幽靈法向量”在點(diǎn)云里到處亂指。對(duì)于點(diǎn)數(shù)不足除了前面說(shuō)的3x3x3鄰域點(diǎn)合并還可以把無(wú)效體素直接標(biāo)記出來(lái)交給后續(xù)處理流程判斷是否丟棄。對(duì)于細(xì)長(zhǎng)結(jié)構(gòu)光靠體素法本身很難根治因?yàn)樗举|(zhì)上就是在估計(jì)局部主方向而細(xì)長(zhǎng)結(jié)構(gòu)本來(lái)就沒(méi)有明確的法向。我的處理習(xí)慣是做法向量之前先做一次平滑比如用 PCL 的 MLS 投影或者簡(jiǎn)單的雙邊濾波把細(xì)桿上的點(diǎn)稍微“攤”開(kāi)一點(diǎn)法向量就會(huì)穩(wěn)定很多。注意平滑強(qiáng)度別設(shè)太大不然桌腿會(huì)被抹成一片糊。5.3 多幀拼接場(chǎng)景的方向一致性別迷信單一視點(diǎn)前面講了視點(diǎn)矯正但在多幀拼接場(chǎng)景里它有一個(gè)天生的盲區(qū)拼接后的點(diǎn)云中不同區(qū)域真正對(duì)應(yīng)的“視線(xiàn)方向”其實(shí)是當(dāng)初各幀傳感器各自的位置而不是某個(gè)固定的全局點(diǎn)。用全局視點(diǎn)矯正會(huì)把很多本來(lái)朝外的法向量翻成朝里后續(xù)做曲面重建時(shí)出現(xiàn)大面積空洞和反向面片。我在多幀場(chǎng)景里更常用方向傳播。先選一個(gè)法向量穩(wěn)定可靠的體素作為種子然后用 BFS/DFS 在體素的鄰接關(guān)系上傳播。每次訪(fǎng)問(wèn)相鄰體素時(shí)如果兩者的法向量夾角超過(guò) 90 度就翻轉(zhuǎn)相鄰體素的法向量。整個(gè)過(guò)程不依賴(lài)任何傳感器位姿適應(yīng)性強(qiáng)得多。實(shí)現(xiàn)時(shí)只需要一個(gè)隊(duì)列和一張?jiān)L問(wèn)標(biāo)記數(shù)組代碼大約三四十行值得放進(jìn)自己的點(diǎn)云處理工具庫(kù)里。最后分享一個(gè)我自己踩過(guò)之后長(zhǎng)期在用的經(jīng)驗(yàn)體素法向量用來(lái)做前置平滑非常穩(wěn)但如果你下游要提取非常精細(xì)的邊緣特征別把體素設(shè)太大也別完全拋棄半徑搜索。更通用的做法是先用小體素的體素法快速拿到主體法向再用半徑搜索在小尺度上修正邊緣部分的法向量。這樣既保住了速度也保住了細(xì)節(jié)。說(shuō)到底法向量估計(jì)的選型沒(méi)有絕對(duì)最優(yōu)只有和你的數(shù)據(jù)、下游任務(wù)匹配不匹配。 點(diǎn)云法向量估計(jì)的體素化方案：從PCL常規(guī)思路到大規(guī)模場(chǎng)景實(shí)戰(zhàn)
做點(diǎn)云處理這么多年我一直覺(jué)得法向量估計(jì)是“最容易忽視但最影響后續(xù)效果”的一步。很多同學(xué)一開(kāi)始接觸 PCLPoint Cloud Library上來(lái)就是pcl::NormalEstimationK近鄰一設(shè)、法向量一出看著可視化里五顏六色的線(xiàn)條還挺開(kāi)心結(jié)果一去做 ICP 配準(zhǔn)或者移動(dòng)立方體重建各種問(wèn)題就開(kāi)始冒出來(lái)。后來(lái)我在處理大規(guī)模場(chǎng)景點(diǎn)云時(shí)逐漸摸出一套“體素法向量Voxel-based Normal”的做法不去糾纏每個(gè)點(diǎn)單獨(dú)找鄰居而是先把點(diǎn)云按空間切成小格子再讓每個(gè)格子里的點(diǎn)一起“投票”算出朝向。今天這篇文章就把這套方法徹底講透。如果你手頭的數(shù)據(jù)是百萬(wàn)級(jí)以上的大場(chǎng)景、密度忽高忽低、或者噪聲明顯又不想在法向量這一步花掉太多時(shí)間那這篇文章應(yīng)該能給你一個(gè)非?？陕涞氐奶娲桨浮２还苣闶亲黾す饫走_(dá)點(diǎn)云、三維重建還是機(jī)械視覺(jué)的定位抓取這套思路都通用。1. 常規(guī)法向量估計(jì)為什么在大場(chǎng)景下容易“翻車(chē)”1.1 K近鄰與半徑搜索各自的“局部性”悖論pcl::NormalEstimation最常用的方式有兩種K近鄰搜索和半徑搜索。K近鄰的邏輯很簡(jiǎn)單以每個(gè)點(diǎn)為中心找最近的 K 個(gè)點(diǎn)對(duì)這 K 個(gè)點(diǎn)做 PCA。聽(tīng)著很合理但實(shí)際用起來(lái)有兩個(gè)隱藏問(wèn)題。第一K 是固定數(shù)量但點(diǎn)云密度是變化的。在密集區(qū)域K 個(gè)點(diǎn)可能只覆蓋了一小塊表面法向量會(huì)被表面上的微小起伏帶偏在稀疏區(qū)域K 個(gè)點(diǎn)可能跨越了好幾個(gè)結(jié)構(gòu)邊緣比如墻角、桌面和椅子扶手混在一起法向量直接被“平均”成一個(gè)四不像的方向。第二每個(gè)點(diǎn)都要在 KD-Tree 里獨(dú)立做一次近鄰搜索。點(diǎn)數(shù)少的時(shí)候還好一旦到了百萬(wàn)甚至千萬(wàn)量級(jí)單次搜索的 logN 開(kāi)銷(xiāo)累加起來(lái)非?？捎^而且 KD-Tree 索引本身還要吃掉大量?jī)?nèi)存。我見(jiàn)過(guò)不少項(xiàng)目的法向量計(jì)算直接成了整個(gè)處理管線(xiàn)的瓶頸。半徑搜索setRadiusSearch其實(shí)是在用“固定的空間尺度”換“鄰居數(shù)量可變”比 K 近鄰更貼近物理直覺(jué)。但它在密度稀疏的區(qū)域經(jīng)常出問(wèn)題——鄰居數(shù)量不滿(mǎn) 3 個(gè)PCA 根本沒(méi)法做最后返回一堆 NaN即便湊夠了點(diǎn)數(shù)因?yàn)辄c(diǎn)分布太稀疏、跨度過(guò)大法向量也很不穩(wěn)定。這等于剛從“密度不均”的坑里跳出來(lái)又掉進(jìn)“局部空洞”的坑里。1.2 體素思路先給空間畫(huà)格子再讓點(diǎn)“對(duì)號(hào)入座”把這兩種常規(guī)思路放在一起看你會(huì)發(fā)現(xiàn)它們本質(zhì)上都在做同一件事按某種規(guī)則找鄰居。問(wèn)題不在于“找鄰居”這個(gè)動(dòng)作而在于找鄰居的規(guī)則太受密度影響。體素法向量換了一個(gè)切入點(diǎn)不管點(diǎn)在哪我先用固定邊長(zhǎng)的立方體把整個(gè)場(chǎng)景空間切開(kāi)然后按每個(gè)點(diǎn)的坐標(biāo)把它分配進(jìn)屬于自己的那個(gè)格子。“先畫(huà)格子再分點(diǎn)”有幾個(gè)天然優(yōu)勢(shì)。第一點(diǎn)云的鄰居關(guān)系變成了格子之間的空間鄰接關(guān)系理解和操作都更直觀。第二每個(gè)格子的空間尺度是固定的無(wú)論點(diǎn)云局部密度怎么變參與計(jì)算的區(qū)域都保持一致。第三格子本質(zhì)是一個(gè)小型空間濾波器體素內(nèi)所有點(diǎn)一起參與計(jì)算能明顯壓制單點(diǎn)噪聲對(duì)法向量方向的干擾。這里得特別強(qiáng)調(diào)一下體素法和 PCL 里的VoxelGrid下采樣是兩回事。VoxelGrid是每個(gè)體素保留一個(gè)重心點(diǎn)或中心點(diǎn)目的是減少點(diǎn)數(shù)而體素法向量是讓每個(gè)體素內(nèi)的所有點(diǎn)共同參與 PCA目的是獲得更穩(wěn)的法向量。兩者可以配合使用但別混為一談。2. 體素法向量的核心原理從“點(diǎn)找鄰居”到“格子里求朝向”2.1 體素劃分的本質(zhì)一套虛擬整數(shù)坐標(biāo)索引點(diǎn)云本身是離散且無(wú)序的體素化本質(zhì)上就是給點(diǎn)云建立一套虛擬的整數(shù)坐標(biāo)索引。對(duì)任意一個(gè)點(diǎn) p先算它相對(duì)包圍盒最小點(diǎn)(min_x, min_y, min_z)的偏移量再除以體素邊長(zhǎng)voxel_size向下取整就得到了體素坐標(biāo)(ix, iy, iz)。ix floor((p.x - min_x) / voxel_size) iy floor((p.y - min_y) / voxel_size) iz floor((p.z - min_z) / voxel_size)有了這套坐標(biāo)你可以用哈希表、八叉樹(shù)或者三維數(shù)組來(lái)管理體素。三維數(shù)組最直觀但體素?cái)?shù)量通常達(dá)到幾十萬(wàn)甚至上百萬(wàn)直接開(kāi)數(shù)組內(nèi)存不劃算。哈希表是按需創(chuàng)建有多少個(gè)格子就建多少個(gè)桶內(nèi)存開(kāi)銷(xiāo)基本跟著點(diǎn)云的實(shí)際分布走大場(chǎng)景下更實(shí)用。八叉樹(shù)當(dāng)然也可以只是遍歷葉子節(jié)點(diǎn)的代碼會(huì)稍微繞一點(diǎn)后面我給的代碼里用的是哈希表方案。實(shí)際編碼時(shí)有一個(gè)容易被忽略的細(xì)節(jié)浮點(diǎn)數(shù)處在體素邊界上時(shí)可能因?yàn)榫仍颉疤钡礁舯诟褡?。比如某個(gè)點(diǎn)的坐標(biāo)剛好等于min_x k * voxel_size理論上它就在邊界上但取整結(jié)果受浮點(diǎn)誤差影響可能掉進(jìn)k-1格。這種“跳格子”雖然只發(fā)生在極少數(shù)邊界點(diǎn)上但后續(xù)算出來(lái)的法向量會(huì)出現(xiàn)不合理的毛刺。我的習(xí)慣是計(jì)算索引之前先給坐標(biāo)加一個(gè)極小的偏移量比如1e-6f代價(jià)幾乎為零效果卻很直接。2.2 體素內(nèi)的 PCA為什么最小特征值方向才是法向量體素收集完點(diǎn)之后法向量計(jì)算就是一個(gè)標(biāo)準(zhǔn)的 PCA 過(guò)程。先求點(diǎn)集的質(zhì)心 μ然后構(gòu)造協(xié)方差矩陣C Σ(p_i - μ)(p_i - μ)^T對(duì) C 做特征值分解得到三個(gè)特征值和對(duì)應(yīng)的三個(gè)特征向量。這里有一個(gè)特別常見(jiàn)、而且很多人最開(kāi)始都會(huì)搞反的點(diǎn)法向量取的是協(xié)方差矩陣最小特征值對(duì)應(yīng)的特征向量不是最大特征值。原因在于物理意義體素內(nèi)的點(diǎn)如果落在某個(gè)局部表面上點(diǎn)云在切平面內(nèi)的兩個(gè)方向上散布很大對(duì)應(yīng)較大的兩個(gè)特征值而垂直于表面的方向上點(diǎn)的變化非常小對(duì)應(yīng)最小的特征值。所以法向量本質(zhì)上是“點(diǎn)云散布最小的那個(gè)方向”。如果你還是不放心可以做個(gè)驗(yàn)證實(shí)驗(yàn)?zāi)靡黄秸膲γ纥c(diǎn)云體素化之后對(duì)任意一個(gè)格子做 PCA把三個(gè)特征向量打印出來(lái)。你會(huì)發(fā)現(xiàn)兩個(gè)大特征值對(duì)應(yīng)的向量幾乎平行于墻面而最小特征值對(duì)應(yīng)的向量垂直于墻面。這個(gè)結(jié)論一旦記住后面看代碼就不會(huì)再犯方向性錯(cuò)誤。另外還要注意協(xié)方差矩陣退化的情況。體素內(nèi)點(diǎn)數(shù)少于 3 時(shí)矩陣秩不夠求出的特征向量不穩(wěn)定即使點(diǎn)數(shù)夠如果點(diǎn)幾乎共線(xiàn)比如采集到一根細(xì)桿最小特征值和第二特征值會(huì)非常接近法向量方向就會(huì)高度敏感。這也是為什么“每個(gè)體素內(nèi)的最少點(diǎn)數(shù)”值得單獨(dú)拎出來(lái)當(dāng)一個(gè)重要的調(diào)參項(xiàng)。2.3 法向量的方向歧義PCA 只給方向不負(fù)責(zé)“朝向”P(pán)CA 給出的法向量只是一個(gè)帶符號(hào)的方向整體翻轉(zhuǎn) 180 度之后照樣滿(mǎn)足特征分解的要求。換句話(huà)說(shuō)同一塊平面上的點(diǎn)有的格子算出來(lái)法向量朝“外”有的格子朝“里”這是所有基于 PCA 的法向量估計(jì)方法都繞不開(kāi)的歧義問(wèn)題。最常用的修正是視點(diǎn)矯正。思路很簡(jiǎn)單法向量應(yīng)該大致指向傳感器方向。取一個(gè)視點(diǎn)坐標(biāo)通常是激光雷達(dá)或相機(jī)的位置計(jì)算視點(diǎn)到當(dāng)前點(diǎn)的方向向量如果法向量與這個(gè)方向向量的點(diǎn)積小于 0就把法向量翻轉(zhuǎn)。if (n · (viewpoint - p) 0) n -n;這個(gè)方法在單幀點(diǎn)云、視點(diǎn)明確的場(chǎng)景下效果很好。但如果你的點(diǎn)云是多次掃描拼接出來(lái)的不同區(qū)域其實(shí)來(lái)自不同的傳感器位置再用一個(gè)全局視點(diǎn)就不太準(zhǔn)確了。更穩(wěn)的做法是方向傳播從某個(gè)法向量可靠的點(diǎn)出發(fā)沿著鄰近體素逐漸向外擴(kuò)散凡是和當(dāng)前法向量夾角大于 90 度的鄰居就翻轉(zhuǎn)。這種方式不依賴(lài)傳感器位姿計(jì)算量稍大卻是多幀拼接場(chǎng)景下更好的選擇。3. 用 PCL 實(shí)現(xiàn)體素法向量從“偽體素法”到自寫(xiě) PCA 模塊3.1 先給你看一個(gè)偷懶方案VoxelGrid NormalEstimation說(shuō)實(shí)話(huà)PCL 里沒(méi)有一個(gè)直接叫“Voxel-based Normal”的現(xiàn)成類(lèi)。如果你想最快看到效果可以先走一套“偽體素法”先用VoxelGrid把點(diǎn)云抽稀再對(duì)剩下的點(diǎn)做NormalEstimation。很多項(xiàng)目里說(shuō)的“體素法向量”其實(shí)就是這個(gè)組合。pcl::VoxelGridpcl::PointXYZ vg; vg.setInputCloud(cloud); vg.setLeafSize(0.05f, 0.05f, 0.05f); pcl::PointCloudpcl::PointXYZ::Ptr downsampled(new pcl::PointCloudpcl::PointXYZ); vg.filter(*downsampled); pcl::NormalEstimationpcl::PointXYZ, pcl::Normal ne; pcl::search::KdTreepcl::PointXYZ::Ptr tree(new pcl::search::KdTreepcl::PointXYZ); ne.setInputCloud(downsampled); ne.setSearchMethod(tree); ne.setKSearch(20); pcl::PointCloudpcl::Normal::Ptr normals(new pcl::PointCloudpcl::Normal); ne.compute(*normals);這個(gè)方案的優(yōu)勢(shì)是代碼量小PCL 版本兼容性好而且確實(shí)能大幅減少計(jì)算量。但你會(huì)發(fā)現(xiàn)一個(gè)問(wèn)題VoxelGrid把每個(gè)體素壓縮成重心點(diǎn)之后NormalEstimation又回到了“K 近鄰找鄰居”的老路只是點(diǎn)少了、算得快了。它并不等于“每個(gè)體素內(nèi)的點(diǎn)共同參與 PCA”。所以我堅(jiān)持把這套方案叫“偽體素法”適合點(diǎn)云太大、需要先降采樣、對(duì)法向量精度要求不那么極端的任務(wù)。3.2 更貼合概念的方案哈希體素 自寫(xiě) PCA既然 PCL 沒(méi)有現(xiàn)成類(lèi)那就自己寫(xiě)一個(gè)。下面這段代碼是完整可編譯的思路分三步第一遍歷點(diǎn)云把每個(gè)點(diǎn)按坐標(biāo)分進(jìn)哈希表管理的體素第二對(duì)每個(gè)體素內(nèi)的點(diǎn)做 PCA第三做視點(diǎn)方向矯正給每個(gè)點(diǎn)填上法向量。#include pcl/point_cloud.h #include pcl/point_types.h #include pcl/common/common.h #include Eigen/Eigenvalues #include unordered_map #include tuple #include vector #include limits #include cmath using PointT pcl::PointXYZ; // 體素坐標(biāo)哈希三個(gè)素?cái)?shù)組合碰撞由 unordered_map 的鍵比較兜底 struct VoxelHash { size_t operator()(const std::tupleint, int, int v) const { size_t ix static_castsize_t(std::get0(v)); size_t iy static_castsize_t(std::get1(v)); size_t iz static_castsize_t(std::get2(v)); return (ix * 73856093u) ^ (iy * 19349669u) ^ (iz * 83492791u); } }; Eigen::Vector3f EstimateVoxelNormal(const std::vectorEigen::Vector3f pts) { if (pts.size() 3) return Eigen::Vector3f::Zero(); Eigen::Vector3f centroid Eigen::Vector3f::Zero(); for (const auto p : pts) centroid p; centroid / static_castfloat(pts.size()); Eigen::Matrix3f cov Eigen::Matrix3f::Zero(); for (const auto p : pts) { Eigen::Vector3f diff p - centroid; cov diff * diff.transpose(); } Eigen::SelfAdjointEigenSolverEigen::Matrix3f solver(cov); Eigen::Vector3f n solver.eigenvectors().col(0); if (n.norm() 1e-12f) return Eigen::Vector3f::Zero(); return n.normalized(); } void ComputeVoxelNormal(pcl::PointCloudPointT::Ptr cloud, float voxel_size, size_t min_pts, const Eigen::Vector3f viewpoint, pcl::PointCloudpcl::Normal::Ptr normals) { Eigen::Vector4f min_pt, max_pt; pcl::getMinMax3D(*cloud, min_pt, max_pt); std::unordered_mapstd::tupleint, int, int, std::vectorsize_t, VoxelHash voxel_map; for (size_t i 0; i cloud-size(); i) { const PointT p cloud-at(i); int ix static_castint(std::floor((p.x - min_pt[0]) / voxel_size)); int iy static_castint(std::floor((p.y - min_pt[1]) / voxel_size)); int iz static_castint(std::floor((p.z - min_pt[2]) / voxel_size)); voxel_map[{ix, iy, iz}].push_back(i); } normals-resize(cloud-size()); for (auto n : *normals) { n.normal_x n.normal_y n.normal_z std::numeric_limitsfloat::quiet_NaN(); } for (const auto kv : voxel_map) { const std::vectorsize_t indices kv.second; if (indices.size() min_pts) continue; std::vectorEigen::Vector3f pts; pts.reserve(indices.size()); for (size_t idx : indices) { const PointT p cloud-at(idx); pts.emplace_back(p.x, p.y, p.z); } Eigen::Vector3f n EstimateVoxelNormal(pts); if (n.squaredNorm() 0.5f) continue; for (size_t idx : indices) { const PointT p cloud-at(idx); Eigen::Vector3f to_view viewpoint - Eigen::Vector3f(p.x, p.y, p.z); Eigen::Vector3f oriented (n.dot(to_view) 0.0f) ? -n : n; pcl::Normal out normals-at(idx); out.normal_x oriented.x(); out.normal_y oriented.y(); out.normal_z oriented.z(); out.curvature 0.0f; } } }3.3 關(guān)鍵代碼拆解三處容易寫(xiě)錯(cuò)的地方第一處是體素坐標(biāo)計(jì)算。這里必須用std::floor而不是直接int強(qiáng)轉(zhuǎn)。坐標(biāo)為正數(shù)時(shí)直接強(qiáng)轉(zhuǎn)等價(jià)于向下取整但一旦浮點(diǎn)誤差導(dǎo)致偏移量微負(fù)強(qiáng)轉(zhuǎn)會(huì)得到一個(gè)錯(cuò)誤的體素索引floor能保證任何情況下取整都對(duì)得上。第二處是剛才反復(fù)強(qiáng)調(diào)的col(0)。Eigen::SelfAdjointEigenSolver返回的特征向量按特征值升序排列所以col(0)就是最小特征值對(duì)應(yīng)的特征向量。如果誤取col(2)你得到的法向量其實(shí)是平行于表面的方向等于整體轉(zhuǎn)了個(gè) 90 度。這個(gè)錯(cuò)誤我見(jiàn)過(guò)不止一次。第三處是輸出法向量的初始化。resize之后pcl::Normal里的值是不確定的如果某些體素因?yàn)辄c(diǎn)數(shù)不足被跳過(guò)這些點(diǎn)的法向量就會(huì)是未初始化垃圾值比 NaN 更坑。所以代碼里先把整個(gè)輸出設(shè)為 NaN后續(xù)只有成功計(jì)算的點(diǎn)才會(huì)被覆蓋。這樣在可視化或下游處理時(shí)你一眼就能認(rèn)出哪些點(diǎn)的法向量不可靠。另外這套代碼給同一個(gè)體素里的所有點(diǎn)填了同樣的法向量。這是體素法的特性它本身輸出的是“體素級(jí)”法向量平滑度很好但空間分辨率有限。如果你下游需要非常細(xì)膩的逐點(diǎn)法向量變化后面可以把體素尺寸縮小或者改用我下一節(jié)要說(shuō)的移動(dòng)窗口思路。4. 參數(shù)怎么調(diào)、效果怎么驗(yàn)一組實(shí)測(cè)數(shù)據(jù)4.1 體素尺寸先估算平均點(diǎn)間距再做選擇體素邊長(zhǎng)是整個(gè)體素法向量的靈魂參數(shù)。設(shè)太大一個(gè)格子里混進(jìn)多個(gè)不同平面法向量被“平均”得面目全非設(shè)太小格子里的點(diǎn)數(shù)太少PCA 算不準(zhǔn)也給不出穩(wěn)定的法向量。比較穩(wěn)的做法是先估算一下點(diǎn)云的平均點(diǎn)間距。用包圍盒體積除以點(diǎn)數(shù)的三次方根得到一個(gè)大致的間距參考值然后取體素邊長(zhǎng)等于這個(gè)間距的 2 到 5 倍。比如一份包圍盒體積約 125 立方米、點(diǎn)數(shù)約 300 萬(wàn)的點(diǎn)云平均點(diǎn)間距大概是cbrt(125 / 3000000)約等于 0.035 米取 4 倍的話(huà)體素邊長(zhǎng)約 0.14 米。這個(gè)值在多數(shù)室內(nèi)場(chǎng)景下都能得到不錯(cuò)的效果。float volume (max_pt[0] - min_pt[0]) * (max_pt[1] - min_pt[1]) * (max_pt[2] - min_pt[2]); float avg_spacing std::cbrt(volume / cloud-size()); float voxel_size avg_spacing * 4.0f;如果你的點(diǎn)云是從網(wǎng)格模型上均勻采樣來(lái)的密度規(guī)則均勻體素邊長(zhǎng)可以回到 2 倍平均點(diǎn)間距附近以保留更多邊緣細(xì)節(jié)。如果是機(jī)載或車(chē)載激光掃描數(shù)據(jù)密度很不均勻體素邊長(zhǎng)取到 5 倍甚至更大都很常見(jiàn)因?yàn)樗粗卣w平滑度而不是局部細(xì)節(jié)。4.2 最小點(diǎn)數(shù)閾值“夠算一次 PCA”只是底線(xiàn)我一開(kāi)始習(xí)慣把min_pts設(shè)成 3想著三個(gè)點(diǎn)就能確定一個(gè)平面。實(shí)際跑下來(lái)大量體素的法向量在不同幀的點(diǎn)云之間跳來(lái)跳去尤其是有輕微噪聲的數(shù)據(jù)三四個(gè)點(diǎn)的 PCA 對(duì)噪聲極度敏感。后來(lái)我把閾值提到 8 到 12效果穩(wěn)定了很多。原理不復(fù)雜PCA 依賴(lài)點(diǎn)云在空間中的散布統(tǒng)計(jì)樣本太少時(shí)統(tǒng)計(jì)量本身的方差就很大算出來(lái)的方向自然不穩(wěn)。閾值也不是越大越好。設(shè)太大那些點(diǎn)數(shù)較少的稀疏體素會(huì)被跳過(guò)法向量出現(xiàn)大片空洞。我常用的策略是min_pts取 8 作為底線(xiàn)同時(shí)配合一個(gè)體素合并的補(bǔ)救動(dòng)作如果某個(gè)體素點(diǎn)數(shù)不足就把它周?chē)?x3x3鄰域里的點(diǎn)全部合并進(jìn)來(lái)一起算 PCA。這樣既不浪費(fèi)稀疏區(qū)域的點(diǎn)又能保證參與計(jì)算的點(diǎn)數(shù)足夠。4.3 一組消費(fèi)級(jí)機(jī)器上的對(duì)比數(shù)據(jù)這里放一組我自己跑過(guò)的對(duì)比結(jié)果。數(shù)據(jù)來(lái)自一份約 300 萬(wàn)點(diǎn)的室內(nèi)走廊點(diǎn)云機(jī)器是普通的移動(dòng)工作站只統(tǒng)計(jì)法向量估計(jì)這一段的耗時(shí)不含點(diǎn)云讀寫(xiě)時(shí)間。不同機(jī)器數(shù)值肯定不一樣但橫向?qū)Ρ鹊内厔?shì)是穩(wěn)定的。方案關(guān)鍵參數(shù)耗時(shí)索引/輔助內(nèi)存法向量表現(xiàn)常規(guī)K近鄰K20約2.2秒KD-Tree索引約120MB邊緣毛刺稀疏區(qū)抖動(dòng)半徑搜索r0.08m約3.6秒KD-Tree索引約120MB均勻但稀疏區(qū)出現(xiàn)不少NaN體素法(固定網(wǎng)格)v0.05m, min_pts8約0.8秒哈希表約10MB平滑穩(wěn)定邊緣略鈍體素法(移動(dòng)窗口)v0.05m, min_pts8約1.7秒哈希表約10MB穩(wěn)定邊緣保留更好從數(shù)據(jù)里能很直觀地看出來(lái)體素法在耗時(shí)上的優(yōu)勢(shì)非常明顯。原因在于它只用一次遍歷就把點(diǎn)分配進(jìn)體素之后是逐體素批量計(jì)算省掉了每個(gè)點(diǎn)單獨(dú)做 KD-Tree 搜索的開(kāi)銷(xiāo)。代價(jià)也很明顯固定網(wǎng)格法在物體邊緣處法向量偏鈍因?yàn)橐粋€(gè)體素可能同時(shí)“裝”了墻面和門(mén)框上的點(diǎn)PCA 結(jié)果被平均掉了。如果你需要兼顧邊緣細(xì)節(jié)就得上移動(dòng)窗口體素這個(gè)下面展開(kāi)說(shuō)。5. 實(shí)操中繞不開(kāi)的坑以及我的解決習(xí)慣5.1 “體素抖動(dòng)”邊界上的點(diǎn)為什么會(huì)突然變方向固定網(wǎng)格法有一個(gè)常被忽略的問(wèn)題一個(gè)點(diǎn)只要稍微移動(dòng)一點(diǎn)就可能從格子的這一側(cè)掉到另一側(cè)導(dǎo)致它參與計(jì)算的法向量發(fā)生跳變。在單幀靜態(tài)點(diǎn)云上你可能看不出影響但如果后面拿同一場(chǎng)景的多幀點(diǎn)云做配準(zhǔn)或形變分析這種“體素抖動(dòng)”會(huì)給結(jié)果引入額外的高頻噪聲。我的解法是移動(dòng)窗口體素。對(duì)每個(gè)查詢(xún)點(diǎn)不預(yù)先固定網(wǎng)格而是以該點(diǎn)自身為中心取一個(gè)邊長(zhǎng)約等于體素尺寸的立方體收集落在立方體內(nèi)的點(diǎn)做 PCA。移動(dòng)窗口天然解決了邊界跳變問(wèn)題因?yàn)槟阌肋h(yuǎn)以當(dāng)前點(diǎn)為中心。代價(jià)是每個(gè)點(diǎn)都要重新做一次鄰域搜索耗時(shí)比固定網(wǎng)格高一截也就是表格里那個(gè) 1.7 秒的由來(lái)。如果不想犧牲太多性能還有一個(gè)折中的土辦法把固定網(wǎng)格法做兩次第二次把所有點(diǎn)坐標(biāo)整體平移半個(gè)體素邊長(zhǎng)分別計(jì)算結(jié)果后對(duì)同一個(gè)點(diǎn)取兩組法向量的平均方向。這能明顯減少因體素邊界導(dǎo)致的跳變計(jì)算量只是翻倍在很多場(chǎng)景下很劃算。5.2 稀疏區(qū)域的大片 NaN 和“幽靈法向量”體素法出現(xiàn) NaN絕大多數(shù)情況是格子里點(diǎn)數(shù)不足被跳過(guò)。還有一類(lèi)情況更迷惑點(diǎn)數(shù)夠但點(diǎn)都分布在一個(gè)幾乎共線(xiàn)的細(xì)長(zhǎng)結(jié)構(gòu)上PCA 給出的法向量方向不穩(wěn)定視覺(jué)上就像有一個(gè)“幽靈法向量”在點(diǎn)云里到處亂指。對(duì)于點(diǎn)數(shù)不足除了前面說(shuō)的3x3x3鄰域點(diǎn)合并還可以把無(wú)效體素直接標(biāo)記出來(lái)交給后續(xù)處理流程判斷是否丟棄。對(duì)于細(xì)長(zhǎng)結(jié)構(gòu)光靠體素法本身很難根治因?yàn)樗举|(zhì)上就是在估計(jì)局部主方向而細(xì)長(zhǎng)結(jié)構(gòu)本來(lái)就沒(méi)有明確的法向。我的處理習(xí)慣是做法向量之前先做一次平滑比如用 PCL 的 MLS 投影或者簡(jiǎn)單的雙邊濾波把細(xì)桿上的點(diǎn)稍微“攤”開(kāi)一點(diǎn)法向量就會(huì)穩(wěn)定很多。注意平滑強(qiáng)度別設(shè)太大不然桌腿會(huì)被抹成一片糊。5.3 多幀拼接場(chǎng)景的方向一致性別迷信單一視點(diǎn)前面講了視點(diǎn)矯正但在多幀拼接場(chǎng)景里它有一個(gè)天生的盲區(qū)拼接后的點(diǎn)云中不同區(qū)域真正對(duì)應(yīng)的“視線(xiàn)方向”其實(shí)是當(dāng)初各幀傳感器各自的位置而不是某個(gè)固定的全局點(diǎn)。用全局視點(diǎn)矯正會(huì)把很多本來(lái)朝外的法向量翻成朝里后續(xù)做曲面重建時(shí)出現(xiàn)大面積空洞和反向面片。我在多幀場(chǎng)景里更常用方向傳播。先選一個(gè)法向量穩(wěn)定可靠的體素作為種子然后用 BFS/DFS 在體素的鄰接關(guān)系上傳播。每次訪(fǎng)問(wèn)相鄰體素時(shí)如果兩者的法向量夾角超過(guò) 90 度就翻轉(zhuǎn)相鄰體素的法向量。整個(gè)過(guò)程不依賴(lài)任何傳感器位姿適應(yīng)性強(qiáng)得多。實(shí)現(xiàn)時(shí)只需要一個(gè)隊(duì)列和一張?jiān)L問(wèn)標(biāo)記數(shù)組代碼大約三四十行值得放進(jìn)自己的點(diǎn)云處理工具庫(kù)里。最后分享一個(gè)我自己踩過(guò)之后長(zhǎng)期在用的經(jīng)驗(yàn)體素法向量用來(lái)做前置平滑非常穩(wěn)但如果你下游要提取非常精細(xì)的邊緣特征別把體素設(shè)太大也別完全拋棄半徑搜索。更通用的做法是先用小體素的體素法快速拿到主體法向再用半徑搜索在小尺度上修正邊緣部分的法向量。這樣既保住了速度也保住了細(xì)節(jié)。說(shuō)到底法向量估計(jì)的選型沒(méi)有絕對(duì)最優(yōu)只有和你的數(shù)據(jù)、下游任務(wù)匹配不匹配。