Fixed some bugs in my ICP v2 with optmization and with the initial hierarchical featured-based classifier