How to apply filter when denoising the mesh?

[xinpp123]

I’m implementing a mesh denoising algorithm using openGL.The mesh is using half-edge structure.
While I had some problem when applying the bilateral filter.
Here’s my code:
void bilatRemove(float FsigmaC, float FsigmaS)
{
map<HE_vert*, float>neighbor;
HE_vert centPoint;
vector3d centNorm, neighPoint;
centNorm.setVector(0, 0, 0);
float amp = AverageEdgeLength() / 100;
int i = 0;
double sigmaC = FsigmaC;
double sigmaS = FsigmaS;

for(vector<HE_vert>::iterator iter = vertex_map.begin();
    iter != vertex_map.end(); ++iter)
{
    neighbor.clear();
    centPoint = *iter;
    centNorm.setVector(0, 0, 0);
    centNorm.setVector(iter->vx, iter->vy, iter->vz);
    double sum = 0, normalizer = 0;
    
    double x = 0, y = 0, z = 0, vx = 0, vy = 0, vz = 0;
    i++;

    
    //store each vertex's neighbor by going through
    HE_edge *curEdge = iter->edge;
    HE_edge *tmpEdge = iter->edge;
    do
    {
        neighbor.insert(make_pair(tmpEdge->to_vert, 0));
        tmpEdge = tmpEdge->twin_edge->next_edge;
    }while(tmpEdge != iter->edge);
    
    for(map<HE_vert*, float>::iterator neighbor_iter = neighbor.begin();
        neighbor_iter != neighbor.end(); ++neighbor_iter)
    {
        vector3d neighEdge;
        neighEdge.xPos = (curEdge->prev_edge->to_vert->x) - (neighbor_iter->first->x);
        neighEdge.yPos = (curEdge->prev_edge->to_vert->y) - (neighbor_iter->first->y);
        neighEdge.zPos = (curEdge->prev_edge->to_vert->z) - (neighbor_iter->first->z);
        //neighEdge.printVector();

        double t = Magnitude(neighEdge);
        double h = Dot(neighEdge, centNorm);
        double omegaC = exp(-t*t / (2.0 * sigmaC * sigmaC));
        double omegaS = exp(-h*h / (2.0 * sigmaS * sigmaS));
        
        sum += omegaC * omegaS * t;
        normalizer += omegaC * omegaS;
    }
//move along the direction of vertex normal
x = curEdge->prev_edge->to_vert->x;
y = curEdge->prev_edge->to_vert->y;
z = curEdge->prev_edge->to_vert->z;

vx = curEdge->prev_edge->to_vert->vx;
vy = curEdge->prev_edge->to_vert->vy;
vz = curEdge->prev_edge->to_vert->vz;
    
x += (sum / normalizer) * vx * amp;
y += (sum / normalizer) * vy * amp;
z += (sum / normalizer) * vz * amp;
    
iter->x = x;
iter->y = y;
iter->z = z;
}
glutPostRedisplay();
getNorm();
cout<<"Noise Removed using Bilateral Filter!

";
}