Beispiel #1
0
 inline vcl_size_t size(Eigen::VectorXf const & v) { return v.rows(); }
Beispiel #2
0
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::PFHEstimation<PointInT, PointNT, PointOutT>::computePointPFHSignature (
      const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
      const std::vector<int> &indices, int nr_split, Eigen::VectorXf &pfh_histogram)
{
  int h_index, h_p;

  // Clear the resultant point histogram
  pfh_histogram.setZero ();

  // Factorization constant
  float hist_incr = 100.0f / static_cast<float> (indices.size () * (indices.size () - 1) / 2);

  std::pair<int, int> key;
  // Iterate over all the points in the neighborhood
  for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
  {
    for (size_t j_idx = 0; j_idx < i_idx; ++j_idx)
    {
      // If the 3D points are invalid, don't bother estimating, just continue
      if (!isFinite (cloud.points[indices[i_idx]]) || !isFinite (cloud.points[indices[j_idx]]))
        continue;

      if (use_cache_)
      {
        // In order to create the key, always use the smaller index as the first key pair member
        int p1, p2;
  //      if (indices[i_idx] >= indices[j_idx])
  //      {
          p1 = indices[i_idx];
          p2 = indices[j_idx];
  //      }
  //      else
  //      {
  //        p1 = indices[j_idx];
  //        p2 = indices[i_idx];
  //      }
        key = std::pair<int, int> (p1, p2);

        // Check to see if we already estimated this pair in the global hashmap
        std::map<std::pair<int, int>, Eigen::Vector4f, std::less<std::pair<int, int> >, Eigen::aligned_allocator<Eigen::Vector4f> >::iterator fm_it = feature_map_.find (key);
        if (fm_it != feature_map_.end ())
          pfh_tuple_ = fm_it->second;
        else
        {
          // Compute the pair NNi to NNj
          if (!computePairFeatures (cloud, normals, indices[i_idx], indices[j_idx],
                                    pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3]))
            continue;
        }
      }
      else
        if (!computePairFeatures (cloud, normals, indices[i_idx], indices[j_idx],
                                  pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3]))
          continue;

      // Normalize the f1, f2, f3 features and push them in the histogram
      f_index_[0] = static_cast<int> (floor (nr_split * ((pfh_tuple_[0] + M_PI) * d_pi_)));
      if (f_index_[0] < 0)         f_index_[0] = 0;
      if (f_index_[0] >= nr_split) f_index_[0] = nr_split - 1;

      f_index_[1] = static_cast<int> (floor (nr_split * ((pfh_tuple_[1] + 1.0) * 0.5)));
      if (f_index_[1] < 0)         f_index_[1] = 0;
      if (f_index_[1] >= nr_split) f_index_[1] = nr_split - 1;

      f_index_[2] = static_cast<int> (floor (nr_split * ((pfh_tuple_[2] + 1.0) * 0.5)));
      if (f_index_[2] < 0)         f_index_[2] = 0;
      if (f_index_[2] >= nr_split) f_index_[2] = nr_split - 1;

      // Copy into the histogram
      h_index = 0;
      h_p     = 1;
      for (int d = 0; d < 3; ++d)
      {
        h_index += h_p * f_index_[d];
        h_p     *= nr_split;
      }
      pfh_histogram[h_index] += hist_incr;

      if (use_cache_)
      {
        // Save the value in the hashmap
        feature_map_[key] = pfh_tuple_;

        // Use a maximum cache so that we don't go overboard on RAM usage
        key_list_.push (key);
        // Check to see if we need to remove an element due to exceeding max_size
        if (key_list_.size () > max_cache_size_)
        {
          // Remove the last element.
          feature_map_.erase (key_list_.back ());
          key_list_.pop ();
        }
      }
    }
  }
}
Beispiel #3
0
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCone<PointT, PointNT>::computeModelCoefficients (
    const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
{
  // Need 3 samples
  if (samples.size () != 3)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
    return (false);
  }

  if (!normals_)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] No input dataset containing normals was given!\n");
    return (false);
  }

  Eigen::Vector4f p1 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z, 0);
  Eigen::Vector4f p2 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z, 0);
  Eigen::Vector4f p3 (input_->points[samples[2]].x, input_->points[samples[2]].y, input_->points[samples[2]].z, 0);

  Eigen::Vector4f n1 (normals_->points[samples[0]].normal[0], normals_->points[samples[0]].normal[1], normals_->points[samples[0]].normal[2], 0);
  Eigen::Vector4f n2 (normals_->points[samples[1]].normal[0], normals_->points[samples[1]].normal[1], normals_->points[samples[1]].normal[2], 0);
  Eigen::Vector4f n3 (normals_->points[samples[2]].normal[0], normals_->points[samples[2]].normal[1], normals_->points[samples[2]].normal[2], 0);

  //calculate apex (intersection of the three planes defined by points and belonging normals
  Eigen::Vector4f ortho12 = n1.cross3(n2);
  Eigen::Vector4f ortho23 = n2.cross3(n3);
  Eigen::Vector4f ortho31 = n3.cross3(n1);

  float denominator = n1.dot(ortho23);

  float d1 = p1.dot (n1);
  float d2 = p2.dot (n2);
  float d3 = p3.dot (n3);

  Eigen::Vector4f apex = (d1 * ortho23 + d2 * ortho31 + d3 * ortho12) / denominator;

  //compute axis (normal of plane defined by: { apex+(p1-apex)/(||p1-apex||), apex+(p2-apex)/(||p2-apex||), apex+(p3-apex)/(||p3-apex||)}
  Eigen::Vector4f ap1 = p1 - apex;
  Eigen::Vector4f ap2 = p2 - apex;
  Eigen::Vector4f ap3 = p3 - apex;

  Eigen::Vector4f np1 = apex + (ap1/ap1.norm ());
  Eigen::Vector4f np2 = apex + (ap2/ap2.norm ());
  Eigen::Vector4f np3 = apex + (ap3/ap3.norm ());

  Eigen::Vector4f np1np2 = np2 - np1;
  Eigen::Vector4f np1np3 = np3 - np1;

  Eigen::Vector4f axis_dir = np1np2.cross3 (np1np3);
  axis_dir.normalize ();

  // normalize the vector (apex->p) for opening angle calculation
  ap1.normalize ();
  ap2.normalize ();
  ap3.normalize ();

  //compute opening angle
  float opening_angle = ( acosf (ap1.dot (axis_dir)) + acosf (ap2.dot (axis_dir)) + acosf (ap3.dot (axis_dir)) ) / 3.0f;

  model_coefficients.resize (7);
  // model_coefficients.template head<3> ()    = line_pt.template head<3> ();
  model_coefficients[0] = apex[0];
  model_coefficients[1] = apex[1];
  model_coefficients[2] = apex[2];
  // model_coefficients.template segment<3> (3) = line_dir.template head<3> ();
  model_coefficients[3] = axis_dir[0];
  model_coefficients[4] = axis_dir[1];
  model_coefficients[5] = axis_dir[2];
  // cone radius
  model_coefficients[6] = opening_angle;

  if (model_coefficients[6] != -std::numeric_limits<double>::max() && model_coefficients[6] < min_angle_)
    return (false);
  if (model_coefficients[6] !=  std::numeric_limits<double>::max() && model_coefficients[6] > max_angle_)
    return (false);

  return (true);
}
Beispiel #4
0
TEST (RANSAC, SampleConsensusModelCircle3D)
{
  srand (0);

  // Use a custom point cloud for these tests until we need something better
  PointCloud<PointXYZ> cloud;
  cloud.points.resize (20);

  cloud.points[0].x = 1.0f;  	    cloud.points[0].y = 5.0f;        cloud.points[0].z = -2.9000001f;
  cloud.points[1].x = 1.034202f;    cloud.points[1].y = 5.0f;        cloud.points[1].z = -2.9060307f;
  cloud.points[2].x = 1.0642787f;   cloud.points[2].y = 5.0f;        cloud.points[2].z = -2.9233956f;
  cloud.points[3].x = 1.0866026f;   cloud.points[3].y = 5.0f;  	     cloud.points[3].z = -2.95f;
  cloud.points[4].x = 1.0984808f;   cloud.points[4].y = 5.0f;  	     cloud.points[4].z = -2.9826353f;
  cloud.points[5].x = 1.0984808f;   cloud.points[5].y = 5.0f;        cloud.points[5].z = -3.0173647f;
  cloud.points[6].x = 1.0866026f;   cloud.points[6].y = 5.0f;  	     cloud.points[6].z = -3.05f;
  cloud.points[7].x = 1.0642787f;   cloud.points[7].y = 5.0f;  	     cloud.points[7].z = -3.0766044f;
  cloud.points[8].x = 1.034202f;    cloud.points[8].y = 5.0f;  	     cloud.points[8].z = -3.0939693f;
  cloud.points[9].x = 1.0f;         cloud.points[9].y = 5.0f;  	     cloud.points[9].z = -3.0999999f;
  cloud.points[10].x = 0.96579796f; cloud.points[10].y = 5.0f; 	     cloud.points[10].z = -3.0939693f;
  cloud.points[11].x = 0.93572122f; cloud.points[11].y = 5.0f; 	     cloud.points[11].z = -3.0766044f;
  cloud.points[12].x = 0.91339743f; cloud.points[12].y = 5.0f; 	     cloud.points[12].z = -3.05f;
  cloud.points[13].x = 0.90151924f; cloud.points[13].y = 5.0f; 	     cloud.points[13].z = -3.0173647f;
  cloud.points[14].x = 0.90151924f; cloud.points[14].y = 5.0f; 	     cloud.points[14].z = -2.9826353f;
  cloud.points[15].x = 0.91339743f; cloud.points[15].y = 5.0f; 	     cloud.points[15].z = -2.95f;
  cloud.points[16].x = 0.93572122f; cloud.points[16].y = 5.0f; 	     cloud.points[16].z = -2.9233956f;
  cloud.points[17].x = 0.96579796f; cloud.points[17].y = 5.0;        cloud.points[17].z = -2.9060307f;
  cloud.points[18].x = 0.85000002f; cloud.points[18].y = 4.8499999f; cloud.points[18].z = -3.1500001f;
  cloud.points[19].x = 1.15f; 	    cloud.points[19].y = 5.1500001f; cloud.points[19].z = -2.8499999f;

  // Create a shared 3d circle model pointer directly
  SampleConsensusModelCircle3DPtr model (new SampleConsensusModelCircle3D<PointXYZ> (cloud.makeShared ()));

  // Create the RANSAC object
  RandomSampleConsensus<PointXYZ> sac (model, 0.03);

  // Algorithm tests
  bool result = sac.computeModel ();
  ASSERT_EQ (result, true);

  std::vector<int> sample;
  sac.getModel (sample);
  EXPECT_EQ (int (sample.size ()), 3);

  std::vector<int> inliers;
  sac.getInliers (inliers);
  EXPECT_EQ (int (inliers.size ()), 18);

  Eigen::VectorXf coeff;
  sac.getModelCoefficients (coeff);
  EXPECT_EQ (int (coeff.size ()), 7);
  EXPECT_NEAR (coeff[0],  1, 1e-3);
  EXPECT_NEAR (coeff[1],  5, 1e-3);
  EXPECT_NEAR (coeff[2], -3, 1e-3);
  EXPECT_NEAR (coeff[3],0.1, 1e-3);
  EXPECT_NEAR (coeff[4],  0, 1e-3);
  EXPECT_NEAR (coeff[5], -1, 1e-3);
  EXPECT_NEAR (coeff[6],  0, 1e-3);

  Eigen::VectorXf coeff_refined;
  model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
  EXPECT_EQ (int (coeff_refined.size ()), 7);
  EXPECT_NEAR (coeff_refined[0],  1, 1e-3);
  EXPECT_NEAR (coeff_refined[1],  5, 1e-3);
  EXPECT_NEAR (coeff_refined[2], -3, 1e-3);
  EXPECT_NEAR (coeff_refined[3],0.1, 1e-3);
  EXPECT_NEAR (coeff_refined[4],  0, 1e-3);
  EXPECT_NEAR (coeff_refined[5], -1, 1e-3);
  EXPECT_NEAR (coeff_refined[6],  0, 1e-3);
}
	Eigen::MatrixXf featureGradient( const Eigen::MatrixXf & a, const Eigen::MatrixXf & b ) const {
		if (ntype_ == NO_NORMALIZATION )
			return kernelGradient( a, b );
		else if (ntype_ == NORMALIZE_SYMMETRIC ) {
			Eigen::MatrixXf fa = lattice_.compute( a*norm_.asDiagonal(), true );
			Eigen::MatrixXf fb = lattice_.compute( b*norm_.asDiagonal() );
			Eigen::MatrixXf ones = Eigen::MatrixXf::Ones( a.rows(), a.cols() );
			Eigen::VectorXf norm3 = norm_.array()*norm_.array()*norm_.array();
			Eigen::MatrixXf r = kernelGradient( 0.5*( a.array()*fb.array() + fa.array()*b.array() ).matrix()*norm3.asDiagonal(), ones );
			return - r + kernelGradient( a*norm_.asDiagonal(), b*norm_.asDiagonal() );
		}
		else if (ntype_ == NORMALIZE_AFTER ) {
			Eigen::MatrixXf fb = lattice_.compute( b );
			
			Eigen::MatrixXf ones = Eigen::MatrixXf::Ones( a.rows(), a.cols() );
			Eigen::VectorXf norm2 = norm_.array()*norm_.array();
			Eigen::MatrixXf r = kernelGradient( ( a.array()*fb.array() ).matrix()*norm2.asDiagonal(), ones );
			return - r + kernelGradient( a*norm_.asDiagonal(), b );
		}
		else /*if (ntype_ == NORMALIZE_BEFORE )*/ {
			Eigen::MatrixXf fa = lattice_.compute( a, true );
			
			Eigen::MatrixXf ones = Eigen::MatrixXf::Ones( a.rows(), a.cols() );
			Eigen::VectorXf norm2 = norm_.array()*norm_.array();
			Eigen::MatrixXf r = kernelGradient( ( fa.array()*b.array() ).matrix()*norm2.asDiagonal(), ones );
			return -r+kernelGradient( a, b*norm_.asDiagonal() );
		}
	}
