int main(int argc, char **argv){ #if 0 DOF6::TFLinkvf rot1,rot2; Eigen::Matrix4f tf1 = build_random_tflink(rot1,3); Eigen::Matrix4f tf2 = build_random_tflink(rot2,3); DOF6::DOF6_Source<DOF6::TFLinkvf,DOF6::TFLinkvf,float> abc(rot1.makeShared(), rot2.makeShared()); abc.getRotation(); abc.getTranslation(); std::cout<<"tf1\n"<<tf1<<"\n"; std::cout<<"tf2\n"<<tf2<<"\n"; std::cout<<"tf1*2\n"<<tf1*tf2<<"\n"; std::cout<<"tf2*1\n"<<tf2*tf1<<"\n"; std::cout<<"tf1\n"<<rot1.getTransformation()<<"\n"; std::cout<<"tf2\n"<<rot2.getTransformation()<<"\n"; std::cout<<"tf1*2\n"<<(rot1+rot2).getTransformation()<<"\n"; rot1.check(); rot2.check(); return 0; pcl::RotationFromCorrespondences rfc; Eigen::Vector3f n, nn, v,n2,n3,z,y,tv; float a = 0.1f; z.fill(0);y.fill(0); z(2)=1;y(1)=1; nn.fill(0); nn(0) = 1; Eigen::AngleAxisf aa(a,nn); nn.fill(100); n.fill(0); n(0) = 1; n2.fill(0); n2=n; n2(1) = 0.2; n3.fill(0); n3=n; n3(2) = 0.2; n2.normalize(); n3.normalize(); tv.fill(1); tv.normalize(); #if 0 #if 0 rfc.add(n,aa.toRotationMatrix()*n+nn, 1*n.cross(y),1*n.cross(z), 1*(aa.toRotationMatrix()*n).cross(y),1*(aa.toRotationMatrix()*n).cross(z), 1,1/sqrtf(3)); #else rfc.add(n,aa.toRotationMatrix()*n, 0*n.cross(y),0*n.cross(z), 0*(aa.toRotationMatrix()*n).cross(y),0*(aa.toRotationMatrix()*n).cross(z), 1,0); #endif #if 1 rfc.add(n2,aa.toRotationMatrix()*n2+nn, tv,tv, tv,tv, 1,1); #else rfc.add(n2,aa.toRotationMatrix()*n2+nn, 1*n2.cross(y),1*n2.cross(z), 1*(aa.toRotationMatrix()*n2).cross(y),1*(aa.toRotationMatrix()*n2).cross(z), 1,1/sqrtf(3)); #endif #else float f=1; Eigen::Vector3f cyl; cyl.fill(1); cyl(0)=1; Eigen::Matrix3f cylM; cylM.fill(0); cylM.diagonal() = cyl; rfc.add(n,aa.toRotationMatrix()*n, f*n.cross(y),f*n.cross(z), f*(aa.toRotationMatrix()*n).cross(y),f*(aa.toRotationMatrix()*n).cross(z), 1,0); rfc.add(n2,aa.toRotationMatrix()*n2+nn, 1*n2.cross(y),1*n2.cross(z), 1*(aa.toRotationMatrix()*n2).cross(y),1*(aa.toRotationMatrix()*n2).cross(z), 1,1); #endif rfc.add(n3,aa.toRotationMatrix()*n3+nn, //tv,tv, //tv,tv, n3.cross(y),n3.cross(z), 1*(aa.toRotationMatrix()*n3).cross(y),1*(aa.toRotationMatrix()*n3).cross(z), 1,1); std::cout<<"comp matrix:\n"<<rfc.getTransformation()<<"\n\n"; std::cout<<"real matrix:\n"<<aa.toRotationMatrix()<<"\n\n"; return 0; //rfc.covariance_.normalize(); rfc.covariance_ = (rfc.var_*rfc.covariance_.inverse().transpose()*rfc.covariance_); std::cout<<"comp matrix: "<<rfc.getTransformation()<<"\n\n"; std::cout<<"real matrix: "<<aa.toRotationMatrix()*aa.toRotationMatrix()<<"\n\n"; return 0; #endif testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); }