TEST(DOF6, Source)
//void t1()
{
time_t ti = time(NULL);
ROS_INFO("init Source with %d",(int)ti);
srand(ti);
for(int i=0; i<CYCLES; i++) {
DOF6::TFLinkvf rot1, rot2;
const Eigen::AngleAxisf aa=createRandomAA();
const Eigen::Vector3f t=createRandomT();
//tf1 should be tf2
Eigen::Matrix4f tf1 = build_random_tflink(rot1,30,0.4,aa,t);
Eigen::Matrix4f tf2 = build_random_tflink(rot2,30,0.2,aa,t);
//check
const float d1=MATRIX_DISTANCE(rot1.getTransformation(),tf1,0.4);
const float d2=MATRIX_DISTANCE(rot2.getTransformation(),tf2,0.2);
DOF6::DOF6_Source<DOF6::TFLinkvf,DOF6::TFLinkvf> abc(rot1.makeShared(), rot2.makeShared());
// std::cout<<"rot\n"<<aa.toRotationMatrix()<<"\n";
// std::cout<<"t\n"<<t<<"\n";
//
// std::cout<<"rot\n"<<rot1.getRotation()<<"\n";
// std::cout<<"t\n"<<rot1.getTranslation()<<"\n";
//
// std::cout<<"rot\n"<<rot2.getRotation()<<"\n";
// std::cout<<"t\n"<<rot2.getTranslation()<<"\n";
//
// std::cout<<"rot\n"<<abc.getRotation().toRotMat()<<"\n";
// std::cout<<"t\n"<<abc.getTranslation()<<"\n";
//
//
// std::cout<<"getRotationVariance "<<rot1.getRotationVariance()<<"\n";
// std::cout<<"getTranslationVariance "<<rot1.getTranslationVariance()<<"\n";
//
// std::cout<<"getRotationVariance "<<rot2.getRotationVariance()<<"\n";
// std::cout<<"getTranslationVariance "<<rot2.getTranslationVariance()<<"\n";
//
// std::cout<<"getRotationVariance "<<abc.getRotationVariance()<<"\n";
// std::cout<<"getTranslationVariance "<<abc.getTranslationVariance()<<"\n";
float d3=MATRIX_DISTANCE((Eigen::Matrix3f)abc.getRotation(),aa.toRotationMatrix(),0.2);
EXPECT_NEAR((abc.getTranslation()-t).norm(),0,0.2);
d3+=(abc.getTranslation()-t).norm();
//EXPECT_LE(d3,std::max(d1,d2));
}
}
Eigen::Matrix4f build_random_tflink(DOF6::TFLinkvf &tflink, const int N=2, const float noise=0.f, const Eigen::AngleAxisf aa=createRandomAA(), const Eigen::Vector3f t=createRandomT()) {
Eigen::Vector3f n, v,n2;
v(0)=0.01f;v(2)=v(1)=1.f; //"some" plane
//std::cout<<"t\n"<<t<<"\n";
std::vector<Eigen::Vector3f> normal;
std::vector<Eigen::Vector3f> normal2;
for(int j=0; j<std::max(2,N); j++) {
//seconds
n(0) = (rand()%1000)/1000.f-0.5f; //init normals
n(1) = (rand()%1000)/1000.f-0.5f;
n(2) = (rand()%1000)/1000.f-0.5f;
n.normalize();
n2 = aa.toRotationMatrix()*n;
n2(0) += 2*noise*((rand()%1000)/1000.f-0.5f); //add noise
n2(1) += 2*noise*((rand()%1000)/1000.f-0.5f);
n2(2) += 2*noise*((rand()%1000)/1000.f-0.5f);
n2.normalize();
normal.push_back(n);
normal2.push_back(n2);
}
pcl::TransformationFromCorrespondences tfc;
for(size_t i=0; i<normal.size(); i++) {
//std::cout<<"normal\n"<<normal2[i]<<"\n";
//std::cout<<"t\n"<<(normal2[i].dot(t)*normal2[i])<<"\n";
int s=rand()%3;
float w = (rand()%100+1)/10.f;
if((s==0 && normal2[i].dot(t)>0)) //plane
{
std::cout<<"PLANE\n";
tflink(DOF6::TFLinkvf::TFLinkObj(normal[i],true,false,w),
DOF6::TFLinkvf::TFLinkObj(normal2[i]*(1+(normal2[i].dot(t)))
,true,false,w));
}
else if((s==1 && normal2[i].dot(t)>0)) //plane
{
std::cout<<"NORMAL\n";
tflink(DOF6::TFLinkvf::TFLinkObj(normal[i], true,true,w),
DOF6::TFLinkvf::TFLinkObj(normal2[i],true,true,w));
}
else
{
std::cout<<"CUBE\n";
tflink(DOF6::TFLinkvf::TFLinkObj(normal[i],false,false,w),
DOF6::TFLinkvf::TFLinkObj(normal2[i]+t,false,false,w));
}
tfc.add(normal[i],normal2[i]+t); //always with t
}
tflink.finish();
Eigen::Matrix4f r=Eigen::Matrix4f::Identity();
for(int i=0; i<3; i++) {
for(int j=0; j<3; j++)
r(i,j) = aa.toRotationMatrix()(i,j);
r.col(3)(i)= t(i);
}
static float dis2=0,dis3=0;
float d2=0,d3=0;
d2=MATRIX_DISTANCE(tflink.getTransformation(),r,10000);
d3=MATRIX_DISTANCE(tfc.getTransformation().matrix(),r,10000);
dis2+=d2;
dis3+=d3;
std::cout<<tflink<<"\n";
std::cout<<"tfl: "<<d2<<"\n";
std::cout<<"tfc: "<<d3<<"\n";
std::cout<<"dis2: "<<dis2<<"\n";
std::cout<<"dis3: "<<dis3<<"\n";
return r;
}