bool
ObjectModelPlaneFromLines::computeModelCoefficients (const std::vector<int> &sample,
		Eigen::VectorXf &modelCoefficients){

	line line1,line2;

	//Create line1 from sample[0] and sample[1]
	line1.pbase = (*inputPointCloud->getPointCloud())[sample[0]];
	line1.director.resize(3);
	line1.director[0]=(*inputPointCloud->getPointCloud())[sample[1]].getX() - (*inputPointCloud->getPointCloud())[sample[0]].getX();
	line1.director[1]=(*inputPointCloud->getPointCloud())[sample[1]].getY() - (*inputPointCloud->getPointCloud())[sample[0]].getY();
	line1.director[2]=(*inputPointCloud->getPointCloud())[sample[1]].getZ() - (*inputPointCloud->getPointCloud())[sample[0]].getZ();

	//Create line1 from sample[2] and sample[3]
	line2.pbase = (*inputPointCloud->getPointCloud())[sample[2]];
	line2.director.resize(3);
	line2.director[0]=(*inputPointCloud->getPointCloud())[sample[3]].getX() - (*inputPointCloud->getPointCloud())[sample[2]].getX();
	line2.director[1]=(*inputPointCloud->getPointCloud())[sample[3]].getY() - (*inputPointCloud->getPointCloud())[sample[2]].getY();
	line2.director[2]=(*inputPointCloud->getPointCloud())[sample[3]].getZ() - (*inputPointCloud->getPointCloud())[sample[2]].getZ();



	//Compute the model co-efficient of the lines from these lines
	std::vector<double> normal;
	normal.resize(3);

	crossProduct3D(line1.director,line2.director,normal);

	modelCoefficients.resize(4);

	modelCoefficients[0] = normal[0];
	modelCoefficients[1] = normal[1];
	modelCoefficients[2] = normal[2];

	modelCoefficients[3]= -modelCoefficients[0] * line1.pbase.getX() -
			modelCoefficients[1] * line1.pbase.getY() -
			modelCoefficients[2] * line1.pbase.getZ();


	if (abs(modelCoefficients[0])<geometryEpsilon&&abs(modelCoefficients[1])<geometryEpsilon&&abs(modelCoefficients[2])<geometryEpsilon)	{
		//Lines are parallel
		if (contains(line1, line2.pbase))
			return false;

		//Use a line's director vector and both pBase's difference to create the plane.
		std::vector<double> baseDifference;
		baseDifference.resize(3);
		baseDifference[0]=line1.pbase.getX()-line2.pbase.getX();
		baseDifference[1]=line1.pbase.getY()-line2.pbase.getY();
		baseDifference[2]=line1.pbase.getZ()-line2.pbase.getZ();

		crossProduct3D(line1.director,baseDifference,normal);
		modelCoefficients[0] = normal[0];
		modelCoefficients[1] = normal[1];
		modelCoefficients[2] = normal[2];

		modelCoefficients[3]=-modelCoefficients[0] * line1.pbase.getX() -modelCoefficients[1] * line1.pbase.getY() -modelCoefficients[2] * line1.pbase.getZ();

	}	else {

		double  x = modelCoefficients[0]*line2.pbase.getX()+
				modelCoefficients[1]*line2.pbase.getY()+
				modelCoefficients[2]*line2.pbase.getZ()+modelCoefficients[3];
		if (abs(x)>=geometryEpsilon) {
			cout<<"Lines do not intersect"<<endl;
			return false;
		}
	}

	return true;
}
Beispiel #7
0
TEST (RANSAC, SampleConsensusModelCone)
{
  srand (0);

  // Use a custom point cloud for these tests until we need something better
  PointCloud<PointXYZ> cloud;
  PointCloud<Normal> normals;
  cloud.points.resize (31); normals.points.resize (31);

  cloud.points[0].x = -0.011247f; cloud.points[0].y = 0.200000f; cloud.points[0].z = 0.965384f; 
  cloud.points[1].x =  0.000000f; cloud.points[1].y = 0.200000f; cloud.points[1].z = 0.963603f; 
  cloud.points[2].x =  0.011247f; cloud.points[2].y = 0.200000f; cloud.points[2].z = 0.965384f; 
  cloud.points[3].x = -0.016045f; cloud.points[3].y = 0.175000f; cloud.points[3].z = 0.977916f; 
  cloud.points[4].x = -0.008435f; cloud.points[4].y = 0.175000f; cloud.points[4].z = 0.974038f; 
  cloud.points[5].x =  0.004218f; cloud.points[5].y = 0.175000f; cloud.points[5].z = 0.973370f; 
  cloud.points[6].x =  0.016045f; cloud.points[6].y = 0.175000f; cloud.points[6].z = 0.977916f; 
  cloud.points[7].x = -0.025420f; cloud.points[7].y = 0.200000f; cloud.points[7].z = 0.974580f; 
  cloud.points[8].x =  0.025420f; cloud.points[8].y = 0.200000f; cloud.points[8].z = 0.974580f; 
  cloud.points[9].x = -0.012710f; cloud.points[9].y = 0.150000f; cloud.points[9].z = 0.987290f; 
  cloud.points[10].x = -0.005624f; cloud.points[10].y = 0.150000f; cloud.points[10].z = 0.982692f; 
  cloud.points[11].x =  0.002812f; cloud.points[11].y = 0.150000f; cloud.points[11].z = 0.982247f; 
  cloud.points[12].x =  0.012710f; cloud.points[12].y = 0.150000f; cloud.points[12].z = 0.987290f; 
  cloud.points[13].x = -0.022084f; cloud.points[13].y = 0.175000f; cloud.points[13].z = 0.983955f; 
  cloud.points[14].x =  0.022084f; cloud.points[14].y = 0.175000f; cloud.points[14].z = 0.983955f; 
  cloud.points[15].x = -0.034616f; cloud.points[15].y = 0.200000f; cloud.points[15].z = 0.988753f; 
  cloud.points[16].x =  0.034616f; cloud.points[16].y = 0.200000f; cloud.points[16].z = 0.988753f; 
  cloud.points[17].x = -0.006044f; cloud.points[17].y = 0.125000f; cloud.points[17].z = 0.993956f; 
  cloud.points[18].x =  0.004835f; cloud.points[18].y = 0.125000f; cloud.points[18].z = 0.993345f; 
  cloud.points[19].x = -0.017308f; cloud.points[19].y = 0.150000f; cloud.points[19].z = 0.994376f; 
  cloud.points[20].x =  0.017308f; cloud.points[20].y = 0.150000f; cloud.points[20].z = 0.994376f; 
  cloud.points[21].x = -0.025962f; cloud.points[21].y = 0.175000f; cloud.points[21].z = 0.991565f; 
  cloud.points[22].x =  0.025962f; cloud.points[22].y = 0.175000f; cloud.points[22].z = 0.991565f; 
  cloud.points[23].x = -0.009099f; cloud.points[23].y = 0.125000f; cloud.points[23].z = 1.000000f; 
  cloud.points[24].x =  0.009099f; cloud.points[24].y = 0.125000f; cloud.points[24].z = 1.000000f; 
  cloud.points[25].x = -0.018199f; cloud.points[25].y = 0.150000f; cloud.points[25].z = 1.000000f; 
  cloud.points[26].x =  0.018199f; cloud.points[26].y = 0.150000f; cloud.points[26].z = 1.000000f; 
  cloud.points[27].x = -0.027298f; cloud.points[27].y = 0.175000f; cloud.points[27].z = 1.000000f; 
  cloud.points[28].x =  0.027298f; cloud.points[28].y = 0.175000f; cloud.points[28].z = 1.000000f; 
  cloud.points[29].x = -0.036397f; cloud.points[29].y = 0.200000f; cloud.points[29].z = 1.000000f; 
  cloud.points[30].x =  0.036397f; cloud.points[30].y = 0.200000f; cloud.points[30].z = 1.000000f; 

  normals.points[0].normal[0] = -0.290381f; normals.points[0].normal[1] =  -0.342020f; normals.points[0].normal[2] =  -0.893701f; 
  normals.points[1].normal[0] =  0.000000f; normals.points[1].normal[1] =  -0.342020f; normals.points[1].normal[2] =  -0.939693f; 
  normals.points[2].normal[0] =  0.290381f; normals.points[2].normal[1] =  -0.342020f; normals.points[2].normal[2] =  -0.893701f; 
  normals.points[3].normal[0] = -0.552338f; normals.points[3].normal[1] =  -0.342020f; normals.points[3].normal[2] =  -0.760227f; 
  normals.points[4].normal[0] = -0.290381f; normals.points[4].normal[1] =  -0.342020f; normals.points[4].normal[2] =  -0.893701f; 
  normals.points[5].normal[0] =  0.145191f; normals.points[5].normal[1] =  -0.342020f; normals.points[5].normal[2] =  -0.916697f; 
  normals.points[6].normal[0] =  0.552337f; normals.points[6].normal[1] =  -0.342020f; normals.points[6].normal[2] =  -0.760227f; 
  normals.points[7].normal[0] = -0.656282f; normals.points[7].normal[1] =  -0.342020f; normals.points[7].normal[2] =  -0.656283f; 
  normals.points[8].normal[0] =  0.656282f; normals.points[8].normal[1] =  -0.342020f; normals.points[8].normal[2] =  -0.656283f; 
  normals.points[9].normal[0] = -0.656283f; normals.points[9].normal[1] =  -0.342020f; normals.points[9].normal[2] =  -0.656282f; 
  normals.points[10].normal[0] = -0.290381f; normals.points[10].normal[1] =  -0.342020f; normals.points[10].normal[2] =  -0.893701f; 
  normals.points[11].normal[0] =  0.145191f; normals.points[11].normal[1] =  -0.342020f; normals.points[11].normal[2] =  -0.916697f; 
  normals.points[12].normal[0] =  0.656282f; normals.points[12].normal[1] =  -0.342020f; normals.points[12].normal[2] =  -0.656282f; 
  normals.points[13].normal[0] = -0.760228f; normals.points[13].normal[1] =  -0.342020f; normals.points[13].normal[2] =  -0.552337f; 
  normals.points[14].normal[0] =  0.760228f; normals.points[14].normal[1] =  -0.342020f; normals.points[14].normal[2] =  -0.552337f; 
  normals.points[15].normal[0] = -0.893701f; normals.points[15].normal[1] =  -0.342020f; normals.points[15].normal[2] =  -0.290380f; 
  normals.points[16].normal[0] =  0.893701f; normals.points[16].normal[1] =  -0.342020f; normals.points[16].normal[2] =  -0.290380f; 
  normals.points[17].normal[0] = -0.624162f; normals.points[17].normal[1] =  -0.342020f; normals.points[17].normal[2] =  -0.624162f; 
  normals.points[18].normal[0] =  0.499329f; normals.points[18].normal[1] =  -0.342020f; normals.points[18].normal[2] =  -0.687268f; 
  normals.points[19].normal[0] = -0.893701f; normals.points[19].normal[1] =  -0.342020f; normals.points[19].normal[2] =  -0.290380f; 
  normals.points[20].normal[0] =  0.893701f; normals.points[20].normal[1] =  -0.342020f; normals.points[20].normal[2] =  -0.290380f; 
  normals.points[21].normal[0] = -0.893701f; normals.points[21].normal[1] =  -0.342020f; normals.points[21].normal[2] =  -0.290381f; 
  normals.points[22].normal[0] =  0.893701f; normals.points[22].normal[1] =  -0.342020f; normals.points[22].normal[2] =  -0.290381f; 
  normals.points[23].normal[0] = -0.939693f; normals.points[23].normal[1] =  -0.342020f; normals.points[23].normal[2] =  0.000000f; 
  normals.points[24].normal[0] =  0.939693f; normals.points[24].normal[1] =  -0.342020f; normals.points[24].normal[2] =  0.000000f; 
  normals.points[25].normal[0] = -0.939693f; normals.points[25].normal[1] =  -0.342020f; normals.points[25].normal[2] =  0.000000f; 
  normals.points[26].normal[0] =  0.939693f; normals.points[26].normal[1] =  -0.342020f; normals.points[26].normal[2] =  0.000000f; 
  normals.points[27].normal[0] = -0.939693f; normals.points[27].normal[1] =  -0.342020f; normals.points[27].normal[2] =  0.000000f; 
  normals.points[28].normal[0] =  0.939693f; normals.points[28].normal[1] =  -0.342020f; normals.points[28].normal[2] =  0.000000f; 
  normals.points[29].normal[0] = -0.939693f; normals.points[29].normal[1] =  -0.342020f; normals.points[29].normal[2] =  0.000000f; 
  normals.points[30].normal[0] =  0.939693f; normals.points[30].normal[1] =  -0.342020f; normals.points[30].normal[2] =  0.000000f; 


  // Create a shared cylinder model pointer directly
  SampleConsensusModelConePtr model (new SampleConsensusModelCone<PointXYZ, Normal> (cloud.makeShared ()));
  model->setInputNormals (normals.makeShared ());

  // Create the RANSAC object
  RandomSampleConsensus<PointXYZ> sac (model, 0.03);

  // Algorithm tests
  bool result = sac.computeModel ();
  ASSERT_EQ (result, true);

  std::vector<int> sample;
  sac.getModel (sample);
  EXPECT_EQ (int (sample.size ()), 3);

  std::vector<int> inliers;
  sac.getInliers (inliers);
  EXPECT_EQ (int (inliers.size ()), 31);

  Eigen::VectorXf coeff;
  sac.getModelCoefficients (coeff);
  EXPECT_EQ (int (coeff.size ()), 7);
  EXPECT_NEAR (coeff[0],  0, 1e-2);
  EXPECT_NEAR (coeff[1],  0.1,  1e-2);
  EXPECT_NEAR (coeff[6],  0.349066, 1e-2);

  Eigen::VectorXf coeff_refined;
  model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
  EXPECT_EQ (int (coeff_refined.size ()), 7);
  EXPECT_NEAR (coeff_refined[6], 0.349066 , 1e-2);
}
Beispiel #8
0
void Algorithm::playFromFile(string filename, int mode)
{
	PointCloudT::Ptr cloud(new PointCloudT);
	interface = new pcl::OpenNIGrabber(filename);
	f = boost::bind(&cloud_cb_, _1, cloud, &new_cloud_available_flag);
	interface->registerCallback(f);
	interface->start();

	// Wait for the first frame:

	while (!new_cloud_available_flag)
		boost::this_thread::sleep(boost::posix_time::milliseconds(1));
	new_cloud_available_flag = false;
	cloud_mutex.lock();    // for not overwriting the point cloud

	// Display pointcloud:
	pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
	viewer->addPointCloud<PointT>(cloud, rgb, "input_cloud");
	viewer->setCameraPosition(0, 0, -2, 0, -1, 0, 0);

	// Add point picking callback to viewer:
		
	//setFloor(viewer, svm_filename, voxel_size, rgb_intrinsics_matrix, min_height, max_height);
	
	
	

	// Main loop:
	if (mode == 1)
	{
		cloud_mutex.unlock();
		mainLoopPlain(viewer, cloud, new_cloud_available_flag);
	}

	if (mode == 2)
	{
		callback_args cb_args;
		PointCloudT::Ptr clicked_points_3d(new PointCloudT);
		cb_args.clicked_points_3d = clicked_points_3d;
		cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(viewer);
		viewer->registerPointPickingCallback(pp_callback, (void*)&cb_args);

		cout << "Kliknij w 3 punkty na podlodze trzymajac wcisniety SHIFT";
		viewer->addText("Kliknij w 3 punkty na podlodze trzymajac wcisniety SHIFT", 250, 300, 20, 1, 1, 1);
		viewer->addText("Nastepnie nacisnij klawisz Q", 250, 250, 20, 1, 1, 1);

		// Spin until 'Q' is pressed:
		viewer->spin();
		std::cout << "Gotowe." << std::endl;


		cloud_mutex.unlock();

		// Ground plane estimation:
		ground_coeffs.resize(4);
		std::vector<int> clicked_points_indices;
		for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++)
			clicked_points_indices.push_back(i);
		pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d);
		model_plane.computeModelCoefficients(clicked_points_indices, ground_coeffs);
		std::cout << "Ground plane: " << ground_coeffs(0) << " " << ground_coeffs(1) << " " << ground_coeffs(2) << " " << ground_coeffs(3) << std::endl;

		// Create classifier for people detection:  
		pcl::people::PersonClassifier<pcl::RGB> person_classifier;
		person_classifier.loadSVMFromFile(svm_filename);   // load trained SVM

		// People detection app initialization:
		// people detection object
		people_detector.setVoxelSize(voxel_size);                        // set the voxel size
		people_detector.setIntrinsics(rgb_intrinsics_matrix);            // set RGB camera intrinsic parameters
		people_detector.setClassifier(person_classifier);                // set person classifier
		// people_detector.setHeightLimits(min_height, max_height);         // set person height limits
		people_detector.setPersonClusterLimits(min_height, max_height, 0.1, 8.0); // set person height limits
		floor_tagged = true;
		// For timing:
		mainLoopAlgorithm(viewer, cloud, dist, new_cloud_available_flag);
	}
	if (mode == 3)
	{
		callback_args cb_args;
		PointCloudT::Ptr clicked_points_3d(new PointCloudT);
		cb_args.clicked_points_3d = clicked_points_3d;
		cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(viewer);
		viewer->registerPointPickingCallback(pp_callback, (void*)&cb_args);

		cout << "Kliknij w 3 punkty na podlodze trzymajac wcisniety SHIFT";
		viewer->addText("Kliknij w 3 punkty na podlodze trzymajac wcisniety SHIFT", 250, 300, 20, 1, 1, 1);
		viewer->addText("Nastepnie nacisnij klawisz Q", 250, 250, 20, 1, 1, 1);

		// Spin until 'Q' is pressed:
		viewer->spin();
		std::cout << "Gotowe." << std::endl;


		cloud_mutex.unlock();

		// Ground plane estimation:
		ground_coeffs.resize(4);
		std::vector<int> clicked_points_indices;
		for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++)
			clicked_points_indices.push_back(i);
		pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d);
		model_plane.computeModelCoefficients(clicked_points_indices, ground_coeffs);
		std::cout << "Ground plane: " << ground_coeffs(0) << " " << ground_coeffs(1) << " " << ground_coeffs(2) << " " << ground_coeffs(3) << std::endl;

		// Create classifier for people detection:  
		pcl::people::PersonClassifier<pcl::RGB> person_classifier;
		person_classifier.loadSVMFromFile(svm_filename);   // load trained SVM

		// People detection app initialization:
		// people detection object
		people_detector.setVoxelSize(voxel_size);                        // set the voxel size
		people_detector.setIntrinsics(rgb_intrinsics_matrix);            // set RGB camera intrinsic parameters
		people_detector.setClassifier(person_classifier);                // set person classifier
		// people_detector.setHeightLimits(min_height, max_height);         // set person height limits
		people_detector.setPersonClusterLimits(min_height, max_height, 0.1, 8.0); // set person height limits
		floor_tagged = true;
		// For timing:
		startRecording("asd", "asd", viewer, cloud, dist, new_cloud_available_flag);
	}
	//mainLoopPlain(viewer, cloud, new_cloud_available_flag);
	
}
Beispiel #9
0
int cPCA2::computePC(std::vector<float>& x,
                     size_t nrow,
                     size_t ncol,
                     bool is_center,
                     bool is_scale,
                     bool is_corr)
{
    _ncol     = ncol;
    _nrow     = nrow;
    _is_center = is_center;
    _is_scale  = is_scale;
    _is_corr   = is_corr;

    if (x.size() != _nrow*_ncol)     { return -1; }
    if ((1 == _ncol) || (1 == nrow)) { return -1; }

    // convert vector to Eigen 2-dimensional matrix
    _xXf.resize(_nrow, _ncol);

    for (size_t i = 0; i < _nrow; ++i) {
        for (size_t j = 0; j < _ncol; ++j) {
            _xXf(i, j) = x[j + i*_ncol];
        }
    }

    // mean and standard deviation for each column
    Eigen::VectorXf     mean_vector(_ncol),
                        sd_vector(_ncol);
    size_t              zero_sd_num = 0;
    float               denom = static_cast<float>((_nrow > 1) ? _nrow - 1 : 1);

    mean_vector = _xXf.colwise().mean();

    Eigen::VectorXf     curr_col;

    for (size_t i = 0; i < _ncol; ++i) {
        curr_col = Eigen::VectorXf::Constant(_nrow, mean_vector(i));    // mean(x) for column x
        curr_col = _xXf.col(i) - curr_col;                              // x - mean(x)
        curr_col = curr_col.array().square();                           // (x-mean(x))^2

        sd_vector(i) = std::sqrt((curr_col.sum())/denom);

        if (0 == sd_vector(i)) {
            zero_sd_num++;
        }
    }

    // if colums with sd == 0 are too many, don't continue calculation
    if (1 > _ncol-zero_sd_num) {
        return -1;
    }

    // delete columns with sd == 0
    Eigen::MatrixXf     tmp(_nrow, _ncol-zero_sd_num);
    Eigen::VectorXf     tmp_mean_vector(_ncol-zero_sd_num);

    size_t              curr_col_num = 0;

    for (size_t i = 0; i < _ncol; ++i) {
        if (0 != sd_vector(i)) {
            tmp.col(curr_col_num) = _xXf.col(i);
            tmp_mean_vector(curr_col_num) = mean_vector(i);
            curr_col_num++;
        }
        else {
            _eliminated_columns.push_back(i);
        }
    }

    _ncol      -= zero_sd_num;
    _xXf        = tmp;
    mean_vector = tmp_mean_vector;
    tmp.resize(0, 0);
    tmp_mean_vector.resize(0);

    // shift to zero
    if (true == _is_center) {
        for (size_t i = 0; i < _ncol; ++i) {
            _xXf.col(i) -= Eigen::VectorXf::Constant(_nrow, mean_vector(i));
        }
    }

    // scale to unit variance
    if ( (false == _is_corr) || (true == _is_scale)) {
        for (size_t i = 0; i < _ncol; ++i) {
            _xXf.col(i) /= std::sqrt(_xXf.col(i).array().square().sum()/denom);
        }
    }

#ifndef NDEBUG
    std::cout << "\nScaled matrix:\n";
    std::cout << _xXf << std::endl;
    std::cout << "\nMean before scaling:\n" << mean_vector.transpose();
    std::cout << "\nStandard deviation before scaling:\n" << sd_vector.transpose();
#endif

    // when _nrow < _ncol then svd will be used
    // if corr is true and _nrow > _ncol then correlation matrix will be used
    // (TODO): What about covariance?
    if ((_nrow < _ncol) || (false == _is_corr)) {
        _method = "svd";

        Eigen::JacobiSVD<Eigen::MatrixXf>   svd(_xXf, Eigen::ComputeThinV);

        Eigen::VectorXf     eigen_singular_values = svd.singularValues();
        Eigen::VectorXf     tmp_vec = eigen_singular_values.array().square();
        float               tmp_sum = tmp_vec.sum();
        size_t              lim = (_nrow < _ncol)? _nrow : _ncol;

        tmp_vec /= tmp_sum;

        // PC's standard deviation and
        // PC's proportion of variance
        _kaiser = 0;
        for (size_t i = 0; i < lim; ++i) {
            _sd.push_back(eigen_singular_values(i)/std::sqrt(denom));

            if (_sd[i] >= 1) {
                _kaiser = (unsigned int) i + 1;
            }

            _prop_of_var.push_back(tmp_vec(i));
        }

        tmp_vec.resize(0);

#ifndef NDEBUG
        std::cout << "\n\nStandard deviations for PCs:\n";
        copy(_sd.begin(), _sd.end(),std::ostream_iterator<float>(std::cout," "));
        std::cout << "\n\nKaiser criterion: PC #" << _kaiser << std::endl;
#endif

        // PC's cumulative proportion
        _thresh95 = 1;
        _cum_prop.push_back(_prop_of_var[0]);

        for (size_t i = 1; i < _prop_of_var.size(); ++i) {
            _cum_prop.push_back(_cum_prop[i-1]+_prop_of_var[i]);

            if (_cum_prop[i] < 0.95) {
                _thresh95 = (unsigned int) i + 1;
            }
        }

#ifndef NDEBUG
        std::cout << "\nCumulative proportion:\n";
        copy(_cum_prop.begin(), _cum_prop.end(),std::ostream_iterator<float>(std::cout," "));
        std::cout << "\n\nThresh95 criterion: PC #" << _thresh95 << std::endl;
#endif

        // scores
        Eigen::MatrixXf     eigen_scores = _xXf * svd.matrixV();

#ifndef NDEBUG
        std::cout << "\n\nRotated values (scores):\n" << eigen_scores;
#endif

        _scores.reserve(lim*lim);

        for (size_t i = 0; i < lim; ++i) {
            for (size_t j = 0; j < lim; ++j) {
                _scores.push_back(eigen_scores(i, j));
            }
        }

        eigen_scores.resize(0, 0);

#ifndef NDEBUG
        std::cout << "\n\nScores in vector:\n";
        copy(_scores.begin(), _scores.end(),std::ostream_iterator<float>(std::cout," "));
        std::cout << "\n";
#endif

    }
    else {    // COR OR COV MATRICES ARE HERE
        _method = "cor";

        // calculate covariance matrix
        Eigen::MatrixXf     eigen_cov; // = MatrixXf::Zero(_ncol, _ncol);
        Eigen::VectorXf     sds;

        // (TODO) should be weighted cov matrix, even if is_center == false
        eigen_cov = (1.0f /((float) _nrow/*-1*/)) * _xXf.transpose() * _xXf;
        sds = eigen_cov.diagonal().array().sqrt();
        Eigen::MatrixXf outer_sds = sds * sds.transpose();
        eigen_cov = eigen_cov.array() / outer_sds.array();
        outer_sds.resize(0, 0);

        // ?if data matrix is scaled, covariance matrix is equal to correlation matrix
        Eigen::EigenSolver<Eigen::MatrixXf>     edc(eigen_cov);
        Eigen::VectorXf                         eigen_eigenvalues = edc.eigenvalues().real();
        Eigen::MatrixXf                         eigen_eigenvectors = edc.eigenvectors().real();

#ifndef NDEBUG
        std::cout << eigen_cov << std::endl;
        std::cout << std::endl << eigen_eigenvalues.transpose() << std::endl;
        std::cout << std::endl << eigen_eigenvectors << std::endl;
#endif

        // the eigenvalues and eigenvectors are not sorted
        // so, we should sort them
        typedef std::pair<float,int>    eigen_pair;
        std::vector<eigen_pair>         ep;

        for (size_t i = 0 ; i < _ncol; ++i) {
            ep.push_back(std::make_pair(eigen_eigenvalues(i), i));
        }

        sort(ep.begin(), ep.end());     // ascending order by default

        // sort them all in descending order
        Eigen::MatrixXf     eigen_eigenvectors_sorted = Eigen::MatrixXf::Zero(eigen_eigenvectors.rows(), eigen_eigenvectors.cols());
        Eigen::VectorXf     eigen_eigenvalues_sorted  = Eigen::VectorXf::Zero(_ncol);
        int                 colnum = 0;

        for (int i = (int) ep.size()-1; i > -1; i--) {
            eigen_eigenvalues_sorted(colnum)         = ep[i].first;
            eigen_eigenvectors_sorted.col(colnum++) += eigen_eigenvectors.col(ep[i].second);
        }

#ifndef NDEBUG
        std::cout << std::endl << eigen_eigenvalues_sorted.transpose() << std::endl;
        std::cout << std::endl << eigen_eigenvectors_sorted << std::endl;
#endif

        // we don't need not sorted arrays anymore
        eigen_eigenvalues.resize(0);
        eigen_eigenvectors.resize(0, 0);

        _sd.clear();
        _prop_of_var.clear();
        _kaiser = 0;

        float       tmp_sum = eigen_eigenvalues_sorted.sum();

        for (size_t i = 0; i < _ncol; ++i) {
            _sd.push_back(std::sqrt(eigen_eigenvalues_sorted(i)));

            if (_sd[i] >= 1) {
                _kaiser = (unsigned int) i + 1;
            }

            _prop_of_var.push_back(eigen_eigenvalues_sorted(i)/tmp_sum);
        }

#ifndef NDEBUG
        std::cout << "\nStandard deviations for PCs:\n";
        copy(_sd.begin(), _sd.end(), std::ostream_iterator<float>(std::cout," "));
        std::cout << "\nProportion of variance:\n";
        copy(_prop_of_var.begin(), _prop_of_var.end(), std::ostream_iterator<float>(std::cout," "));
        std::cout << "\nKaiser criterion: PC #" << _kaiser << std::endl;
#endif

        // PC's cumulative proportion
        _cum_prop.clear();
        _thresh95 = 1;
        _cum_prop.push_back(_prop_of_var[0]);

        for (size_t i = 1; i < _prop_of_var.size(); ++i) {
            _cum_prop.push_back(_cum_prop[i-1]+_prop_of_var[i]);

            if (_cum_prop[i] < 0.95) {
                _thresh95 = (unsigned int) i + 1;
            }
        }

#ifndef NDEBUG
        std::cout << "\n\nCumulative proportions:\n";
        copy(_cum_prop.begin(), _cum_prop.end(), std::ostream_iterator<float>(std::cout," "));
        std::cout << "\n\n95% threshold: PC #" << _thresh95 << std::endl;
#endif

        // scores for PCA with correlation matrix
        // scale before calculating new values

        for (size_t i = 0; i < _ncol; ++i) {
            _xXf.col(i) /= sds(i);
        }

        sds.resize(0);
        Eigen::MatrixXf     eigen_scores = _xXf * eigen_eigenvectors_sorted;

#ifndef NDEBUG
        std::cout << "\n\nRotated values (scores):\n" << eigen_scores;
#endif

        _scores.clear();
        _scores.reserve(_ncol*_nrow);

        for (size_t i = 0; i < _nrow; ++i) {
            for (size_t j = 0; j < _ncol; ++j) {
                _scores.push_back(eigen_scores(i, j));
            }
        }

        eigen_scores.resize(0, 0);

#ifndef NDEBUG
        std::cout << "\n\nScores in vector:\n";
        copy(_scores.begin(), _scores.end(), std::ostream_iterator<float>(std::cout," "));
        std::cout << "\n";
#endif
    }

    return 0;
}
Beispiel #10
0
int main(int argc, char *argv[])
{
	if ( argc != 3 )
    {
        std::cout<<"usage: "<< argv[0] <<" <input file> <output file>\n";
        return 1;
    }
	
	std::ifstream infile(argv[1]);
	std::ofstream outfile(argv[2]);
	
	float poissonRatio, youngModulus;
	infile >> poissonRatio >> youngModulus;

	Eigen::Matrix3f D;
	D <<
		1.0f,        	poissonRatio,	0.0f,
		poissonRatio,	1.0,         	0.0f,
		0.0f,        	0.0f,        	(1.0f - poissonRatio) / 2.0f;

	D *= youngModulus / (1.0f - pow(poissonRatio, 2.0f));

	infile >> nodesCount;
	nodesX.resize(nodesCount);
	nodesY.resize(nodesCount);

	for (int i = 0; i < nodesCount; ++i)
	{
		infile >> nodesX[i] >> nodesY[i];
	}

	int elementCount;
	infile >> elementCount;

	for (int i = 0; i < elementCount; ++i)
	{
		Element element;
		infile >> element.nodesIds[0] >> element.nodesIds[1] >> element.nodesIds[2];
		elements.push_back(element);
	}

	int constraintCount;
	infile >> constraintCount;

	for (int i = 0; i < constraintCount; ++i)
	{
		Constraint constraint;
		int type;
		infile >> constraint.node >> type;
		constraint.type = static_cast<Constraint::Type>(type);
		constraints.push_back(constraint);
	}

	loads.resize(2 * nodesCount);
	loads.setZero();

	int loadsCount;
	infile >> loadsCount;

	for (int i = 0; i < loadsCount; ++i)
	{
		int node;
		float x, y;
		infile >> node >> x >> y;
		loads[2 * node + 0] = x;
		loads[2 * node + 1] = y;
	}
	
	std::vector<Eigen::Triplet<float> > triplets;
	for (std::vector<Element>::iterator it = elements.begin(); it != elements.end(); ++it)
	{
		it->CalculateStiffnessMatrix(D, triplets);
	}

	Eigen::SparseMatrix<float> globalK(2 * nodesCount, 2 * nodesCount);
	globalK.setFromTriplets(triplets.begin(), triplets.end());

	ApplyConstraints(globalK, constraints);

	Eigen::SimplicialLDLT<Eigen::SparseMatrix<float> > solver(globalK);

	Eigen::VectorXf displacements = solver.solve(loads);

	outfile << displacements << std::endl;

	for (std::vector<Element>::iterator it = elements.begin(); it != elements.end(); ++it)
	{
		Eigen::Matrix<float, 6, 1> delta;
		delta << displacements.segment<2>(2 * it->nodesIds[0]),
		         displacements.segment<2>(2 * it->nodesIds[1]),
		         displacements.segment<2>(2 * it->nodesIds[2]);

		Eigen::Vector3f sigma = D * it->B * delta;
		float sigma_mises = sqrt(sigma[0] * sigma[0] - sigma[0] * sigma[1] + sigma[1] * sigma[1] + 3.0f * sigma[2] * sigma[2]);

		outfile << sigma_mises << std::endl;
	}
	return 0;
}
Beispiel #11
0
template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::projectPoints (
      const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 4)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelPlane::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
    return;
  }

  projected_points.header = input_->header;
  projected_points.is_dense = input_->is_dense;

  Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);

  // normalize the vector perpendicular to the plane...
  mc.normalize ();
  // ... and store the resulting normal as a local copy of the model coefficients
  Eigen::Vector4f tmp_mc = model_coefficients;
  tmp_mc[0] = mc[0];
  tmp_mc[1] = mc[1];
  tmp_mc[2] = mc[2];

  // Copy all the data fields from the input cloud to the projected one?
  if (copy_data_fields)
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (input_->points.size ());
    projected_points.width    = input_->width;
    projected_points.height   = input_->height;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < input_->points.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the plane
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      // Calculate the distance from the point to the plane
      Eigen::Vector4f p (input_->points[inliers[i]].x,
                         input_->points[inliers[i]].y,
                         input_->points[inliers[i]].z,
                         1);
      // use normalized coefficients to calculate the scalar projection
      float distance_to_plane = tmp_mc.dot (p);

      pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap ();
      pp = p - mc * distance_to_plane;        // mc[3] = 0, therefore the 3rd coordinate is safe
    }
  }
  else
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (inliers.size ());
    projected_points.width    = static_cast<uint32_t> (inliers.size ());
    projected_points.height   = 1;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < inliers.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the plane
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      // Calculate the distance from the point to the plane
      Eigen::Vector4f p (input_->points[inliers[i]].x,
                         input_->points[inliers[i]].y,
                         input_->points[inliers[i]].z,
                         1);
      // use normalized coefficients to calculate the scalar projection
      float distance_to_plane = tmp_mc.dot (p);

      pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap ();
      pp = p - mc * distance_to_plane;        // mc[3] = 0, therefore the 3rd coordinate is safe
    }
  }
}
Beispiel #12
0
template <typename PointT> bool
pcl::SampleConsensusModelSphere<PointT>::computeModelCoefficients (
      const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
{
  // Need 4 samples
  if (samples.size () != 4)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelSphere::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ());
    return (false);
  }

  Eigen::Matrix4f temp;
  for (int i = 0; i < 4; i++)
  {
    temp (i, 0) = input_->points[samples[i]].x;
    temp (i, 1) = input_->points[samples[i]].y;
    temp (i, 2) = input_->points[samples[i]].z;
    temp (i, 3) = 1;
  }
  float m11 = temp.determinant ();
  if (m11 == 0)
    return (false);             // the points don't define a sphere!

  for (int i = 0; i < 4; ++i)
    temp (i, 0) = (input_->points[samples[i]].x) * (input_->points[samples[i]].x) +
                  (input_->points[samples[i]].y) * (input_->points[samples[i]].y) +
                  (input_->points[samples[i]].z) * (input_->points[samples[i]].z);
  float m12 = temp.determinant ();

  for (int i = 0; i < 4; ++i)
  {
    temp (i, 1) = temp (i, 0);
    temp (i, 0) = input_->points[samples[i]].x;
  }
  float m13 = temp.determinant ();

  for (int i = 0; i < 4; ++i)
  {
    temp (i, 2) = temp (i, 1);
    temp (i, 1) = input_->points[samples[i]].y;
  }
  float m14 = temp.determinant ();

  for (int i = 0; i < 4; ++i)
  {
    temp (i, 0) = temp (i, 2);
    temp (i, 1) = input_->points[samples[i]].x;
    temp (i, 2) = input_->points[samples[i]].y;
    temp (i, 3) = input_->points[samples[i]].z;
  }
  float m15 = temp.determinant ();

  // Center (x , y, z)
  model_coefficients.resize (4);
  model_coefficients[0] = 0.5f * m12 / m11;
  model_coefficients[1] = 0.5f * m13 / m11;
  model_coefficients[2] = 0.5f * m14 / m11;
  // Radius
  model_coefficients[3] = sqrtf (
                                 model_coefficients[0] * model_coefficients[0] +
                                 model_coefficients[1] * model_coefficients[1] +
                                 model_coefficients[2] * model_coefficients[2] - m15 / m11);

  return (true);
}
Beispiel #13
0
template <typename PointT> void
pcl::SampleConsensusModelCircle2D<PointT>::projectPoints (
      const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients,
      PointCloud &projected_points, bool copy_data_fields)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 3)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
    return;
  }

  projected_points.header   = input_->header;
  projected_points.is_dense = input_->is_dense;

  // Copy all the data fields from the input cloud to the projected one?
  if (copy_data_fields)
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (input_->points.size ());
    projected_points.width    = input_->width;
    projected_points.height   = input_->height;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < projected_points.points.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the plane
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      float dx = input_->points[inliers[i]].x - model_coefficients[0];
      float dy = input_->points[inliers[i]].y - model_coefficients[1];
      float a = sqrtf ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) );

      projected_points.points[inliers[i]].x = a * dx + model_coefficients[0];
      projected_points.points[inliers[i]].y = a * dy + model_coefficients[1];
    }
  }
  else
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (inliers.size ());
    projected_points.width    = static_cast<uint32_t> (inliers.size ());
    projected_points.height   = 1;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < inliers.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the plane
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      float dx = input_->points[inliers[i]].x - model_coefficients[0];
      float dy = input_->points[inliers[i]].y - model_coefficients[1];
      float a = sqrtf ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) );

      projected_points.points[i].x = a * dx + model_coefficients[0];
      projected_points.points[i].y = a * dy + model_coefficients[1];
    }
  }
}
Beispiel #14
0
template<typename PointT> inline void 
pcl::PCA<PointT>::update (const PointT& input_point, FLAG flag) 
{
  if (!compute_done_)
    initCompute ();
  if (!compute_done_)
    PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::update] PCA initCompute failed");

  Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
  const size_t n = eigenvectors_.cols ();// number of eigen vectors
  Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) / float(n + 1);
  Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
  Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
  Eigen::VectorXf h = y - input;
  if (h.norm() > 0) 
    h.normalize ();
  else
    h.setZero ();
  float gamma = h.dot(input - mean_.head<3>());
  Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
  D.block(0,0,n,n) = a * a.transpose();
  D /=  float(n)/float((n+1) * (n+1));
  for(std::size_t i=0; i < a.size(); i++) {
    D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
    D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i);
    D(i,D.cols()-1) = D(D.rows()-1,i);
    D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma;
  }

  Eigen::MatrixXf R(D.rows(), D.cols());
  Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D, false);
  Eigen::VectorXf alphap = D_evd.eigenvalues().real();
  eigenvalues_.resize(eigenvalues_.size() +1);
  for(std::size_t i=0;i<eigenvalues_.size();i++) {
    eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
    R.col(i) = D.col(D.cols()-i-1);
  }
  Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
  Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
  Up.rightCols<1>() = h;
  eigenvectors_ = Up*R;
  if (!basis_only_) {
    Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
    coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
    for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
      coefficients_(coefficients_.rows()-1,i) = 0;
      coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
    }
    a.resize(a.size()+1);
    a(a.size()-1) = 0;
    coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
  }
  mean_.head<3>() = meanp;
  switch (flag) 
  {
    case increase:
      if (eigenvectors_.rows() >= eigenvectors_.cols())
        break;
    case preserve:
      if (!basis_only_)
        coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
      eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
      eigenvalues_.resize(eigenvalues_.size()-1);
      break;
    default:
      PCL_ERROR("[pcl::PCA] unknown flag\n");
  }
}
Beispiel #15
0
  int Fit(Vector& res_G,  // residual under NULL -- may change when permuting
          Vector& v_G,    // variance under NULL -- may change when permuting
          Matrix& X_G,    // covariance
          Matrix& G_G,    // genotype
          Vector& w_G)    // weight
  {
    this->nPeople = X_G.rows;
    this->nMarker = G_G.cols;
    this->nCovariate = X_G.cols;

    // calculation w_sqrt
    G_to_Eigen(w_G, &this->w_sqrt);
    w_sqrt = w_sqrt.cwiseSqrt();

    // calculate K = G * W * G'
    Eigen::MatrixXf G;
    G_to_Eigen(G_G, &G);
    this->K_sqrt.noalias() = w_sqrt.asDiagonal() * G.transpose();

    // calculate Q = ||res * K||
    Eigen::VectorXf res;
    G_to_Eigen(res_G, &res);
    this->Q = (this->K_sqrt * res).squaredNorm();

    // calculate P0 = V - V X (X' V X)^(-1) X' V
    Eigen::VectorXf v;
    G_to_Eigen(v_G, &v);
    if (this->nCovariate == 1) {
      P0 = -v * v.transpose() / v.sum();
      // printf("dim(P0) = %d, %d\n", P0.rows(), P0.cols());
      // printf("dim(v) = %d\n", v.size());
      P0.diagonal() += v;
      // printf("dim(v) = %d\n", v.size());
    } else {
      Eigen::MatrixXf X;
      G_to_Eigen(X_G, &X);
      Eigen::MatrixXf XtV;  // X^t V
      XtV.noalias() = X.transpose() * v.asDiagonal();
      P0 = -XtV.transpose() * ((XtV * X).inverse()) * XtV;
      P0.diagonal() += v;
    }
    // dump();
    // Eigen::MatrixXf tmp = K_sqrt * P0 * K_sqrt.transpose();
    // dumpToFile(tmp, "out.tmp");
    // eigen decomposition
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> es;
    es.compute(K_sqrt * P0 * K_sqrt.transpose());

#ifdef DEBUG
    std::ofstream k("K");
    k << K_sqrt;
    k.close();
#endif
    // std::ofstream p("P0");
    // p << P0;
    // p.close();

    this->mixChiSq.reset();
    int r_ub = std::min(nPeople, nMarker);
    int r = 0;  // es.eigenvalues().size();
    int eigen_len = es.eigenvalues().size();
    for (int i = eigen_len - 1; i >= 0; i--) {
      if (es.eigenvalues()[i] > ZBOUND && r < r_ub) {
        this->mixChiSq.addLambda(es.eigenvalues()[i]);
        r++;
      } else {
        break;
      }
    }
    // calculate p-value
    this->pValue = this->mixChiSq.getPvalue(this->Q);
    if (this->pValue <= 0.0 || this->pValue == 1.0) {
      this->pValue = this->mixChiSq.getLiuPvalue(this->Q);
    }
    return 0;
  };
