隔了一段時間回頭再看costmap的更新，結合自己使用gridmap的經歷，發現costmap中那麼多程式碼無非是做2件事情，一是限定感測器探測掃描過的區域大小和邊界（即後續要更新的區域），二是根據感測器模型和貝葉斯方法計算上述區域中所有柵格的更新後的被佔據概率。

         costmap中使用的是扇形感測器模型，認為感測器的探測區域是一個以sensor為中心的一定視場角的扇形，然後評估扇形中每個柵格到sensor的距離和與sensor中軸線的夾角，以此計算最新一次觀測該柵格的概率。我在實際應用中，遇到了以下幾個問題：

• 障礙物面向sensor的表面與sensor的距離是r，障礙物理應位於比r更遠的位置，因此，扇形的半徑R應略大於測距返回值r，否則，位於r附近的柵格（被佔據概率較大）會從扇形中排除而不被更新到，導致錯誤。
• 如下程式碼所示，計算概率時依據柵格到sensor的距離，分為4種情況。0---r-2dr，這個範圍內的概率很小，並且所有柵格概率相同；r-2dr---r-dr，這個範圍內的概率較小，方程2的最大值是0.5；r-dr---r+dr，這個範圍內的概率較大，方程3的最小值是0.5；其他的給賦予值0.5。因為感測器測距值是有抖動的，導致距離分界值也會抖動，對於接近r的某一個柵格A，有可能會使用方程3計算，則B可能使用方程2計算，下一次觀測時，也可能A、B都使用方程3或2計算，導致A、B的概率值也在忽上忽下的抖動，始終不會平穩的更新。這個問題是感測器資料引起的，我想到的避免方法就是對距離平滑濾波，但是會降低更新速度，影響使用效果。
• 使用gridmap時，可以在sensor座標系下求取扇形所在多邊形的5個頂點（sensor, plm, plt, prt, prm, 注意要按順序組成閉合空間），然後轉換到全域性座標系，依據對角度、距離的限制，劃定扇形的範圍，使用多邊形迭代器處理其中的柵格。

double Costmap2D::SensorModel(double r, double phi, double theta)
{
    double lbda = Delta(phi)*Gamma(theta);
    double delta = (resolution_ > 1 ? resolution_/100.0 : resolution_);
    double res = -1.0;

    if(phi >= 0.0 and phi < r - 2 * delta * r)
        res = (1- lbda) * (0.5);        //方程1
    else if(phi < r - delta * r)
        res = lbda* 0.5 * pow((phi - (r - 2*delta*r))/(delta*r), 2)+(1-lbda)*.5;        //方程2
    else if(phi < r + delta * r){
        double J = (r-phi)/(delta*r);
        res = lbda * ((1-(0.5)*pow(J,2)) -0.5) + 0.5;        //方程3
    }
    else
        res = 0.5;        //方程4
    return res;
}