fixed bug in icp

#386 のコミットでできてしまったbugです。
複数の物体をtemplateにしてICPをしたときに、
どの物体が当てはまったかの結果が正しく返っていなかったので、修正しました。