Beispiel #16
0
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::PFHRGBEstimation<PointInT, PointNT, PointOutT>::computePointPFHRGBSignature (
    const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
    const std::vector<int> &indices, int nr_split, Eigen::VectorXf &pfhrgb_histogram)
{
  int h_index, h_p;

  // Clear the resultant point histogram
  pfhrgb_histogram.setZero ();

  // Factorization constant
  float hist_incr = 100.0f / static_cast<float> (indices.size () * indices.size () - 1);

  // Iterate over all the points in the neighborhood
  for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
  {
    for (size_t j_idx = 0; j_idx < indices.size (); ++j_idx)
    {
      // Avoid unnecessary returns
      if (i_idx == j_idx)
        continue;

      // Compute the pair NNi to NNj
      if (!computeRGBPairFeatures (cloud, normals, indices[i_idx], indices[j_idx],
                                   pfhrgb_tuple_[0], pfhrgb_tuple_[1], pfhrgb_tuple_[2], pfhrgb_tuple_[3],
                                   pfhrgb_tuple_[4], pfhrgb_tuple_[5], pfhrgb_tuple_[6]))
        continue;

      // Normalize the f1, f2, f3, f5, f6, f7 features and push them in the histogram
      f_index_[0] = static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[0] + M_PI) * d_pi_)));
      if (f_index_[0] < 0)         f_index_[0] = 0;
      if (f_index_[0] >= nr_split) f_index_[0] = nr_split - 1;

      f_index_[1] = static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[1] + 1.0) * 0.5)));
      if (f_index_[1] < 0)         f_index_[1] = 0;
      if (f_index_[1] >= nr_split) f_index_[1] = nr_split - 1;

      f_index_[2] = static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[2] + 1.0) * 0.5)));
      if (f_index_[2] < 0)         f_index_[2] = 0;
      if (f_index_[2] >= nr_split) f_index_[2] = nr_split - 1;

      // color ratios are in [-1, 1]
      f_index_[4] = static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[4] + 1.0) * 0.5)));
      if (f_index_[4] < 0)         f_index_[4] = 0;
      if (f_index_[4] >= nr_split) f_index_[4] = nr_split - 1;

      f_index_[5] = static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[5] + 1.0) * 0.5)));
      if (f_index_[5] < 0)         f_index_[5] = 0;
      if (f_index_[5] >= nr_split) f_index_[5] = nr_split - 1;

      f_index_[6] = static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[6] + 1.0) * 0.5)));
      if (f_index_[6] < 0)         f_index_[6] = 0;
      if (f_index_[6] >= nr_split) f_index_[6] = nr_split - 1;


      // Copy into the histogram
      h_index = 0;
      h_p     = 1;
      for (int d = 0; d < 3; ++d)
      {
        h_index += h_p * f_index_[d];
        h_p     *= nr_split;
      }
      pfhrgb_histogram[h_index] += hist_incr;

      // and the colors
      h_index = 125;
      h_p     = 1;
      for (int d = 4; d < 7; ++d)
      {
        h_index += h_p * f_index_[d];
        h_p     *= nr_split;
      }
      pfhrgb_histogram[h_index] += hist_incr;
    }
  }
}
Beispiel #17
0
template <typename PointT> void
pcl::SampleConsensusModelCircle3D<PointT>::projectPoints (
      const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients,
      PointCloud &projected_points, bool copy_data_fields)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 7)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
    return;
  }

  projected_points.header   = input_->header;
  projected_points.is_dense = input_->is_dense;

  // Copy all the data fields from the input cloud to the projected one?
  if (copy_data_fields)
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (input_->points.size ());
    projected_points.width    = input_->width;
    projected_points.height   = input_->height;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < projected_points.points.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the plane
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      // what i have:
      // P : Sample Point
      Eigen::Vector3d P (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z);
      // C : Circle Center
      Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
      // N : Circle (Plane) Normal
      Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
      // r : Radius
      double r = model_coefficients[3];

      Eigen::Vector3d helper_vectorPC = P - C;
      // 1.1. get line parameter
      //float lambda = (helper_vectorPC.dot(N)) / N.squaredNorm() ;
      double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
      // Projected Point on plane
      Eigen::Vector3d P_proj = P + lambda * N;
      Eigen::Vector3d helper_vectorP_projC = P_proj - C;

      // K : Point on Circle
      Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();

      projected_points.points[i].x = static_cast<float> (K[0]);
      projected_points.points[i].y = static_cast<float> (K[1]);
      projected_points.points[i].z = static_cast<float> (K[2]);
    }
  }
  else
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (inliers.size ());
    projected_points.width    = uint32_t (inliers.size ());
    projected_points.height   = 1;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < inliers.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the plane
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      // what i have:
      // P : Sample Point
      Eigen::Vector3d P (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z);
      // C : Circle Center
      Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
      // N : Circle (Plane) Normal
      Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
      // r : Radius
      double r = model_coefficients[3];

      Eigen::Vector3d helper_vectorPC = P - C;
      // 1.1. get line parameter
      double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
      // Projected Point on plane
      Eigen::Vector3d P_proj = P + lambda * N;
      Eigen::Vector3d helper_vectorP_projC = P_proj - C;

      // K : Point on Circle
      Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();

      projected_points.points[i].x = static_cast<float> (K[0]);
      projected_points.points[i].y = static_cast<float> (K[1]);
      projected_points.points[i].z = static_cast<float> (K[2]);
    }
  }
}
Beispiel #18
0
bool key_down(igl::viewer::Viewer& viewer, unsigned char key, int modifier)
{

  if (key == '1')
  {
    display_mode = 1;
    update_display(viewer);
  }

  if (key == '2')
  {
    display_mode = 2;
    update_display(viewer);
  }

  if (key == '3')
  {
    display_mode = 3;
    update_display(viewer);
  }

  if (key == '4')
  {
    display_mode = 4;
    update_display(viewer);
  }

  if (key == '5')
  {
    display_mode = 5;
    update_display(viewer);
  }

  if (key == '6')
  {
    display_mode = 6;
    update_display(viewer);
  }

  if (key == '7')
  {
    display_mode = 7;
    update_display(viewer);
  }

  if (key == '8')
  {
    display_mode = 8;
    update_display(viewer);
  }

  if (key == '9')
  {
    display_mode = 9;
    update_display(viewer);
  }

  if (key == '0')
  {
    display_mode = 0;
    update_display(viewer);
  }

  if (key == 'A')
  {
    //do a batch of iterations
    printf("--Improving Curl--\n");
    for (int bi = 0; bi<5; ++bi)
    {
      printf("\n\n **** Batch %d ****\n", iter);
      igl::integrable_polyvector_fields_solve(ipfdata, params, two_pv, iter ==0);
      iter++;
      params.wSmooth *= params.redFactor_wsmooth;
    }
    // Post process current field
    // Compute curl_minimizing matchings and curl
    printf("--Matchings and curl--\n");
    Eigen::MatrixXi match_ab, match_ba;  // matchings across interior edges
    double avgCurl = igl::polyvector_field_matchings(two_pv, V, F, true, match_ab, match_ba, curl);
    double maxCurl = curl.maxCoeff();
    printf("curl -- max: %.5g, avg: %.5g\n", maxCurl,  avgCurl);
    // Compute singularities
    printf("--Singularities--\n");
    igl::polyvector_field_singularities_from_matchings(V, F, match_ab, match_ba, singularities);
    printf("#singularities: %ld\n", singularities.rows());
    // Get mesh cuts based on singularities
    printf("--Cuts--\n");
    igl::polyvector_field_cut_mesh_with_singularities(V, F, singularities, cuts);
    // Comb field
    printf("--Combing--\n");
    Eigen::MatrixXd combed;
    igl::polyvector_field_comb_from_matchings_and_cuts(V, F, two_pv, match_ab, match_ba, cuts, combed);
    // Reconstruct integrable vector fields from combed field
    printf("--Cut mesh--\n");
    igl::cut_mesh(V, F, cuts, Vcut, Fcut);
    printf("--Poisson--\n");
    double avgPoisson = igl::polyvector_field_poisson_reconstruction(Vcut, Fcut, combed, scalars, two_pv_poisson, poisson_error);
    double maxPoisson = poisson_error.maxCoeff();
    printf("poisson error -- max: %.5g, avg: %.5g\n", maxPoisson, avgPoisson);

    update_display(viewer);
  }

  return false;
}
Beispiel #19
0
TEST (RANSAC, SampleConsensusModelNormalSphere)
{
  srand (0);

  // Use a custom point cloud for these tests until we need something better
  PointCloud<PointXYZ> cloud;
  PointCloud<Normal> normals;
  cloud.points.resize (27); normals.points.resize (27);
  cloud.points[0].x = -0.014695f; cloud.points[0].y =  0.009549f; cloud.points[0].z = 0.954775f; 
  cloud.points[1].x =  0.014695f; cloud.points[1].y =  0.009549f; cloud.points[1].z = 0.954775f; 
  cloud.points[2].x = -0.014695f; cloud.points[2].y =  0.040451f; cloud.points[2].z = 0.954775f; 
  cloud.points[3].x =  0.014695f; cloud.points[3].y =  0.040451f; cloud.points[3].z = 0.954775f; 
  cloud.points[4].x = -0.009082f; cloud.points[4].y = -0.015451f; cloud.points[4].z = 0.972049f; 
  cloud.points[5].x =  0.009082f; cloud.points[5].y = -0.015451f; cloud.points[5].z = 0.972049f; 
  cloud.points[6].x = -0.038471f; cloud.points[6].y =  0.009549f; cloud.points[6].z = 0.972049f; 
  cloud.points[7].x =  0.038471f; cloud.points[7].y =  0.009549f; cloud.points[7].z = 0.972049f; 
  cloud.points[8].x = -0.038471f; cloud.points[8].y =  0.040451f; cloud.points[8].z = 0.972049f; 
  cloud.points[9].x =  0.038471f; cloud.points[9].y =  0.040451f; cloud.points[9].z = 0.972049f; 
  cloud.points[10].x = -0.009082f; cloud.points[10].y =  0.065451f; cloud.points[10].z = 0.972049f; 
  cloud.points[11].x =  0.009082f; cloud.points[11].y =  0.065451f; cloud.points[11].z = 0.972049f; 
  cloud.points[12].x = -0.023776f; cloud.points[12].y = -0.015451f; cloud.points[12].z = 0.982725f; 
  cloud.points[13].x =  0.023776f; cloud.points[13].y = -0.015451f; cloud.points[13].z = 0.982725f; 
  cloud.points[14].x = -0.023776f; cloud.points[14].y =  0.065451f; cloud.points[14].z = 0.982725f; 
  cloud.points[15].x =  0.023776f; cloud.points[15].y =  0.065451f; cloud.points[15].z = 0.982725f; 
  cloud.points[16].x = -0.000000f; cloud.points[16].y = -0.025000f; cloud.points[16].z = 1.000000f; 
  cloud.points[17].x =  0.000000f; cloud.points[17].y = -0.025000f; cloud.points[17].z = 1.000000f; 
  cloud.points[18].x = -0.029389f; cloud.points[18].y = -0.015451f; cloud.points[18].z = 1.000000f; 
  cloud.points[19].x =  0.029389f; cloud.points[19].y = -0.015451f; cloud.points[19].z = 1.000000f; 
  cloud.points[20].x = -0.047553f; cloud.points[20].y =  0.009549f; cloud.points[20].z = 1.000000f; 
  cloud.points[21].x =  0.047553f; cloud.points[21].y =  0.009549f; cloud.points[21].z = 1.000000f; 
  cloud.points[22].x = -0.047553f; cloud.points[22].y =  0.040451f; cloud.points[22].z = 1.000000f; 
  cloud.points[23].x =  0.047553f; cloud.points[23].y =  0.040451f; cloud.points[23].z = 1.000000f; 
  cloud.points[24].x = -0.029389f; cloud.points[24].y =  0.065451f; cloud.points[24].z = 1.000000f; 
  cloud.points[25].x =  0.029389f; cloud.points[25].y =  0.065451f; cloud.points[25].z = 1.000000f; 
  cloud.points[26].x =  0.000000f; cloud.points[26].y =  0.075000f; cloud.points[26].z = 1.000000f; 
  
  normals.points[0].normal[0] = -0.293893f; normals.points[0].normal[1] =  -0.309017f; normals.points[0].normal[2] =  -0.904509f; 
  normals.points[1].normal[0] =  0.293893f; normals.points[1].normal[1] =  -0.309017f; normals.points[1].normal[2] =  -0.904508f; 
  normals.points[2].normal[0] = -0.293893f; normals.points[2].normal[1] =   0.309017f; normals.points[2].normal[2] =  -0.904509f; 
  normals.points[3].normal[0] =  0.293893f; normals.points[3].normal[1] =   0.309017f; normals.points[3].normal[2] =  -0.904508f; 
  normals.points[4].normal[0] = -0.181636f; normals.points[4].normal[1] =  -0.809017f; normals.points[4].normal[2] =  -0.559017f; 
  normals.points[5].normal[0] =  0.181636f; normals.points[5].normal[1] =  -0.809017f; normals.points[5].normal[2] =  -0.559017f; 
  normals.points[6].normal[0] = -0.769421f; normals.points[6].normal[1] =  -0.309017f; normals.points[6].normal[2] =  -0.559017f; 
  normals.points[7].normal[0] =  0.769421f; normals.points[7].normal[1] =  -0.309017f; normals.points[7].normal[2] =  -0.559017f; 
  normals.points[8].normal[0] = -0.769421f; normals.points[8].normal[1] =   0.309017f; normals.points[8].normal[2] =  -0.559017f; 
  normals.points[9].normal[0] =  0.769421f; normals.points[9].normal[1] =   0.309017f; normals.points[9].normal[2] =  -0.559017f; 
  normals.points[10].normal[0] = -0.181636f; normals.points[10].normal[1] =  0.809017f; normals.points[10].normal[2] =  -0.559017f; 
  normals.points[11].normal[0] =  0.181636f; normals.points[11].normal[1] =  0.809017f; normals.points[11].normal[2] =  -0.559017f; 
  normals.points[12].normal[0] = -0.475528f; normals.points[12].normal[1] = -0.809017f; normals.points[12].normal[2] =  -0.345491f; 
  normals.points[13].normal[0] =  0.475528f; normals.points[13].normal[1] = -0.809017f; normals.points[13].normal[2] =  -0.345491f; 
  normals.points[14].normal[0] = -0.475528f; normals.points[14].normal[1] =  0.809017f; normals.points[14].normal[2] =  -0.345491f; 
  normals.points[15].normal[0] =  0.475528f; normals.points[15].normal[1] =  0.809017f; normals.points[15].normal[2] =  -0.345491f; 
  normals.points[16].normal[0] = -0.000000f; normals.points[16].normal[1] = -1.000000f; normals.points[16].normal[2] =  0.000000f; 
  normals.points[17].normal[0] =  0.000000f; normals.points[17].normal[1] = -1.000000f; normals.points[17].normal[2] =  0.000000f; 
  normals.points[18].normal[0] = -0.587785f; normals.points[18].normal[1] = -0.809017f; normals.points[18].normal[2] =  0.000000f; 
  normals.points[19].normal[0] =  0.587785f; normals.points[19].normal[1] = -0.809017f; normals.points[19].normal[2] =  0.000000f; 
  normals.points[20].normal[0] = -0.951057f; normals.points[20].normal[1] = -0.309017f; normals.points[20].normal[2] =  0.000000f; 
  normals.points[21].normal[0] =  0.951057f; normals.points[21].normal[1] = -0.309017f; normals.points[21].normal[2] =  0.000000f; 
  normals.points[22].normal[0] = -0.951057f; normals.points[22].normal[1] =  0.309017f; normals.points[22].normal[2] =  0.000000f; 
  normals.points[23].normal[0] =  0.951057f; normals.points[23].normal[1] =  0.309017f; normals.points[23].normal[2] =  0.000000f; 
  normals.points[24].normal[0] = -0.587785f; normals.points[24].normal[1] =  0.809017f; normals.points[24].normal[2] =  0.000000f; 
  normals.points[25].normal[0] =  0.587785f; normals.points[25].normal[1] =  0.809017f; normals.points[25].normal[2] =  0.000000f; 
  normals.points[26].normal[0] =  0.000000f; normals.points[26].normal[1] =  1.000000f; normals.points[26].normal[2] =  0.000000f; 
  
  // Create a shared sphere model pointer directly
  SampleConsensusModelNormalSpherePtr model (new SampleConsensusModelNormalSphere<PointXYZ, Normal> (cloud.makeShared ()));
  model->setInputNormals(normals.makeShared ());

  // Create the RANSAC object
  RandomSampleConsensus<PointXYZ> sac (model, 0.03);

  // Algorithm tests
  bool result = sac.computeModel ();
  ASSERT_EQ (result, true); 

  std::vector<int> sample;
  sac.getModel (sample);
  EXPECT_EQ (int (sample.size ()), 4);

  std::vector<int> inliers;
  sac.getInliers (inliers);
  EXPECT_EQ (int (inliers.size ()), 27);

  Eigen::VectorXf coeff;
  sac.getModelCoefficients (coeff);
  EXPECT_EQ (int (coeff.size ()), 4);
  EXPECT_NEAR (coeff[0], 0.0,   1e-2);
  EXPECT_NEAR (coeff[1], 0.025, 1e-2);
  EXPECT_NEAR (coeff[2], 1.0,   1e-2);
  EXPECT_NEAR (coeff[3], 0.05,  1e-2);
  Eigen::VectorXf coeff_refined;
  model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
  EXPECT_EQ (int (coeff_refined.size ()), 4);
  EXPECT_NEAR (coeff_refined[0], 0.0,   1e-2);
  EXPECT_NEAR (coeff_refined[1], 0.025, 1e-2);
  EXPECT_NEAR (coeff_refined[2], 1.0,   1e-2);
  EXPECT_NEAR (coeff_refined[3], 0.05,  1e-2);	 
}
Beispiel #20
0
int main(int argc, char *argv[])
{

  // Load a mesh
  igl::readOBJ(TUTORIAL_SHARED_PATH "/inspired_mesh.obj", V, F);

  printf("--Initialization--\n");
  V_border = igl::is_border_vertex(V,F);
  igl::adjacency_list(F, VV);
  igl::vertex_triangle_adjacency(V,F,VF,VFi);
  igl::triangle_triangle_adjacency(F,TT,TTi);
  igl::edge_topology(V,F,E,F2E,E2F);

  // Generate "subdivided" mesh for visualization of curl terms
  igl::false_barycentric_subdivision(V, F, Vbs, Fbs);

  // Compute scale for visualizing fields
  global_scale =  .2*igl::avg_edge_length(V, F);

  //Compute scale for visualizing texture
  uv_scale = 0.6/igl::avg_edge_length(V, F);

  // Compute face barycenters
  igl::barycenter(V, F, B);

  // Compute local basis for faces
  igl::local_basis(V,F,B1,B2,B3);

  //Generate random vectors for constraints
  generate_constraints();

  // Interpolate a 2-PolyVector field to be used as the original field
  printf("--Initial solution--\n");
  igl::n_polyvector(V, F, b, bc, two_pv_ori);

  // Post process original field
  // Compute curl_minimizing matchings and curl
  Eigen::MatrixXi match_ab, match_ba;  // matchings across interior edges
  printf("--Matchings and curl--\n");
  double avgCurl = igl::polyvector_field_matchings(two_pv_ori, V, F, true, match_ab, match_ba, curl_ori);
  double maxCurl = curl_ori.maxCoeff();
  printf("original curl -- max: %.5g, avg: %.5g\n", maxCurl,  avgCurl);

  printf("--Singularities--\n");
  // Compute singularities
  igl::polyvector_field_singularities_from_matchings(V, F, V_border, VF, TT, E2F, F2E, match_ab, match_ba, singularities_ori);
  printf("original #singularities: %ld\n", singularities.rows());

  printf("--Cuts--\n");
 // Get mesh cuts based on singularities
  igl::polyvector_field_cut_mesh_with_singularities(V, F, VF, VV, TT, TTi, singularities_ori, cuts_ori);

  printf("--Combing--\n");
// Comb field
  Eigen::MatrixXd combed;
  igl::polyvector_field_comb_from_matchings_and_cuts(V, F, TT, E2F, F2E, two_pv_ori, match_ab, match_ba, cuts_ori, combed);

  printf("--Cut mesh--\n");
  // Reconstruct integrable vector fields from combed field
  igl::cut_mesh(V, F, VF, VFi, TT, TTi, V_border, cuts_ori, Vcut_ori, Fcut_ori);

  printf("--Poisson--\n");
  double avgPoisson = igl::polyvector_field_poisson_reconstruction(Vcut_ori, Fcut_ori, combed, scalars_ori, two_pv_poisson_ori, poisson_error_ori);
  double maxPoisson = poisson_error_ori.maxCoeff();
  printf("poisson error -- max: %.5g, avg: %.5g\n", maxPoisson, avgPoisson);


  // Set the curl-free 2-PolyVector to equal the original field
  two_pv = two_pv_ori;
  singularities = singularities_ori;
  curl = curl_ori;
  cuts = cuts_ori;
  two_pv_poisson = two_pv_poisson_ori;
  poisson_error = poisson_error_ori;
  Vcut = Vcut_ori;
  Fcut = Fcut_ori;
  scalars = scalars_ori;

  printf("--Integrable - Precomputation--\n");
  // Precompute stuff for solver
  igl::integrable_polyvector_fields_precompute(V, F, b, bc, blevel, two_pv_ori, ipfdata);

  cerr<<"Done. Press keys 1-0 for various visualizations, 'A' to improve integrability." <<endl;

  igl::viewer::Viewer viewer;
  viewer.callback_key_down = &key_down;
  viewer.core.show_lines = false;
  key_down(viewer,'2',0);

  // Replace the standard texture with an integer shift invariant texture
  line_texture(texture_R, texture_G, texture_B);

  viewer.launch();

  return 0;
}
Beispiel #21
0
TEST (RANSAC, SampleConsensusModelCylinder)
{
  srand (0);

  // Use a custom point cloud for these tests until we need something better
  PointCloud<PointXYZ> cloud;
  PointCloud<Normal> normals;
  cloud.points.resize (20); normals.points.resize (20);

  cloud.points[0].x =  -0.499902f; cloud.points[0].y =  2.199701f; cloud.points[0].z =  0.000008f;
  cloud.points[1].x =  -0.875397f; cloud.points[1].y =  2.030177f; cloud.points[1].z =  0.050104f;
  cloud.points[2].x =  -0.995875f; cloud.points[2].y =  1.635973f; cloud.points[2].z =  0.099846f;
  cloud.points[3].x =  -0.779523f; cloud.points[3].y =  1.285527f; cloud.points[3].z =  0.149961f;
  cloud.points[4].x =  -0.373285f; cloud.points[4].y =  1.216488f; cloud.points[4].z =  0.199959f;
  cloud.points[5].x =  -0.052893f; cloud.points[5].y =  1.475973f; cloud.points[5].z =  0.250101f;
  cloud.points[6].x =  -0.036558f; cloud.points[6].y =  1.887591f; cloud.points[6].z =  0.299839f;
  cloud.points[7].x =  -0.335048f; cloud.points[7].y =  2.171994f; cloud.points[7].z =  0.350001f;
  cloud.points[8].x =  -0.745456f; cloud.points[8].y =  2.135528f; cloud.points[8].z =  0.400072f;
  cloud.points[9].x =  -0.989282f; cloud.points[9].y =  1.803311f; cloud.points[9].z =  0.449983f;
  cloud.points[10].x = -0.900651f; cloud.points[10].y = 1.400701f; cloud.points[10].z = 0.500126f;
  cloud.points[11].x = -0.539658f; cloud.points[11].y = 1.201468f; cloud.points[11].z = 0.550079f;
  cloud.points[12].x = -0.151875f; cloud.points[12].y = 1.340951f; cloud.points[12].z = 0.599983f;
  cloud.points[13].x = -0.000724f; cloud.points[13].y = 1.724373f; cloud.points[13].z = 0.649882f;
  cloud.points[14].x = -0.188573f; cloud.points[14].y = 2.090983f; cloud.points[14].z = 0.699854f;
  cloud.points[15].x = -0.587925f; cloud.points[15].y = 2.192257f; cloud.points[15].z = 0.749956f;
  cloud.points[16].x = -0.927724f; cloud.points[16].y = 1.958846f; cloud.points[16].z = 0.800008f;
  cloud.points[17].x = -0.976888f; cloud.points[17].y = 1.549655f; cloud.points[17].z = 0.849970f;
  cloud.points[18].x = -0.702003f; cloud.points[18].y = 1.242707f; cloud.points[18].z = 0.899954f;
  cloud.points[19].x = -0.289916f; cloud.points[19].y = 1.246296f; cloud.points[19].z = 0.950075f;

  normals.points[0].normal[0] =   0.000098f; normals.points[0].normal[1] =   1.000098f; normals.points[0].normal[2] =   0.000008f;
  normals.points[1].normal[0] =  -0.750891f; normals.points[1].normal[1] =   0.660413f; normals.points[1].normal[2] =   0.000104f;
  normals.points[2].normal[0] =  -0.991765f; normals.points[2].normal[1] =  -0.127949f; normals.points[2].normal[2] =  -0.000154f;
  normals.points[3].normal[0] =  -0.558918f; normals.points[3].normal[1] =  -0.829439f; normals.points[3].normal[2] =  -0.000039f;
  normals.points[4].normal[0] =   0.253627f; normals.points[4].normal[1] =  -0.967447f; normals.points[4].normal[2] =  -0.000041f;
  normals.points[5].normal[0] =   0.894105f; normals.points[5].normal[1] =  -0.447965f; normals.points[5].normal[2] =   0.000101f;
  normals.points[6].normal[0] =   0.926852f; normals.points[6].normal[1] =   0.375543f; normals.points[6].normal[2] =  -0.000161f;
  normals.points[7].normal[0] =   0.329948f; normals.points[7].normal[1] =   0.943941f; normals.points[7].normal[2] =   0.000001f;
  normals.points[8].normal[0] =  -0.490966f; normals.points[8].normal[1] =   0.871203f; normals.points[8].normal[2] =   0.000072f;
  normals.points[9].normal[0] =  -0.978507f; normals.points[9].normal[1] =   0.206425f; normals.points[9].normal[2] =  -0.000017f;
  normals.points[10].normal[0] = -0.801227f; normals.points[10].normal[1] = -0.598534f; normals.points[10].normal[2] =  0.000126f;
  normals.points[11].normal[0] = -0.079447f; normals.points[11].normal[1] = -0.996697f; normals.points[11].normal[2] =  0.000079f;
  normals.points[12].normal[0] =  0.696154f; normals.points[12].normal[1] = -0.717889f; normals.points[12].normal[2] = -0.000017f;
  normals.points[13].normal[0] =  0.998685f; normals.points[13].normal[1] =  0.048502f; normals.points[13].normal[2] = -0.000118f;
  normals.points[14].normal[0] =  0.622933f; normals.points[14].normal[1] =  0.782133f; normals.points[14].normal[2] = -0.000146f;
  normals.points[15].normal[0] = -0.175948f; normals.points[15].normal[1] =  0.984480f; normals.points[15].normal[2] = -0.000044f;
  normals.points[16].normal[0] = -0.855476f; normals.points[16].normal[1] =  0.517824f; normals.points[16].normal[2] =  0.000008f;
  normals.points[17].normal[0] = -0.953769f; normals.points[17].normal[1] = -0.300571f; normals.points[17].normal[2] = -0.000030f;
  normals.points[18].normal[0] = -0.404035f; normals.points[18].normal[1] = -0.914700f; normals.points[18].normal[2] = -0.000046f;
  normals.points[19].normal[0] =  0.420154f; normals.points[19].normal[1] = -0.907445f; normals.points[19].normal[2] =  0.000075f;

  // Create a shared cylinder model pointer directly
  SampleConsensusModelCylinderPtr model (new SampleConsensusModelCylinder<PointXYZ, Normal> (cloud.makeShared ()));
  model->setInputNormals (normals.makeShared ());

  // Create the RANSAC object
  RandomSampleConsensus<PointXYZ> sac (model, 0.03);

  // Algorithm tests
  bool result = sac.computeModel ();
  ASSERT_EQ (result, true);

  std::vector<int> sample;
  sac.getModel (sample);
  EXPECT_EQ (int (sample.size ()), 2);

  std::vector<int> inliers;
  sac.getInliers (inliers);
  EXPECT_EQ (int (inliers.size ()), 20);

  Eigen::VectorXf coeff;
  sac.getModelCoefficients (coeff);
  EXPECT_EQ (int (coeff.size ()), 7);
  EXPECT_NEAR (coeff[0], -0.5, 1e-3);
  EXPECT_NEAR (coeff[1],  1.7,  1e-3);
  EXPECT_NEAR (coeff[6],  0.5, 1e-3);

  Eigen::VectorXf coeff_refined;
  model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
  EXPECT_EQ (int (coeff_refined.size ()), 7);
  EXPECT_NEAR (coeff_refined[6], 0.5, 1e-3);
}
	virtual Eigen::VectorXf initialValue() {
		Eigen::VectorXf p( unary_*initial_u_param_.rows() + pairwise_*initial_lbl_param_.rows() + kernel_*initial_knl_param_.rows() );
		p << (unary_?initial_u_param_:Eigen::VectorXf()), (pairwise_?initial_lbl_param_:Eigen::VectorXf()), (kernel_?initial_knl_param_:Eigen::VectorXf());
		return p;
	}
