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