Esempio n. 1
0
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));
  }
}
Esempio n. 2
0
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;
}