Beispiel #23
0
TEST (RANSAC, SampleConsensusModelLine)
{
  srand (0);

  // Use a custom point cloud for these tests until we need something better
  PointCloud<PointXYZ> cloud;
  cloud.points.resize (10);

  cloud.points[0].x = 1.0f;  cloud.points[0].y = 2.0f;  cloud.points[0].z = 3.0f;
  cloud.points[1].x = 4.0f;  cloud.points[1].y = 5.0f;  cloud.points[1].z = 6.0f;
  cloud.points[2].x = 7.0f;  cloud.points[2].y = 8.0f;  cloud.points[2].z = 9.0f;
  cloud.points[3].x = 10.0f; cloud.points[3].y = 11.0f; cloud.points[3].z = 12.0f;
  cloud.points[4].x = 13.0f; cloud.points[4].y = 14.0f; cloud.points[4].z = 15.0f;
  cloud.points[5].x = 16.0f; cloud.points[5].y = 17.0f; cloud.points[5].z = 18.0f;
  cloud.points[6].x = 19.0f; cloud.points[6].y = 20.0f; cloud.points[6].z = 21.0f;
  cloud.points[7].x = 22.0f; cloud.points[7].y = 23.0f; cloud.points[7].z = 24.0f;
  cloud.points[8].x = -5.0f; cloud.points[8].y = 1.57f; cloud.points[8].z = 0.75f;
  cloud.points[9].x = 4.0f;  cloud.points[9].y = 2.0f;  cloud.points[9].z = 3.0f;

  // Create a shared line model pointer directly
  SampleConsensusModelLinePtr model (new SampleConsensusModelLine<PointXYZ> (cloud.makeShared ()));

  // Create the RANSAC object
  RandomSampleConsensus<PointXYZ> sac (model, 0.001);

  // Algorithm tests
  bool result = sac.computeModel ();
  ASSERT_EQ (result, true);

  std::vector<int> sample;
  sac.getModel (sample);
  EXPECT_EQ (int (sample.size ()), 2);

  std::vector<int> inliers;
  sac.getInliers (inliers);
  EXPECT_EQ (int (inliers.size ()), 8);

  Eigen::VectorXf coeff;
  sac.getModelCoefficients (coeff);
  EXPECT_EQ (int (coeff.size ()), 6);
  EXPECT_NEAR (coeff[4]/coeff[3], 1, 1e-4);
  EXPECT_NEAR (coeff[5]/coeff[3], 1, 1e-4);

  Eigen::VectorXf coeff_refined;
  model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
  EXPECT_EQ (int (coeff_refined.size ()), 6);
  EXPECT_NEAR (coeff[4]/coeff[3], 1, 1e-4);
  EXPECT_NEAR (coeff[5]/coeff[3], 1, 1e-4);

  // Projection tests
  PointCloud<PointXYZ> proj_points;
  model->projectPoints (inliers, coeff_refined, proj_points);

  EXPECT_NEAR (proj_points.points[2].x, 7.0, 1e-4);
  EXPECT_NEAR (proj_points.points[2].y, 8.0, 1e-4);
  EXPECT_NEAR (proj_points.points[2].z, 9.0, 1e-4);

  EXPECT_NEAR (proj_points.points[3].x, 10.0, 1e-4);
  EXPECT_NEAR (proj_points.points[3].y, 11.0, 1e-4);
  EXPECT_NEAR (proj_points.points[3].z, 12.0, 1e-4);

  EXPECT_NEAR (proj_points.points[5].x, 16.0, 1e-4);
  EXPECT_NEAR (proj_points.points[5].y, 17.0, 1e-4);
  EXPECT_NEAR (proj_points.points[5].z, 18.0, 1e-4);
}
	virtual double gradient( const Eigen::VectorXf & x, Eigen::VectorXf & dx ) {
		int p = 0;
		if (unary_) {
			crf_.setUnaryParameters( x.segment( p, initial_u_param_.rows() ) );
			p += initial_u_param_.rows();
		}
		if (pairwise_) {
			crf_.setLabelCompatibilityParameters( x.segment( p, initial_lbl_param_.rows() ) );
			p += initial_lbl_param_.rows();
		}
		if (kernel_)
			crf_.setKernelParameters( x.segment( p, initial_knl_param_.rows() ) );
		
		Eigen::VectorXf du = 0*initial_u_param_, dl = 0*initial_u_param_, dk = 0*initial_knl_param_;
		double r = crf_.gradient( NIT_, objective_, unary_?&du:NULL, pairwise_?&dl:NULL, kernel_?&dk:NULL );
		dx.resize( unary_*du.rows() + pairwise_*dl.rows() + kernel_*dk.rows() );
		dx << -(unary_?du:Eigen::VectorXf()), -(pairwise_?dl:Eigen::VectorXf()), -(kernel_?dk:Eigen::VectorXf());
		r = -r;
		if( l2_norm_ > 0 ) {
			dx += l2_norm_ * x;
			r += 0.5*l2_norm_ * (x.dot(x));
		}
		
		return r;
	}
