PLearn 0.1
|
00001 #include <plearn/io/load_and_save.h> 00002 #include <plearn/math/TMat_maths.h> 00003 #include <map> 00004 #include "Molecule.h" 00005 #include "ICP.h" 00006 using namespace PLearn; 00007 using namespace std; 00008 00009 void align_(string m1_vrml_file ,Mat m1_chem , string m2_vrml_file ,Mat m2_chem , int rotx , int roty, int rotz, Mat &wm , double &error) { 00010 00011 00012 ICP icp; 00013 00014 icp.model_features = m1_chem ; 00015 icp.scene_features = m2_chem ; 00016 00017 00018 icp.setOption("model_file",m1_vrml_file) ; 00019 icp.setOption("scene_file",m2_vrml_file) ; 00020 icp.setOption("verbosity","0") ; 00021 00022 icp.setOption("weight_method","oracle"); 00023 char buf[100] ; 00024 sprintf(buf,"0 0 0 %d %d %d" , rotx, roty, rotz) ; 00025 icp.initial_transform = string(buf) ; 00026 icp.setOption("write_vtx_match","0"); 00027 icp.setOption("max_iter","100"); 00028 icp.setOption("error_t","0.01"); 00029 icp.setOption("dist_t","0.1"); 00030 icp.build(); 00031 icp.run(); 00032 00033 int m1_size = m1_chem.length(); 00034 int m2_size = m2_chem.length(); 00035 00036 wm = Mat(m1_size , m2_size) ; 00037 wm.fill(0.0) ; 00038 for(int i=0 ; i<icp.vtx_matching.length() ; ++i) { 00039 wm[ (int)icp.vtx_matching[i][0] ][ (int)icp.vtx_matching[i][1] ] = 1 ; 00040 } 00041 error = icp.match->error ; 00042 } 00043 00044 00045 void align(string m1_vrml_file ,Mat m1_chem , string m2_vrml_file ,Mat m2_chem , Mat & wm) { 00046 int m1_size = m1_chem.length(); 00047 int m2_size = m2_chem.length(); 00048 bool switched = m1_size < m2_size ; 00049 double error = 1e10 ; 00050 int angle = 360 ; 00051 for(int rotx = 0 ; rotx<360 ; rotx += angle) 00052 for(int roty = 0 ; roty<360 ; roty += angle) 00053 for(int rotz = 0 ; rotz<360 ; rotz += angle) { 00054 double current_error ; 00055 Mat current_wm ; 00056 if (switched){ 00057 align_(m2_vrml_file , m2_chem , m1_vrml_file , m1_chem , rotx,roty,rotz,current_wm,current_error) ; 00058 } 00059 else{ 00060 align_(m1_vrml_file , m1_chem , m2_vrml_file , m2_chem , rotx,roty,rotz,current_wm,current_error) ; 00061 } 00062 cout << "current error " << current_error << endl ; 00063 if (error > current_error){ 00064 error = current_error ; 00065 wm = current_wm ; 00066 } 00067 } 00068 if (switched) 00069 wm = transpose(wm) ; 00070 00071 00072 00073 } 00074 00075 00076 00077