00001
00002
00003
00004
00005
00006
00007
00008 #include<math.h>
00009 #include<vector>
00010 #include "icp.h"
00011 using namespace std;
00012
00013 Icp::Icp(umap &m1, umap &m2,pcl::PointCloud<pcl::PointXYZ> &cloud,double voxel_size):
00014 m_1(m1),
00015 m_2(m2),
00016 cloud_source(cloud),
00017 first_iter(true),
00018 _is_Fit(false)
00019 {
00020 this->voxel_size = voxel_size;
00021 m_1_copy = m_1;
00022 R = Matrix3d::Identity();
00023 t.setZero();
00024 jd_x = 0;
00025 jd_y = 0;
00026 jd_z = 0;
00027 RT.open("RT.txt");
00028 }
00029
00030 void Icp::least_Square()
00031 {
00032 MatrixXd A(mat_param.size(),6);
00033 VectorXd b(mat_param.size());
00034 for(int i=0;i<mat_param.size();i++)
00035 {
00036 double param1 = mat_param[i].u_b(0);
00037 double param2 = mat_param[i].u_b(1);
00038 double param3 = mat_param[i].u_b(2);
00039 Matrix3d skew_mat;
00040 skew_mat<<0,-param3,param2,param3,0,-param1,-param2,param1,0;
00041 RowVector3d r1 = mat_param[i].v1.transpose();
00042 RowVector3d r2 = r1*skew_mat;
00043
00044 A(i,0) = -r2(0);
00045 A(i,1) = -r2(1);
00046 A(i,2) = -r2(2);
00047 A(i,3) = r1(0);
00048 A(i,4) = r1(1);
00049 A(i,5) = r1(2);
00050
00051 b(i) = r1*(mat_param[i].u_a - mat_param[i].u_b);
00052 }
00053
00054
00055
00056
00057 VectorXd x = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
00058
00059 Matrix3d result_R;
00060
00061 result_R << cos(x(1))*cos(x(2)) ,-sin(x(2))*cos(x(0)) + cos(x(2))*sin(x(1))*sin(x(0)) , sin(x(2))*sin(x(0))+cos(x(2))*sin(x(1))*cos(x(0))
00062 , sin(x(2))*cos(x(1)) , cos(x(2))*cos(x(0))+sin(x(2))*sin(x(1))*sin(x(0)) , -cos(x(2))*sin(x(0)) + sin(x(2))*sin(x(1))*cos(x(0))
00063 , -sin(x(1)) , cos(x(1))*sin(x(0)) , cos(x(1))*cos(x(0));
00064
00065 MatrixXd _result_R = result_R.transpose();
00066 Vector3d result_t;
00067
00068 result_t << -x(3),-x(4),-x(5);
00069 R = _result_R*R;
00070 t = _result_R*t +result_t;
00071
00072 jd_x+=x(0);
00073 jd_y+=x(1);
00074 jd_z+=x(2);
00075 cout <<"Angle computed:=\t"<<jd_x<<"\t"<<jd_y<<"\t"<<jd_z<<endl;
00076
00077 RT<<"Rotation Matrix:\n"<<R<<endl;
00078 RT<<"t=\n"<<t<<endl;
00079 _is_Fit=is_Fit(x(0),x(1),x(2),result_t);
00080 }
00081 bool Icp::voxel_Merge(unordered_map_voxel &v1,unordered_map_voxel &v2)
00082 {
00083 umap::iterator m1_iter = m_1_copy.find(v1);
00084 umap::iterator m2_iter = m_2.find(v2);
00085
00086 temp_ls.S_merged = (m1_iter->second.matS + m2_iter->second.matS);
00087
00088 temp_ls.u_a = m1_iter->second.u;
00089 temp_ls.u_b = m2_iter->second.u;
00090 Vector3d u_average=(temp_ls.u_a+temp_ls.u_b)/2;
00091 vector<eigen_sort> vector1(3);
00092
00093 EigenSolver<MatrixXd> es(temp_ls.S_merged);
00094 VectorXcd eivals = es.eigenvalues();
00095 MatrixXcd eigenvectors = es.eigenvectors();
00096 vector1[0]=eigen_sort(eivals(0).real(),eigenvectors.col(0));
00097 vector1[1]=eigen_sort(eivals(1).real(),eigenvectors.col(1));
00098 vector1[2]=eigen_sort(eivals(2).real(),eigenvectors.col(2));
00099
00100 sort(vector1.begin(),vector1.end());
00101
00102 double lamada1 = vector1[0].eigen_value;
00103 double lamada2 = vector1[1].eigen_value;
00104 double lamada3 = vector1[2].eigen_value;
00105
00106 Vector3d tempVector=Vector3d(0,0,0)-u_average;
00107 if(Vector3d(vector1[0].eigen_vector(0).real(),vector1[0].eigen_vector(1).real(),vector1[0].eigen_vector(2).real()).dot(tempVector)<0)
00108 {
00109 vector1[0].eigen_vector(2).real() = -vector1[0].eigen_vector(2).real();
00110 }
00111 if(vector1[2].eigen_vector(2).real()<0)
00112 {
00113 vector1[2].eigen_vector(2).real() = -vector1[2].eigen_vector(2).real();
00114 }
00115
00116 temp_ls.v1 << vector1[0].eigen_vector(0).real(),vector1[0].eigen_vector(1).real(),vector1[0].eigen_vector(2).real();
00117 temp_ls.lamada1 = lamada1;
00118 mat_param.push_back(temp_ls);
00119 return true;
00120 }
00121
00122 bool Icp::linear_System()
00123 {
00124 return true;
00125 }
00126
00127 bool Icp::correction()
00128 {
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140 pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
00141 Eigen::Matrix4f transformation_matrix;
00142 transformation_matrix.setZero();
00143
00144 transformation_matrix (0,0) = R(0,0);transformation_matrix (0,1) = R(0,1);transformation_matrix (0,2) = R(0,2);
00145 transformation_matrix (1,0) = R(1,0);transformation_matrix (1,1) = R(1,1);transformation_matrix (1,2) = R(1,2);
00146 transformation_matrix (2,0) = R(2,0);transformation_matrix (2,1) = R(2,1);transformation_matrix (2,2) = R(2,2);
00147 transformation_matrix (0,3) = t(0);transformation_matrix (1,3) = t(1);transformation_matrix (2,3) = t(2);
00148
00149 pcl::transformPointCloud (cloud_source, transformed_cloud, transformation_matrix);
00150 Voxelize* voxelize1;
00151 voxelize1 = new Voxelize;
00152 m_1_copy.clear();
00153 if(first_iter)
00154 {
00155 m_1_copy = m_1;
00156 first_iter= false;
00157 }
00158 else
00159 voxelize1->generateUmap(transformed_cloud,voxel_size,m_1_copy);
00160 delete voxelize1;
00161 return true;
00162 }
00163
00164 Eigen::Matrix4f Icp::icpFit()
00165 {
00166
00167 double t0 = ros::Time::now().toSec();
00168 cout<<t0<<endl;
00169 int iter_count = 0;
00170 Voxelize* voxelize2;
00171 voxelize2 = new Voxelize;
00172
00173 int iteration_time=13;
00174 vector<bool> iteration_Result;
00175 Eigen::Matrix4f error_Return=Eigen::Matrix4f::Identity();
00176 error_Return(3,3)=0;
00177 for(int count =0;count<iteration_time;count ++)
00178 {
00179
00180 mat_param.clear();
00181 correction();
00182 double t1 = ros::Time::now().toSec();
00183 pair<umap::iterator,bool> search_result;
00184 for(umap::iterator iter=m_2.begin();iter!=m_2.end();iter++)
00185 {
00186 search_result = voxelize2->neighbor_search(m_1_copy,m_2,iter->first);
00187
00188 if(iter->second.p<0.5)
00189 {
00190 continue;
00191 }
00192
00193 if(search_result.second)
00194 {
00195 unordered_map_voxel v1(search_result.first->first.x(),search_result.first->first.y(),search_result.first->first.z());
00196 unordered_map_voxel v2(iter->first.x(),iter->first.y(),iter->first.z());
00197 voxel_Merge(v1,v2);
00198 }
00199 }
00200
00201 double t2 = ros::Time::now().toSec();
00202 if(mat_param.size()<6)
00203 break;
00204 linear_System();
00205 least_Square();
00206 iteration_Result.push_back(_is_Fit);
00207 if(end_of_iteration(iteration_Result)==true)
00208 {
00209 break;
00210 }
00211 if(iter_count>14)
00212 {
00213 return error_Return;
00214 }
00215 iter_count++;
00216
00217 }
00218 delete voxelize2;
00219
00220 Eigen::Matrix4f transformation_matrix;
00221 transformation_matrix.setZero();
00222
00223 transformation_matrix (0,0) = R(0,0);transformation_matrix (0,1) = R(0,1);transformation_matrix (0,2) = R(0,2);
00224 transformation_matrix (1,0) = R(1,0);transformation_matrix (1,1) = R(1,1);transformation_matrix (1,2) = R(1,2);
00225 transformation_matrix (2,0) = R(2,0);transformation_matrix (2,1) = R(2,1);transformation_matrix (2,2) = R(2,2);
00226 transformation_matrix (0,3) = t(0);transformation_matrix (1,3) = t(1);transformation_matrix (2,3) = t(2);
00227 cout <<"transformation_matrix=\n"<<transformation_matrix<<endl;
00228 double t4 = ros::Time::now().toSec();
00229 cout <<"匹配时间为:"<<(t4-t0)<<endl;
00230 return transformation_matrix;
00231 }
00232
00233 bool Icp::is_Fit(double angle_x,double angle_y,double angle_z,Vector3d shift_t)
00234 {
00235 double criterion_angle=3.1415926*5/180;
00236 double criterion_shift=0.1;
00237 double angle=(fabs(angle_x)+fabs(angle_y)+fabs(angle_z));
00238 double shift=(fabs(shift_t(0))+fabs(shift_t(1))+fabs(shift_t(2)));
00239 cout<<"Total Angle:"<<angle*180/3.1415926<<"\t"<<"Total shift:"<<shift<<"\t";
00240
00241 if((angle<criterion_angle)&&(shift<criterion_shift))
00242 return true;
00243 else
00244 return false;
00245 }
00246
00247 bool Icp::end_of_iteration(const vector<bool>& result)
00248 {
00249 if(result.size()<3)
00250 return false;
00251 int n=result.size()-1;
00252 if(result[n]&&result[n-1]&&(result.size()>3))
00253 return true;
00254 }