# Revision history [back]

### iterative closest point

I want to implement ICP(iterative closest point) algorithm

1. Associate points by the nearest neighbor criteria.
2. Estimate transformation parameters using a mean square cost function.
3. Transform the points using the estimated parameters.
4. Iterate (re-associate the points and so on). For every point in 1st set I found nearist point in 2nd set, but I don't understand how to do the 2nd step.

### iterative closest point

I want to implement ICP(iterative closest point) algorithm

1. Associate points by the nearest neighbor criteria.
2. Estimate transformation parameters using a mean square cost function.
3. Transform the points using the estimated parameters.
4. Iterate (re-associate the points and so on). For every point in 1st set I found nearist point in 2nd set, but I don't understand how to do the 2nd step.

### iterative closest point

I want to implement ICP(iterative closest point) algorithm

1. Associate points by the nearest neighbor criteria.
2. Estimate transformation parameters using a mean square cost function.
3. Transform the points using the estimated parameters.
4. Iterate (re-associate the points and so on). For every point in 1st set I found nearist nearest point in 2nd set, but I don't understand how to do the 2nd step.

### iterative closest point

I want to implement ICP(iterative closest point) algorithm

1. Associate points by the nearest neighbor criteria.
2. Estimate transformation parameters using a mean square cost function.
3. Transform the points using the estimated parameters.
4. Iterate (re-associate the points and so on). For every point in 1st set I found nearest point in 2nd set, but I don't understand how to do the 2nd step.

I tried folowing code< but it does't work

    vector<Point> vec_pair;
for(int i=0;i<vec_M.size();++i)
{
double min_dist=INT_MAX;
int id=-1;
for(int j=0;j<vec_T.size();++j)
{
double metric= sqrt(double(vec_T[j].x-vec_M[i].x)*(vec_T[j].x-vec_M[i].x)+
(vec_T[j].y-vec_M[i].y)*(vec_T[j].y-vec_M[i].y));
if(min_dist>metric)
{
min_dist=metric;
id=j;
}
}
line(img,vec_M[i],vec_T[id],cvScalar(0,0,255));
vec_pair.push_back(vec_T[id]);
}
Mat m= estimateRigidTransform(vec_M,vec_pair,1);
cout<<m;


### iterative closest point

I want to implement ICP(iterative closest point) algorithm

1. Associate points by the nearest neighbor criteria.
2. Estimate transformation parameters using a mean square cost function.
3. Transform the points using the estimated parameters.
4. Iterate (re-associate the points and so on). For every point in 1st set I found nearest point in 2nd set, but I don't understand how to do the 2nd step.

I tried folowing code< code, but it does't workwork, maybe I need to reject some pairs?

    vector<Point> vec_pair;
for(int i=0;i<vec_M.size();++i)
{
double min_dist=INT_MAX;
int id=-1;
for(int j=0;j<vec_T.size();++j)
{
double metric= sqrt(double(vec_T[j].x-vec_M[i].x)*(vec_T[j].x-vec_M[i].x)+
(vec_T[j].y-vec_M[i].y)*(vec_T[j].y-vec_M[i].y));
if(min_dist>metric)
{
min_dist=metric;
id=j;
}
}
line(img,vec_M[i],vec_T[id],cvScalar(0,0,255));
vec_pair.push_back(vec_T[id]);
}
Mat m= estimateRigidTransform(vec_M,vec_pair,1);
cout<<m;