Пример #1
0
void X3DConverter::startShape(const std::array<float, 12>& matrix) {

    // Finding axis/angle from matrix using Eigen for its bullet proof implementation.
    Eigen::Transform<float, 3, Eigen::Affine> t;
    t.setIdentity();
    for (unsigned int i = 0; i < 3; i++) {
        for (unsigned int j = 0; j < 4; j++) {
            t(i, j) = matrix[i+j*3];
        }
    }

    Eigen::Matrix3f rotationMatrix;
	Eigen::Matrix3f scaleMatrix;
    t.computeRotationScaling(&rotationMatrix, &scaleMatrix);
	Eigen::Quaternionf q;
	Eigen::AngleAxisf aa;
	q = rotationMatrix;
    aa = q;

	Eigen::Vector3f scale = scaleMatrix.diagonal();
	Eigen::Vector3f translation = t.translation();

    startNode(ID::Transform);
    m_writers.back()->setSFVec3f(ID::translation, translation.x(), translation.y() , translation.z());
    m_writers.back()->setSFRotation(ID::rotation, aa.axis().x(), aa.axis().y(), aa.axis().z(), aa.angle());
    m_writers.back()->setSFVec3f(ID::scale, scale.x(), scale.y(), scale.z());
    startNode(ID::Shape);
    startNode(ID::Appearance);
    startNode(ID::Material);
    m_writers.back()->setSFColor<vector<float> >(ID::diffuseColor, RVMColorHelper::color(m_materials.back()));
    endNode(ID::Material); // Material
    endNode(ID::Appearance); // Appearance

}
Пример #2
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));
  }
}
Пример #3
0
gsl_vector* VelStereoMatcher::stereoToVec(const StereoProperties stereo)
{
  Eigen::Affine3f tform = stereo.getLeftCamera().tform;
  Eigen::Matrix4f intrinsics = stereo.getLeftCamera().intrinsics.matrix();

  Eigen::Vector3f tran;
  tran = tform.translation();
  float x = tran[0];
  float y = tran[1];
  float z = tran[2];

  Eigen::Matrix3f mat = tform.rotation();
  Eigen::AngleAxisf axis;
  axis = mat;
  float ax = axis.axis()[0] * axis.angle();
  float ay = axis.axis()[1] * axis.angle();
  float az = axis.axis()[2] * axis.angle();

  float fx = intrinsics(0,0);
  float fy = intrinsics(1,1);
  float cx = intrinsics(0,2);
  float cy = intrinsics(1,2);

  float baseline = stereo.baseline;

  gsl_vector* vec = gsl_vector_alloc(11);
  gsl_vector_set(vec, 0, x);
  gsl_vector_set(vec, 1, y);
  gsl_vector_set(vec, 2, z);
  gsl_vector_set(vec, 3, ax);
  gsl_vector_set(vec, 4, ay);
  gsl_vector_set(vec, 5, az);

  gsl_vector_set(vec, 6, fx);
  gsl_vector_set(vec, 7, fy);
  gsl_vector_set(vec, 8, cx);
  gsl_vector_set(vec, 9, cy);
  gsl_vector_set(vec, 10, baseline);

  return vec;

}
Пример #4
0
vtkSmartPointer<vtkDataSet> 
pcl::visualization::createCube (const pcl::ModelCoefficients &coefficients)
{
  // coefficients = [Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth]
  vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
  t->Identity ();
  t->Translate (coefficients.values[0], coefficients.values[1], coefficients.values[2]);
  
  Eigen::AngleAxisf a (Eigen::Quaternionf (coefficients.values[6], coefficients.values[3],
                                           coefficients.values[4], coefficients.values[5]));
  t->RotateWXYZ (pcl::rad2deg (a.angle ()), a.axis ()[0], a.axis ()[1], a.axis ()[2]);
  
  vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
  cube->SetXLength (coefficients.values[7]);
  cube->SetYLength (coefficients.values[8]);
  cube->SetZLength (coefficients.values[9]);
  
  vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
  tf->SetTransform (t);
  tf->SetInputConnection (cube->GetOutputPort ());
  
  return (tf->GetOutput ());
}
Пример #5
0
vtkSmartPointer<vtkDataSet> 
pcl::visualization::createCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation,
                                double width, double height, double depth)
{
  // coefficients = [Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth]
  vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
  t->Identity ();
  t->Translate (translation.x (), translation.y (), translation.z ());
  
  Eigen::AngleAxisf a (rotation);
  t->RotateWXYZ (pcl::rad2deg (a.angle ()), a.axis ()[0], a.axis ()[1], a.axis ()[2]);
  
  vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
  cube->SetXLength (width);
  cube->SetYLength (height);
  cube->SetZLength (depth);
  
  vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
  tf->SetTransform (t);
  tf->SetInputConnection (cube->GetOutputPort ());
  
  return (tf->GetOutput ());
}
Пример #6
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;
}
Пример #7
0
//tracker step
void Tracker::track()
{
    storage->readSceneProcessed(scene);
    if (error_count >= 30){
        //failed 30 times in a row
        ROS_ERROR("[Tracker::%s] Object is lost ... stopping tracker...",__func__);
        started = false;
        lost_it = true;
        return;
    }
    PXC::Ptr target (new PXC);
    crop_a_box(scene, target, (*bounding_box)*factor, false, *transform, false);
    if (target->points.size() <= 15){
        ROS_ERROR_THROTTLE(10,"[Tracker::%s] Not enought points in bounding box, retryng with larger bounding box", __func__);
        factor += 0.2;
        rej_distance +=0.005;
        ++error_count;
        return;
    }
    /*
     *  Alignment
     */
    //check if user changed leaf size
    double val;
    PXC::Ptr aligned = boost::make_shared<PXC>();
    basic_config->get("downsampling_leaf_size", val);
    if (val != leaf){
      leaf = val;
      computeModel();
      icp.setInputSource(model);
      pcl::CentroidPoint<PX> mc;
      for (size_t i=0; i<model->points.size(); ++i)
          mc.add(model->points[i]);
      mc.get(model_centroid);
    }
    // bool feat_align(true);
    // if (feat_align){
    //     NTC::Ptr target_n = boost::make_shared<NTC>();
    //     pcl::PointCloud<pcl::FPFHSignature33>::Ptr target_f = boost::make_shared<pcl::PointCloud<pcl::FPFHSignature33>>();
    //     ne.setRadiusSearch(2.0f*leaf);
    //     ne.useSensorOriginAsViewPoint();
    //     ne.setInputCloud(target);
    //     ne.compute(*target_n);
    //     fpfh.setInputNormals(target_n);
    //     fpfh.setInputCloud(target);
    //     fpfh.setRadiusSearch(3.5f*leaf);
    //     fpfh.compute(*target_f);
    //     //Assemble correspondences based on model-target features
    //     SearchT tree (true, CreatorT(new IndexT(4)));
    //     tree.setPointRepresentation (RepT(new pcl::DefaultFeatureRepresentation<pcl::FPFHSignature33>));
    //     tree.setChecks(256);
    //     tree.setInputCloud(target_f);
    //     //Search model features over target features
    //     //If model features are n, these will be n*k_nn matrices
    //     std::vector<std::vector<int>> k_idx;
    //     std::vector<std::vector<float>> k_dist;
    //     int k_nn(1);
    //     tree.nearestKSearch (*model_feat, std::vector<int> (), k_nn, k_idx, k_dist);
    //     //define a distance threshold
    //     float dist_thresh_m = 125.0f;
    //     //fill in model-target correpsondences
    //     pcl::Correspondences corr_m_over_t;
    //     for(size_t i=0; i < k_idx.size(); ++i)
    //     {
    //         if (k_dist[i][0] > dist_thresh_m){
    //             //we have a correspondence
    //             PX p1 (model->points[i]);
    //             PX p2 (target->points[k_idx[i][0]]);
    //             Eigen::Vector3f diff (p1.x - p2.x, p1.y - p2.y, p1.z - p2.z);
    //             float eu_dist = diff.squaredNorm();
    //             //Add a correspondence only if distance is below threshold
    //             pcl::Correspondence cor(i, k_idx[i][0], eu_dist);
    //             corr_m_over_t.push_back(cor);
    //         }
    //     }
    //     //Estimate the rigid transformation of model -> target
    //     teDQ->estimateRigidTransformation(*model, *target, corr_m_over_t, *transform);
    // }
    crd->setMaximumDistance(rej_distance);
    icp.setInputTarget(target);
    if (centroid_counter >=10){
        pcl::CentroidPoint<PX> tc;
        for (size_t i=0; i<target->points.size(); ++i)
            tc.add(target->points[i]);
        PX target_centroid, mc_transformed;
        mc_transformed = pcl::transformPoint(model_centroid, Eigen::Affine3f(*transform));
        tc.get(target_centroid);
        Eigen::Matrix4f Tcen, guess;
        Tcen << 1, 0, 0,  (target_centroid.x - mc_transformed.x),
                0, 1, 0,  (target_centroid.y - mc_transformed.y),
                0, 0, 1,  (target_centroid.z - mc_transformed.z),
                0, 0, 0,  1;
        guess = Tcen*(*transform);
        icp.align(*aligned, guess);
        centroid_counter = 0;
        ROS_WARN("[Tracker::%s] Centroid Translation Performed!",__func__);
        centroid_counter = 0;
    }
    else if (disturbance_counter >= 20)
    {
        float angx = D2R*UniformRealIn(30.0,90.0,true);
        float angy = D2R*UniformRealIn(30.0,90.0,true);
        float angz = D2R*UniformRealIn(30.0,90.0,true);
        Eigen::Matrix4f T_rotx, T_roty, T_rotz;
        if (UniformIntIn(0,1))
            angx *= -1;
        if (UniformIntIn(0,1))
            angy *= -1;
        if (UniformIntIn(0,1))
            angz *= -1;
        Eigen::AngleAxisf rotx (angx, Eigen::Vector3f::UnitX());
        T_rotx<< rotx.matrix()(0,0), rotx.matrix()(0,1), rotx.matrix()(0,2), 0,
                rotx.matrix()(1,0), rotx.matrix()(1,1), rotx.matrix()(1,2), 0,
                rotx.matrix()(2,0), rotx.matrix()(2,1), rotx.matrix()(2,2), 0,
                0,                0,                  0,                 1;
        Eigen::AngleAxisf roty (angy, Eigen::Vector3f::UnitY());
        T_roty<< roty.matrix()(0,0), roty.matrix()(0,1), roty.matrix()(0,2), 0,
                roty.matrix()(1,0), roty.matrix()(1,1), roty.matrix()(1,2), 0,
                roty.matrix()(2,0), roty.matrix()(2,1), roty.matrix()(2,2), 0,
                0,                0,                  0,                 1;
        Eigen::AngleAxisf rotz (angz, Eigen::Vector3f::UnitZ());
        T_rotz<< rotz.matrix()(0,0), rotz.matrix()(0,1), rotz.matrix()(0,2), 0,
                rotz.matrix()(1,0), rotz.matrix()(1,1), rotz.matrix()(1,2), 0,
                rotz.matrix()(2,0), rotz.matrix()(2,1), rotz.matrix()(2,2), 0,
                0,                0,                  0,                 1;
        Eigen::Matrix4f disturbed, inverse;
        inverse = transform->inverse();
        disturbed = (T_rotz*T_roty*T_rotx*inverse).inverse();
        ROS_WARN("[Tracker::%s] Triggered Disturbance! With angles %g, %g, %g",__func__,  angx*R2D, angy*R2D, angz*R2D);
        icp.align(*aligned, disturbed);
        disturbance_counter = 0;
    }
    else
        icp.align(*aligned, *transform);
    fitness = icp.getFitnessScore();
    // ROS_WARN("Fitness %g", fitness);
    *(transform) = icp.getFinalTransformation();
    //adjust distance and factor according to fitness
    if (fitness > 0.001 ){
        //fitness is high something is prolly wrong
        rej_distance +=0.001;
        factor += 0.05;
        if (rej_distance > 2.0)
            rej_distance = 2.0;
        if (factor > 5.0)
            factor = 5.0;
        ++disturbance_counter;
        ++centroid_counter;
    }
    else if (fitness < 0.0006){
        //all looks good
        rej_distance -=0.005;
        if(rej_distance < 0.015)
            rej_distance = 0.015; //we dont want to go lower than this
        factor -=0.05;
        if(factor < 1.1)
            factor = 1.1;
        error_count = 0;
        disturbance_counter = 0;
        centroid_counter = 0;
    }
}
Пример #8
0
void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
  std::ostringstream s;
  s << "Feedback from marker '" << feedback->marker_name << "' "
	<< " / control '" << feedback->control_name << "'";

  switch ( feedback->event_type )
  {
  case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
	  mouseInUse = true;
	  break;
  case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
  {
	  // Compute FK for end effectors that have changed
	  // Call IK to get the joint states
	  moveit_msgs::GetPositionFKRequest req;
	  req.fk_link_names.push_back("RightArm");
	  req.robot_state.joint_state = planState;
	  req.header.stamp = ros::Time::now();

	  moveit_msgs::GetPositionFKResponse resp;
	  gFKinClient.call(req, resp);

	  std::cerr << "Response: " << resp.pose_stamped[0] << std::endl;

	  if (resp.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
	  {
		  joyInt.currentPose = resp.pose_stamped[0];
	  }
	  else
	  {
		  ROS_ERROR_STREAM("Failed to solve FK: " << resp.error_code.val);
	  }

	  gRPosePublisher.publish(joyInt.currentPose);
	  mouseInUse = false;
	  break;
  }

  case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
  {
	  Eigen::AngleAxisf aa;
	  aa = Eigen::Quaternionf(feedback->pose.orientation.w,
							  feedback->pose.orientation.x,
							  feedback->pose.orientation.y,
							  feedback->pose.orientation.z);

	  boost::shared_ptr<const urdf::Joint> targetJoint = huboModel.getJoint(feedback->marker_name);
	  Eigen::Vector3f axisVector = hubo_motion_ros::toEVector3(targetJoint->axis);
	  float angle;
	  // Get sign of angle
	  if (aa.axis().dot(axisVector) < 0)
	  {
		  angle = -aa.angle();
	  }
	  else
	  {
		  angle = aa.angle();
	  }

	  // Trim angle to joint limits
	  if (angle > targetJoint->limits->upper)
	  {
		  angle = targetJoint->limits->upper;
	  }
	  else if (angle < targetJoint->limits->lower)
	  {
		  angle = targetJoint->limits->lower;
	  }

	  // Locate the index of the solution joint in the plan state
	  for (int j = 0; j < planState.name.size(); j++)
	  {
		  if (feedback->marker_name == planState.name[j])
		  {
			  planState.position[j] = angle;
		  }
	  }

	  prevAA = aa;

	  // Time and Frame stamps
	  planState.header.frame_id = "/Body_TSY";
	  planState.header.stamp = ros::Time::now();

	  gStatePublisher.publish(planState);
	  break;
  }
  }

  gIntServer->applyChanges();
}