掃描匹配的方法

1.0 cartographer 前端兩種匹配：

1.1 相關方法

double RealTimeCorrelativeScanMatcher2D::Match(
    const transform::Rigid2d& initial_pose_estimate,
    const sensor::PointCloud& point_cloud, const Grid2D& grid,
    transform::Rigid2d* pose_estimate)

輸入 ：
預計位姿（推算位子）
點雲數據
概率柵格地圖
估計位姿（引用輸出）

返回：
最佳得分

步驟：
// 初始位姿的角度
//將激光轉到初始位姿的角度——點雲開始爲：baselink座標系
//確定查詢窗口 —— 角度+空間平面
//得到旋轉的點雲——查詢角度範圍 ×2 /角度分辨率——點雲：對各個角度旋轉
//將點雲轉到當前位姿下——平移
//生成平移的候選值， 只記錄 x,y的偏移
//求取得分——遍歷候選值， 再遍歷激光點，遍歷激光點時將xy的偏移量傳入
//取得分最大的候選值的——偏移量
//輸入的初始位子 + 偏移量

1.2 ceres 優化：

void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,
                               const transform::Rigid2d& initial_pose_estimate,
                               const sensor::PointCloud& point_cloud,
                               const Grid2D& grid,
                               transform::Rigid2d* const pose_estimate,
                               ceres::Solver::Summary* const summary) 

輸入 ：
預計位姿（推算位子）
scanMatch匹配後位姿
點雲數據
概率柵格地圖
優化後位姿（輸出引用）
ceres 優化器

步驟：
構建觀測殘差——柵格不連續，三次卷積插值 使其連續可導
構建平移變化量殘差
構建旋轉變化量殘差
ceres 求解，得pose+cost

返回： 最佳得分

https://www.jianshu.com/p/9a8089cf7d3f
https://blog.csdn.net/xiaoma_bk/article/details/85046905

2.0 掃描匹配的方法

參考：https://blog.csdn.net/datase/article/details/78163063

2.1 Beam Model

Beam Model我將它叫做測量光束模型。個人理解，它是一種完全的物理模型，只針對激光發出的測量光束建模。將一次測量誤差分解爲四個誤差。

2.2 2.Likehood field

似然場模型，和測量光束模型相比，考慮了地圖的因素。不再是對激光的掃描線物理建模，而是考慮測量到的物體的因素。

似然比模型本身是一個傳感器觀測模型，之所以可以實現掃描匹配，是通過劃分柵格，步進的方式求的最大的Score，將此作爲最佳的位姿。

（以下沒有具體分析）

for k=1:size(zt,1)
    if zt(k,2)>0
        d = -grid_dim/2;
    else
        d = grid_dim/2;
    end
    phi = pi_to_pi(zt(k,2) + x(3));
    if zt(k,1) ~= Z_max
        ppx = [x(1),x(1) + zt(k,1)*cos(phi) + d];
        ppy = [x(2),x(2) + zt(k,1)*sin(phi) + d];
        end_points = [end_points;ppx(2),ppy(2)];
         
        wm = likelihood_field_range_finder_model(X(j,:)',xsensor,...
                   zt(k,:)',nearest_wall, grid_dim, std_hit,Z_weights,Z_max);
        W(j) = W(j) * wm;
    else
        dist = Z_max + std_hit*randn(1);
        ppx = [x(1),x(1) + dist*cos(phi) + d];
        ppy = [x(2),x(2) + dist*sin(phi) + d];
        missed_points = [missed_points;ppx(2),ppy(2)];               
    end
    set(handle_sensor_ray(k),'XData', ppx, 'YData', ppy)
end

function q = likelihood_field_range_finder_model(X,x_sensor,zt,N,dim,std_hit,Zw,z_max)
% retorna probabilidad de medida range finder :)
% X col, zt col, xsen col
[n,m] = size(N);
 
% Robot global position and orientation
theta = X(3);
 
% Beam global angle
theta_sen = zt(2);
phi = pi_to_pi(theta + theta_sen);
 
%Tranf matrix in case sensor has relative position respecto to robot's CG
rotS = [cos(theta),-sin(theta);sin(theta),cos(theta)];
 
% Prob. distros parameters
sigmaR = std_hit;
zhit  = Zw(1);
zrand = Zw(2);
zmax  = Zw(3);
 
% Actual algo
q = 1;
if zt(1) ~= z_max
    % get global pos of end point of measument
    xz = X(1:2) + rotS*x_sensor + zt(1)*[cos(phi);
                                         sin(phi)];
    xi = floor(xz(1)/dim) + 1;
    yi = floor(xz(2)/dim) + 1;
     
    % if end point doesn't lay inside map: unknown
    if xi<1 || xi>n || yi<1 || yi>m
        q = 1.0/z_max; % all measurements equally likely, uniform in range [0-zmax]
        return
    end
     
    dist2 = N(xi,yi);
    gd = gauss_1D(0,sigmaR,dist2);
    q = zhit*gd + zrand/zmax;
end
 
end

3.Correlation based sensor models相關分析模型

》 https://blog.csdn.net/djfjkj52/article/details/106490787

4.MCL

蒙特卡洛方法

5.AngleHistogram

角度直方圖

6.ICP/PLICP/MBICP/IDL

屬於ICP系列，經典ICP方法，點到線距離ICP

FAST ICP

% Fast scan matching, note this may get stuck in local minimas

function [pose, bestHits] = FastMatch(gridmap, scan, pose, searchResolution)

% Grid map information
metricMap = gridmap.metricMap;
ipixel = 1 / gridmap.pixelSize;
minX   = gridmap.topLeftCorner(1);
minY   = gridmap.topLeftCorner(2);
nCols  = size(metricMap, 2);
nRows  = size(metricMap, 1);

% Go down the hill
maxIter = 50;
maxDepth = 3;
iter = 0;
depth = 0;

pixelScan = scan * ipixel;
bestPose  = pose;
bestScore = Inf;
t = searchResolution(1);
r = searchResolution(3);

while iter < maxIter
    noChange = true;
    
    % Rotation
    for theta = pose(3) + [-r, 0, r]
        
        ct = cos(theta);
        st = sin(theta);
        S  = pixelScan * [ct, st; -st, ct];
        
        % Translation
        for tx = pose(1) + [-t, 0, t]
            Sx = round(S(:,1)+(tx-minX)*ipixel) + 1;
            for ty = pose(2) + [-t, 0, t]
                Sy = round(S(:,2)+(ty-minY)*ipixel) + 1;
                
                isIn = Sx>1 & Sy>1 & Sx<nCols & Sy<nRows;
                ix = Sx(isIn);
                iy = Sy(isIn);
                
                % metric socre
                idx = iy + (ix-1)*nRows;
                hits = metricMap(idx);
                score = sum(hits);
                
                % update 
                if score < bestScore
                    noChange  = false;
                    bestPose  = [tx; ty; theta];
                    bestScore = score;
                    bestHits  = hits;
                end
                
            end
        end
    end
    
    % No better match was found, increase resolution
    if noChange
        r = r / 2;
        t = t / 2;
        depth = depth + 1;
        if depth > maxDepth
            break;
        end
    end
    
    pose = bestPose;
    iter = iter + 1;
    
end

7.NDT

正態分佈變換

8.pIC

結合概率的方法

9.線特徵