Exemplos de Matrix3f::fill em C++ (Cpp)

Exemplo n.º 1

Arquivo: atoms.cpp Projeto: Etimr/cob_environment_perception

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();
}