Beispiel #25
0
void Eigen_to_G(const Eigen::VectorXf& EigenV, Vector* _GV) {
  Vector& GV = *_GV;
  GV.Dimension(EigenV.size());
  for (int i = 0; i < EigenV.size(); i++) GV[i] = EigenV(i);
}
int main( int argc, char* argv[]){
	if (argc<4){
		printf("Usage: %s image annotations output\n", argv[0] );
		return 1;
	}
	// Number of labels [use only 4 to make our lives a bit easier]
	const int M = 4;
	// Load the color image and some crude annotations (which are used in a simple classifier)
	int W, H, GW, GH;
	unsigned char * im = readPPM( argv[1], W, H );
	if (!im){
		printf("Failed to load image!\n");
		return 1;
	}
	unsigned char * anno = readPPM( argv[2], GW, GH );
	if (!anno){
		printf("Failed to load annotations!\n");
		return 1;
	}
	if (W!=GW || H!=GH){
		printf("Annotation size doesn't match image!\n");
		return 1;
	}
	// Get the labeling
	VectorXs labeling = getLabeling( anno, W*H, M );
	const int N = W*H;
	
	// Get the logistic features (unary term)
	// Here we just use the color as a feature
  Eigen::MatrixXf logistic_feature( 4, N ), logistic_transform( M, 4 );
	logistic_feature.fill( 1.f );
	for( int i=0; i<N; i++ )
		for( int k=0; k<3; k++ )
			logistic_feature(k,i) = im[3*i+k] / 255.;
	
	for( int j=0; j<logistic_transform.cols(); j++ )
		for( int i=0; i<logistic_transform.rows(); i++ )
			logistic_transform(i,j) = 0.01*(1-2.*random()/RAND_MAX);
	
	// Setup the CRF model
	DenseCRF2D crf(W, H, M);
	// Add a logistic unary term
	crf.setUnaryEnergy( logistic_transform, logistic_feature );
	
	// Add simple pairwise potts terms
	crf.addPairwiseGaussian( 3, 3, new PottsCompatibility( 1 ) );
	// Add a longer range label compatibility term
  crf.addPairwiseBilateral( 80.0f, 80.0f, 13.0f, 13.0f, 13.0f, &*im, new MatrixCompatibility::MatrixCompatibility( Eigen::MatrixXf::Identity(M,M) ) );
	
	// Choose your loss function
// 	LogLikelihood objective( labeling, 0.01 ); // Log likelihood loss
// 	Hamming objective( labeling, 0.0 ); // Global accuracy
// 	Hamming objective( labeling, 1.0 ); // Class average accuracy
// 	Hamming objective( labeling, 0.2 ); // Hamming loss close to intersection over union
	IntersectionOverUnion objective( labeling ); // Intersection over union accuracy
	
	int NIT = 5;
	const bool verbose = true;
	
  Eigen::MatrixXf learning_params( 3, 3 );
	// Optimize the CRF in 3 phases:
	//  * First unary only
	//  * Unary and pairwise
	//  * Full CRF
	learning_params<<1,0,0,
	                 1,1,0,
					 1,1,1;
	
	for( int i=0; i<learning_params.rows(); i++ ) {
		// Setup the energy
		CRFEnergy energy( crf, objective, NIT, learning_params(i,0), learning_params(i,1), learning_params(i,2) );
		energy.setL2Norm( 1e-3 );
		
		// Minimize the energy
		Eigen::VectorXf p = minimizeLBFGS( energy, 2, true );
		
		// Save the values
		int id = 0;
		if( learning_params(i,0) ) {
			crf.setUnaryParameters( p.segment( id, crf.unaryParameters().rows() ) );
			id += crf.unaryParameters().rows();
		}
		if( learning_params(i,1) ) {
			crf.setLabelCompatibilityParameters( p.segment( id, crf.labelCompatibilityParameters().rows() ) );
			id += crf.labelCompatibilityParameters().rows();
		}
		if( learning_params(i,2) )
			crf.setKernelParameters( p.segment( id, crf.kernelParameters().rows() ) );
	}
	// Return the parameters
	std::cout<<"Unary parameters: "<<crf.unaryParameters().transpose()<<std::endl;
	std::cout<<"Pairwise parameters: "<<crf.labelCompatibilityParameters().transpose()<<std::endl;
	std::cout<<"Kernel parameters: "<<crf.kernelParameters().transpose()<<std::endl;
	
	// Do map inference
	VectorXs map = crf.map(NIT);
	
	// Store the result
	unsigned char *res = colorize( map, W, H );
	writePPM( argv[3], W, H, res );
	
	delete[] im;
	delete[] anno;
	delete[] res;
}
Beispiel #27
0
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCone<PointT, PointNT>::projectPoints (
      const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 7)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCone::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
    return;
  }

  projected_points.header = input_->header;
  projected_points.is_dense = input_->is_dense;

  Eigen::Vector4f apex  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  float opening_angle = model_coefficients[6];

  float apexdotdir = apex.dot (axis_dir);
  float dirdotdir = 1.0f / axis_dir.dot (axis_dir);

  // Copy all the data fields from the input cloud to the projected one?
  if (copy_data_fields)
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (input_->points.size ());
    projected_points.width    = input_->width;
    projected_points.height   = input_->height;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < projected_points.points.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the cone
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      Eigen::Vector4f pt (input_->points[inliers[i]].x, 
                          input_->points[inliers[i]].y, 
                          input_->points[inliers[i]].z, 
                          1);

      float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;

      pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap ();
      pp.matrix () = apex + k * axis_dir;

      Eigen::Vector4f dir = pt - pp;
      dir.normalize ();

      // Calculate the actual radius of the cone at the level of the projected point
      Eigen::Vector4f height = apex - pp;
      float actual_cone_radius = tanf (opening_angle) * height.norm ();

      // Calculate the projection of the point onto the cone
      pp += dir * actual_cone_radius;
    }
  }
  else
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (inliers.size ());
    projected_points.width    = static_cast<uint32_t> (inliers.size ());
    projected_points.height   = 1;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < inliers.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the cone
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap ();
      pcl::Vector4fMapConst pt = input_->points[inliers[i]].getVector4fMap ();

      float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
      // Calculate the projection of the point on the line
      pp.matrix () = apex + k * axis_dir;

      Eigen::Vector4f dir = pt - pp;
      dir.normalize ();

      // Calculate the actual radius of the cone at the level of the projected point
      Eigen::Vector4f height = apex - pp;
      float actual_cone_radius = tanf (opening_angle) * height.norm ();

      // Calculate the projection of the point onto the cone
      pp += dir * actual_cone_radius;
    }
  }
}
Beispiel #28
0
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCylinder<PointT, PointNT>::computeModelCoefficients (
      const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
{
  // Need 2 samples
  if (samples.size () != 2)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ());
    return (false);
  }

  if (!normals_)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] No input dataset containing normals was given!\n");
    return (false);
  }

  Eigen::Vector4f p1 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z, 0);
  Eigen::Vector4f p2 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z, 0);

  Eigen::Vector4f n1 (normals_->points[samples[0]].normal[0], normals_->points[samples[0]].normal[1], normals_->points[samples[0]].normal[2], 0);
  Eigen::Vector4f n2 (normals_->points[samples[1]].normal[0], normals_->points[samples[1]].normal[1], normals_->points[samples[1]].normal[2], 0);
  Eigen::Vector4f w = n1 + p1 - p2;

  float a = n1.dot (n1);
  float b = n1.dot (n2);
  float c = n2.dot (n2);
  float d = n1.dot (w);
  float e = n2.dot (w);
  float denominator = a*c - b*b;
  float sc, tc;
  // Compute the line parameters of the two closest points
  if (denominator < 1e-8)          // The lines are almost parallel
  {
    sc = 0.0f;
    tc = (b > c ? d / b : e / c);  // Use the largest denominator
  }
  else
  {
    sc = (b*e - c*d) / denominator;
    tc = (a*e - b*d) / denominator;
  }

  // point_on_axis, axis_direction
  Eigen::Vector4f line_pt  = p1 + n1 + sc * n1;
  Eigen::Vector4f line_dir = p2 + tc * n2 - line_pt;
  line_dir.normalize ();

  model_coefficients.resize (7);
  // model_coefficients.template head<3> ()    = line_pt.template head<3> ();
  model_coefficients[0] = line_pt[0];
  model_coefficients[1] = line_pt[1];
  model_coefficients[2] = line_pt[2];
  // model_coefficients.template segment<3> (3) = line_dir.template head<3> ();
  model_coefficients[3] = line_dir[0];
  model_coefficients[4] = line_dir[1];
  model_coefficients[5] = line_dir[2];
  // cylinder radius
  model_coefficients[6] = static_cast<float> (sqrt (pcl::sqrPointToLineDistance (p1, line_pt, line_dir)));

  if (model_coefficients[6] > radius_max_ || model_coefficients[6] < radius_min_)
    return (false);

  return (true);
}
void SimDynamicsWindow::updateJointInfo()
{
    //std::stringstream info;
    std::string info;
    int n = UI.comboBoxRobotNode->currentIndex();
    QString qMin("0");
    QString qMax("0");
    QString qName("Name: <not set>");
    QString qJV("Joint value: 0");
    QString qTarget("Joint target: 0");
    QString qVel("Joint velocity: 0");
    QString qVelTarget("Joint velocity target: 0");
    QString qGP("GlobalPose (simox): 0/0/0");
    QString qVisu("VISU (simox): 0/0/0");
    QString qCom("COM (bullet): 0/0/0");
    QString tmp;
    RobotNodeRevolutePtr rn;

    if (n >= 0 && n < (int)robotNodes.size())
    {
        rn = robotNodes[n];
    }

    SimDynamics::DynamicsObjectPtr dynRN = dynamicsRobot->getDynamicsRobotNode(rn);
    SimDynamics::BulletObjectPtr bulletRN = boost::dynamic_pointer_cast<SimDynamics::BulletObject>(dynRN);

    if (bulletRN)
    {
        //      cout << "FORCE: " << bulletRN->getRigidBody()->getTotalForce()[0] << ", " << bulletRN->getRigidBody()->getTotalForce()[1] << ", " << bulletRN->getRigidBody()->getTotalForce()[2] << endl;
        //      cout << "TORQUE: " << bulletRN->getRigidBody()->getTotalTorque()[0] << ", " << bulletRN->getRigidBody()->getTotalTorque()[1] << ", " << bulletRN->getRigidBody()->getTotalTorque()[2] << endl;
        //      cout << "getLinearVelocity: " << bulletRN->getRigidBody()->getLinearVelocity()[0] << ", " << bulletRN->getRigidBody()->getLinearVelocity()[1] << ", " << bulletRN->getRigidBody()->getLinearVelocity()[2] << endl;
        //      cout << "getAngularVelocity: " << bulletRN->getRigidBody()->getAngularVelocity()[0] << ", " << bulletRN->getRigidBody()->getAngularVelocity()[1] << ", " << bulletRN->getRigidBody()->getAngularVelocity()[2] << endl;

    }

    BulletRobotPtr bulletRobot = boost::dynamic_pointer_cast<SimDynamics::BulletRobot>(dynamicsRobot);

    if (rn && bulletRobot && bulletRobot->hasLink(rn))
    {
        BulletRobot::LinkInfo linkInfo = bulletRobot->getLink(rn);

        linkInfo.nodeA->showCoordinateSystem(true);
        linkInfo.nodeB->showCoordinateSystem(true);

        bulletRobot->enableForceTorqueFeedback(linkInfo);
        Eigen::VectorXf ftA = bulletRobot->getForceTorqueFeedbackA(linkInfo);
        Eigen::VectorXf ftB = bulletRobot->getForceTorqueFeedbackB(linkInfo);

        std::stringstream streamFTA;

        streamFTA << "ForceTorqueA: " << std::fixed;
        streamFTA.precision(1);

        for (int i = 0; i < 6; ++i)
        {
            streamFTA << ftA[i] << ",";
        }

        UI.label_ForceTorqueA->setText(QString::fromStdString(streamFTA.str()));
        std::stringstream streamFTB;
        streamFTB << "ForceTorqueB: " << std::fixed;
        streamFTB.precision(1);

        for (int i = 0; i < 6; ++i)
        {
            streamFTB << ftB[i] << ",";
        }

        UI.label_ForceTorqueB->setText(QString::fromStdString(streamFTB.str()));

        //      cout << "ForceTorqueA:" << endl;
        //      MathTools::print(ftA);
        //      cout << "ForceTorqueB:" << endl;
        //      MathTools::print(ftB);

        forceSep->removeAllChildren();

        SoUnits* u = new SoUnits();
        u->units = SoUnits::MILLIMETERS;
        forceSep->addChild(u);

        Eigen::Vector3f n = ftA.head(3);
        //n = linkInfo.nodeA->toGlobalCoordinateSystemVec(n);
        float l = ftA.head(3).norm();
        float w = 5.0f;
        SoSeparator* forceA = new SoSeparator;
        SoSeparator* arrowForceA = CoinVisualizationFactory::CreateArrow(n, l, w, VisualizationFactory::Color::Red());

        /*
        cout << "FORCE_A: " << linkInfo.dynNode1->getRigidBody()->getTotalForce()[0] << "," << linkInfo.dynNode1->getRigidBody()->getTotalForce()[1] << "," << linkInfo.dynNode1->getRigidBody()->getTotalForce()[2] << endl;
        cout << "TORQUEA: " << linkInfo.dynNode1->getRigidBody()->getTotalTorque()[0] << "," << linkInfo.dynNode1->getRigidBody()->getTotalTorque()[1] << "," << linkInfo.dynNode1->getRigidBody()->getTotalTorque()[2] << endl;
        cout << "FORCE_B: " << linkInfo.dynNode2->getRigidBody()->getTotalForce()[0] << "," << linkInfo.dynNode2->getRigidBody()->getTotalForce()[1] << "," << linkInfo.dynNode2->getRigidBody()->getTotalForce()[2] << endl;
        cout << "TORQUEB: " << linkInfo.dynNode2->getRigidBody()->getTotalTorque()[0] << "," << linkInfo.dynNode2->getRigidBody()->getTotalTorque()[1] << "," << linkInfo.dynNode2->getRigidBody()->getTotalTorque()[2] << endl;
        */

        // show as nodeA local coords system
        /*
        Eigen::Matrix4f comCoord = linkInfo.nodeA->getGlobalPose();
        Eigen::Matrix4f comLocal = Eigen::Matrix4f::Identity();
        comLocal.block(0,3,3,1) = linkInfo.nodeA->getCoMLocal();
        comCoord = comCoord * comLocal;
        */
        // show as global coords
        Eigen::Matrix4f comGlobal = Eigen::Matrix4f::Identity();
        comGlobal.block(0, 3, 3, 1) = linkInfo.nodeA->getCoMGlobal();
        SoMatrixTransform* m = CoinVisualizationFactory::getMatrixTransform(comGlobal);
        forceA->addChild(m);
        forceA->addChild(arrowForceA);

        forceSep->addChild(forceA);

        Eigen::Vector3f jointGlobal = linkInfo.nodeJoint->getGlobalPose().block(0, 3, 3, 1);
        Eigen::Vector3f comBGlobal = linkInfo.nodeB->getCoMGlobal();

        // force that is applied on objectA by objectB
        Eigen::Vector3f FBGlobal =  ftA.head(3);
        Eigen::Vector3f TBGlobal =  ftB.tail(3) ;

        Eigen::VectorXf torqueJointGlobal  = bulletRobot->getJointForceTorqueGlobal(linkInfo);//= TBGlobal  - (comBGlobal-jointGlobal).cross(FBGlobal) * 0.001;
        //        cout << "torqueJointGlobal: " << torqueJointGlobal << endl;
        std::stringstream streamFTJoint;
        streamFTJoint.precision(1);
        streamFTJoint << "ForceTorqueJoint: " << std::fixed;

        for (int i = 0; i < 6; ++i)
        {
            streamFTJoint << torqueJointGlobal[i] << ",";
        }

        UI.label_ForceTorqueJoint->setText(QString::fromStdString(streamFTJoint.str()));


        n = ftB.head(3);
        //n = linkInfo.nodeB->toGlobalCoordinateSystemVec(n);
        l = ftB.head(3).norm();
        w = 5.0f;
        SoSeparator* forceB = new SoSeparator;
        SoSeparator* arrowForceB = CoinVisualizationFactory::CreateArrow(n, l, w, VisualizationFactory::Color::Red());

        comGlobal = Eigen::Matrix4f::Identity();
        comGlobal.block(0, 3, 3, 1) = linkInfo.nodeB->getCoMGlobal();
        m = CoinVisualizationFactory::getMatrixTransform(comGlobal);
        forceB->addChild(m);
        forceB->addChild(arrowForceB);

        forceSep->addChild(forceB);



        /*
                if (!linkInfo.joint->needsFeedback())
                {
                    linkInfo.joint->enableFeedback(true);
                    btJointFeedback* feedback = new btJointFeedback;
                    feedback->m_appliedForceBodyA = btVector3(0, 0, 0);
                    feedback->m_appliedForceBodyB = btVector3(0, 0, 0);
                    feedback->m_appliedTorqueBodyA = btVector3(0, 0, 0);
                    feedback->m_appliedTorqueBodyB = btVector3(0, 0, 0);
                    linkInfo.joint->setJointFeedback(feedback);
                }
                else
                {
                    btJointFeedback* feedback = linkInfo.joint->getJointFeedback();
                    cout << "feedback->m_appliedForceBodyA: " << feedback->m_appliedForceBodyA[0] << "," << feedback->m_appliedForceBodyA[1] << "," << feedback->m_appliedForceBodyA[2] << endl;
                    cout << "feedback->m_appliedForceBodyB: " << feedback->m_appliedForceBodyB[0] << "," << feedback->m_appliedForceBodyB[1] << "," << feedback->m_appliedForceBodyB[2] << endl;
                    cout << "feedback->m_appliedTorqueBodyA: " << feedback->m_appliedTorqueBodyA[0] << "," << feedback->m_appliedTorqueBodyA[1] << "," << feedback->m_appliedTorqueBodyA[2] << endl;
                    cout << "feedback->m_appliedTorqueBodyB: " << feedback->m_appliedTorqueBodyB[0] << "," << feedback->m_appliedTorqueBodyB[1] << "," << feedback->m_appliedTorqueBodyB[2] << endl;

                }
                */
    }

    if (rn)
    {
        qMin = QString::number(rn->getJointLimitLo(), 'f', 4);
        qMax = QString::number(rn->getJointLimitHi(), 'f', 4);
        qName = QString("Name: ");
        qName += QString(rn->getName().c_str());
        qJV = QString("Joint value: ");
        tmp = QString::number(rn->getJointValue(), 'f', 3);
        qJV += tmp;
        info += "jv rn:";
        std::string a1 = (const char*)tmp.toAscii();
        info += a1;

        qJV += QString(" / ");

        if (dynamicsRobot->isNodeActuated(rn))
        {
            tmp = QString::number(dynamicsRobot->getJointAngle(rn), 'f', 3);
        }
        else
        {
            tmp = QString("-");
        }

        qJV += tmp;
        info += ",\tjv bul:";
        a1 = (const char*)tmp.toAscii();
        info += a1;

        qTarget = QString("Joint target: ");

        if (dynamicsRobot->isNodeActuated(rn))
        {
            tmp = QString::number(dynamicsRobot->getNodeTarget(rn), 'f', 3);
        }
        else
        {
            tmp = QString("-");
        }

        qTarget += tmp;
        info += std::string(",targ:");
        a1 = (const char*)tmp.toAscii();
        info += a1;

        qVel = QString("Joint velocity: ");

        if (dynamicsRobot->isNodeActuated(rn))
        {
            tmp = QString::number(dynamicsRobot->getJointSpeed(rn), 'f', 3);
        }
        else
        {
            tmp = QString("-");
        }

        qVel += tmp;
        info += ",vel:";
        a1 = (const char*)tmp.toAscii();
        info += a1;

        qVelTarget = QString("Joint velocity target: ");

        if (dynamicsRobot->isNodeActuated(rn))
        {
            tmp = QString::number(dynamicsRobot->getJointTargetSpeed(rn), 'f', 3);
        }
        else
        {
            tmp = QString("-");
        }

        qVelTarget += tmp;
        info += ",velTarget:";
        a1 = (const char*)tmp.toAscii();
        info += a1;

        Eigen::Matrix4f gp = rn->getGlobalPose();

        qGP = QString("GlobalPose (simox):");
        tmp = QString::number(gp(0, 3), 'f', 2);
        qGP += tmp;
        info += ",gp:";
        info += (const char*)tmp.toAscii();

        qGP += QString("/");
        tmp = QString::number(gp(1, 3), 'f', 2);
        qGP += tmp;
        info += "/";
        info += (const char*)tmp.toAscii();

        qGP += QString("/");
        tmp = QString::number(gp(2, 3), 'f', 2);
        qGP += tmp;
        info += "/";
        info += (const char*)tmp.toAscii();

        gp = rn->getGlobalPose();
        qVisu = QString("VISU (simox):");
        qVisu += QString::number(gp(0, 3), 'f', 2);
        qVisu += QString("/");
        qVisu += QString::number(gp(1, 3), 'f', 2);
        qVisu += QString("/");
        qVisu += QString::number(gp(2, 3), 'f', 2);

        if (dynamicsRobot->hasDynamicsRobotNode(rn))
        {
            gp = dynamicsRobot->getComGlobal(rn);
        }
        else
        {
            gp = Eigen::Matrix4f::Identity();
        }

        qCom = QString("COM (bullet):");
        qCom += QString::number(gp(0, 3), 'f', 2);
        qCom += QString("/");
        qCom += QString::number(gp(1, 3), 'f', 2);
        qCom += QString("/");
        qCom += QString::number(gp(2, 3), 'f', 2);
    }

    UI.label_TargetMin->setText(qMin);
    UI.label_TargetMax->setText(qMax);
    UI.label_RNName->setText(qName);
    UI.label_RNValue->setText(qJV);
    UI.label_RNTarget->setText(qTarget);
    UI.label_RNVelocity->setText(qVel);
    UI.label_RNVelocityTarget->setText(qVelTarget);
    UI.label_RNPosGP->setText(qGP);
    UI.label_RNPosVisu->setText(qVisu);
    UI.label_RNPosCom->setText(qCom);

#if 0

    // print some joint info
    if (viewer->engineRunning())
    {
        cout << info << endl;
    }

#endif
}
Beispiel #30
0
 inline void resize(Eigen::VectorXf & v,
                    vcl_size_t new_size)
 {
   v.resize(new_size);
 }