fix bugs in icp.hpp