// source point clouds are assumed to contain their normals
int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector<Pose3DPtr>& poses)
{
for (size_t i=0; i<poses.size(); i++)
{
double poseICP[16]={0};
Mat srcTemp = transformPCPose(srcPC, poses[i]->pose);
registerModelToScene(srcTemp, dstPC, poses[i]->residual, poseICP);
poses[i]->appendPose(poseICP);
}
return 0;
}
// source point clouds are assumed to contain their normals
int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector<Pose3DPtr>& poses)
{
#if defined _OPENMP
#pragma omp parallel for
#endif
for (int i=0; i<(int)poses.size(); i++)
{
Matx44d poseICP = Matx44d::eye();
Mat srcTemp = transformPCPose(srcPC, poses[i]->pose);
registerModelToScene(srcTemp, dstPC, poses[i]->residual, poseICP);
poses[i]->appendPose(poseICP);
}
return 0;
}