C++ Vector3f::dot方法代码示例

int
main (int argc, char** argv)
{
  if (argc < 2)
  {
    std::cerr << "Error: Please specify a PCD file (rosrun cob_3d_features profile_ne test.pcd)." << std::endl;
    return -1;
  }
  PointCloud<PointXYZ>::Ptr p (new PointCloud<PointXYZ>);
  PointCloud<Normal>::Ptr n (new PointCloud<Normal>);
  PointCloud<InterestPoint>::Ptr ip (new PointCloud<InterestPoint>);
  pcl::PointCloud<pcl::PointNormal> p_n;
  PrecisionStopWatch t;
  std::string file_ = argv[1];
  PCDReader r;
  if (r.read (file_, *p) == -1)
    return -1;

  Eigen::Vector3f normal;
  determinePlaneNormal (p, normal);
  //std::cout << normal << std::endl;

  cob_3d_features::OrganizedNormalEstimation<PointXYZ, Normal, PointLabel> ne;
  ne.setPixelSearchRadius (8, 2, 2);
  //ne.setSkipDistantPointThreshold(8);
  ne.setInputCloud (p);
  PointCloud<PointLabel>::Ptr labels (new PointCloud<PointLabel>);
  ne.setOutputLabels (labels);
  t.precisionStart ();
  ne.compute (*n);
  std::cout << t.precisionStop () << "s\t for organized normal estimation" << std::endl;

  cob_3d_features::OrganizedNormalEstimationOMP<PointXYZ, Normal, PointLabel> ne_omp;
  ne_omp.setPixelSearchRadius (8, 2, 2);
  //ne.setSkipDistantPointThreshold(8);
  ne_omp.setInputCloud (p);
  //PointCloud<PointLabel>::Ptr labels(new PointCloud<PointLabel>);
  ne_omp.setOutputLabels (labels);
  t.precisionStart ();
  ne_omp.compute (*n);
  std::cout << t.precisionStop () << "s\t for organized normal estimation (OMP)" << std::endl;
  concatenateFields (*p, *n, p_n);
  io::savePCDFileASCII ("normals_organized.pcd", p_n);

  double good_thr = 0.97;
  unsigned int ctr = 0, nan_ctr = 0;
  double d_sum = 0;
  for (unsigned int i = 0; i < p->size (); i++)
  {
    if (pcl_isnan(n->points[i].normal[0]))
    {
      nan_ctr++;
      continue;
    }
    double d = normal.dot (n->points[i].getNormalVector3fMap ());
    d_sum += fabs (1 - fabs (d));
    if (fabs (d) > good_thr)
      ctr++;
  }
  std::cout << "Average error: " << d_sum / p->size () << std::endl;
  std::cout << "Ratio of good normals: " << (double)ctr / p->size () << std::endl;
  std::cout << "Invalid normals: " << nan_ctr << std::endl;

  IntegralImageNormalEstimation<PointXYZ, Normal> ne2;
  ne2.setNormalEstimationMethod (ne2.COVARIANCE_MATRIX);
  ne2.setMaxDepthChangeFactor (0.02f);
  ne2.setNormalSmoothingSize (10.0f);
  ne2.setDepthDependentSmoothing (true);
  ne2.setInputCloud (p);
  t.precisionStart ();
  ne2.compute (*n);
  std::cout << t.precisionStop () << "s\t for integral image normal estimation" << std::endl;
  concatenateFields (*p, *n, p_n);
  io::savePCDFileASCII ("normals_integral.pcd", p_n);

  ctr = 0;
  nan_ctr = 0;
  d_sum = 0;
  for (unsigned int i = 0; i < p->size (); i++)
  {
    if (pcl_isnan(n->points[i].normal[0]))
    {
      nan_ctr++;
      continue;
    }
    double d = normal.dot (n->points[i].getNormalVector3fMap ());
    d_sum += fabs (1 - fabs (d));
    if (fabs (d) > good_thr)
      ctr++;
  }
  std::cout << "Average error: " << d_sum / p->size () << std::endl;
  std::cout << "Ratio of good normals: " << (double)ctr / p->size () << std::endl;
  std::cout << "Invalid normals: " << nan_ctr << std::endl;

  NormalEstimationOMP<PointXYZ, Normal> ne3;
  ne3.setInputCloud (p);
  ne3.setNumberOfThreads (4);
  ne3.setKSearch (256);
  //ne3.setRadiusSearch(0.01);
  t.precisionStart ();
//.........这里部分代码省略.........

int test_eigen(int argc, char *argv[])
{
	int rc = 0;
	warnx("testing eigen");

	{
		Eigen::Vector2f v;
		Eigen::Vector2f v1(1.0f, 2.0f);
		Eigen::Vector2f v2(1.0f, -1.0f);
		float data[2] = {1.0f, 2.0f};
		TEST_OP("Constructor Vector2f()", Eigen::Vector2f v3);
		TEST_OP_VERIFY("Constructor Vector2f(Vector2f)", Eigen::Vector2f v3(v1), v3.isApprox(v1));
		TEST_OP_VERIFY("Constructor Vector2f(float[])", Eigen::Vector2f v3(data), v3[0] == data[0] && v3[1] == data[1]);
		TEST_OP_VERIFY("Constructor Vector2f(float, float)", Eigen::Vector2f v3(1.0f, 2.0f), v3(0) == 1.0f && v3(1) == 2.0f);
		TEST_OP_VERIFY("Vector2f = Vector2f", v = v1, v.isApprox(v1));
		VERIFY_OP("Vector2f + Vector2f", v = v + v1, v.isApprox(v1 + v1));
		VERIFY_OP("Vector2f - Vector2f", v = v - v1, v.isApprox(v1));
		VERIFY_OP("Vector2f += Vector2f", v += v1, v.isApprox(v1 + v1));
		VERIFY_OP("Vector2f -= Vector2f", v -= v1, v.isApprox(v1));
		TEST_OP_VERIFY("Vector2f dot Vector2f", v.dot(v1), fabs(v.dot(v1) - 5.0f) <= FLT_EPSILON);
		//TEST_OP("Vector2f cross Vector2f", v1.cross(v2)); //cross product for 2d array?
	}

	{
		Eigen::Vector3f v;
		Eigen::Vector3f v1(1.0f, 2.0f, 0.0f);
		Eigen::Vector3f v2(1.0f, -1.0f, 2.0f);
		float data[3] = {1.0f, 2.0f, 3.0f};
		TEST_OP("Constructor Vector3f()", Eigen::Vector3f v3);
		TEST_OP("Constructor Vector3f(Vector3f)", Eigen::Vector3f v3(v1));
		TEST_OP("Constructor Vector3f(float[])", Eigen::Vector3f v3(data));
		TEST_OP("Constructor Vector3f(float, float, float)", Eigen::Vector3f v3(1.0f, 2.0f, 3.0f));
		TEST_OP("Vector3f = Vector3f", v = v1);
		TEST_OP("Vector3f + Vector3f", v + v1);
		TEST_OP("Vector3f - Vector3f", v - v1);
		TEST_OP("Vector3f += Vector3f", v += v1);
		TEST_OP("Vector3f -= Vector3f", v -= v1);
		TEST_OP("Vector3f * float", v1 * 2.0f);
		TEST_OP("Vector3f / float", v1 / 2.0f);
		TEST_OP("Vector3f *= float", v1 *= 2.0f);
		TEST_OP("Vector3f /= float", v1 /= 2.0f);
		TEST_OP("Vector3f dot Vector3f", v.dot(v1));
		TEST_OP("Vector3f cross Vector3f", v1.cross(v2));
		TEST_OP("Vector3f length", v1.norm());
		TEST_OP("Vector3f length squared", v1.squaredNorm());
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
		// Need pragma here intead of moving variable out of TEST_OP and just reference because
		// TEST_OP measures performance of vector operations.
		TEST_OP("Vector<3> element read", volatile float a = v1(0));
		TEST_OP("Vector<3> element read direct", volatile float a = v1.data()[0]);
#pragma GCC diagnostic pop
		TEST_OP("Vector<3> element write", v1(0) = 1.0f);
		TEST_OP("Vector<3> element write direct", v1.data()[0] = 1.0f);
	}

	{
		Eigen::Vector4f v(0.0f, 0.0f, 0.0f, 0.0f);
		Eigen::Vector4f v1(1.0f, 2.0f, 0.0f, -1.0f);
		Eigen::Vector4f v2(1.0f, -1.0f, 2.0f, 0.0f);
		Eigen::Vector4f vres;
		float data[4] = {1.0f, 2.0f, 3.0f, 4.0f};
		TEST_OP("Constructor Vector<4>()", Eigen::Vector4f v3);
		TEST_OP("Constructor Vector<4>(Vector<4>)", Eigen::Vector4f v3(v1));
		TEST_OP("Constructor Vector<4>(float[])", Eigen::Vector4f v3(data));
		TEST_OP("Constructor Vector<4>(float, float, float, float)", Eigen::Vector4f v3(1.0f, 2.0f, 3.0f, 4.0f));
		TEST_OP("Vector<4> = Vector<4>", v = v1);
		TEST_OP("Vector<4> + Vector<4>", v + v1);
		TEST_OP("Vector<4> - Vector<4>", v - v1);
		TEST_OP("Vector<4> += Vector<4>", v += v1);
		TEST_OP("Vector<4> -= Vector<4>", v -= v1);
		TEST_OP("Vector<4> dot Vector<4>", v.dot(v1));
	}

	{
		Eigen::Vector10f v1;
		v1.Zero();
		float data[10];
		TEST_OP("Constructor Vector<10>()", Eigen::Vector10f v3);
		TEST_OP("Constructor Vector<10>(Vector<10>)", Eigen::Vector10f v3(v1));
		TEST_OP("Constructor Vector<10>(float[])", Eigen::Vector10f v3(data));
	}

	{
		Eigen::Matrix3f m1;
		m1.setIdentity();
		Eigen::Matrix3f m2;
		m2.setIdentity();
		Eigen::Vector3f v1(1.0f, 2.0f, 0.0f);
		TEST_OP("Matrix3f * Vector3f", m1 * v1);
		TEST_OP("Matrix3f + Matrix3f", m1 + m2);
		TEST_OP("Matrix3f * Matrix3f", m1 * m2);
	}

	{
		Eigen::Matrix<float, 10, 10> m1;
		m1.setIdentity();
		Eigen::Matrix<float, 10, 10> m2;
		m2.setIdentity();
		Eigen::Vector10f v1;
//.........这里部分代码省略.........

double RobustViconTracker::angle_between(const Eigen::Vector3f& v1, const Eigen::Vector3f& v2)
{
  return acos(v1.dot(v2) / (v1.norm() * v2.norm()));
}

  CloudPtr
      get ()
      {
        //lock while we swap our cloud and reset it.
        boost::mutex::scoped_lock lock (mtx_);
        CloudPtr temp_cloud (new Cloud);
        CloudPtr temp_cloud2 (new Cloud);
        CloudPtr temp_cloud3 (new Cloud);
        CloudPtr temp_cloud4 (new Cloud);
        CloudPtr temp_cloud5 (new Cloud);
        CloudConstPtr empty_cloud;


        cout << "===============================\n"
                "======Start of frame===========\n"
                "===============================\n";
        //cerr << "cloud size orig: " << cloud_->size() << endl;
        voxel_grid.setInputCloud (cloud_);
        voxel_grid.filter (*temp_cloud);  // filter cloud for z depth

        //cerr << "cloud size postzfilter: " << temp_cloud->size() << endl;

        pcl::ModelCoefficients::Ptr planecoefficients (new pcl::ModelCoefficients ());
        pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices ());
        std::vector<pcl::ModelCoefficients> linecoefficients1;
        pcl::ModelCoefficients model;
        model.values.resize (6);
        pcl::PointIndices::Ptr line_inliers (new pcl::PointIndices ());
        std::vector<Eigen::Vector3f> corners;

        if(temp_cloud->size() > MIN_CLOUD_POINTS) {
          plane_seg.setInputCloud (temp_cloud);
          plane_seg.segment (*plane_inliers, *planecoefficients); // find plane
        }

        //cerr << "plane inliers size: " << plane_inliers->indices.size() << endl;

        cout << "planecoeffs: " 
            << planecoefficients->values[0]  << " "
            << planecoefficients->values[1]  << " "
            << planecoefficients->values[2]  << " "
            << planecoefficients->values[3]  << " "
            << endl;

        Eigen::Vector3f pn = Eigen::Vector3f(
            planecoefficients->values[0],
            planecoefficients->values[1],
            planecoefficients->values[2]);

        float planedeg = pcl::rad2deg(acos(pn.dot(Eigen::Vector3f::UnitZ())));
        cout << "angle of camera to floor normal: " << planedeg << " degrees" << endl;
        cout << "distance of camera to floor: " << planecoefficients->values[3]
            << " meters" <<  endl;

        plane_extract.setNegative (true); 
        plane_extract.setInputCloud (temp_cloud);
        plane_extract.setIndices (plane_inliers);
        plane_extract.filter (*temp_cloud2);   // remove plane
        plane_extract.setNegative (false); 
        plane_extract.filter (*temp_cloud5);   // only plane

        for(size_t i = 0; i < temp_cloud5->size (); ++i)
        {
          temp_cloud5->points[i].r = 0; 
          temp_cloud5->points[i].g = 255; 
          temp_cloud5->points[i].b = 0; 
          // tint found plane green for ground
        }

        for(size_t j = 0 ; j < MAX_LINES && temp_cloud2->size() > MIN_CLOUD_POINTS; j++) 
          // look for x lines until cloud gets too small
        {
//          cerr << "cloud size: " << temp_cloud2->size() << endl;

          line_seg.setInputCloud (temp_cloud2);
          line_seg.segment (*line_inliers, model); // find line

 //         cerr << "line inliears size: " << line_inliers->indices.size() << endl;

          if(line_inliers->indices.size() < MIN_CLOUD_POINTS)
            break;

          linecoefficients1.push_back (model);  // store line coeffs

          line_extract.setNegative (true); 
          line_extract.setInputCloud (temp_cloud2);
          line_extract.setIndices (line_inliers);
          line_extract.filter (*temp_cloud3);  // remove plane
          line_extract.setNegative (false); 
          line_extract.filter (*temp_cloud4);  // only plane
          for(size_t i = 0; i < temp_cloud4->size (); ++i) {
            temp_cloud4->points[i].g = 0; 
            if(j%2) {
              temp_cloud4->points[i].r = 255-j*int(255/MAX_LINES); 
              temp_cloud4->points[i].b = 0+j*int(255/MAX_LINES); 
            } else {
              temp_cloud4->points[i].b = 255-j*int(255/MAX_LINES); 
              temp_cloud4->points[i].r = 0+j*int(255/MAX_LINES); 
            }
          }
//.........这里部分代码省略.........

template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::computePoint (
    size_t index, const pcl::PointCloud<PointNT> &normals, float rf[9], std::vector<float> &desc)
{
  // The RF is formed as this x_axis | y_axis | normal
  Eigen::Map<Eigen::Vector3f> x_axis (rf);
  Eigen::Map<Eigen::Vector3f> y_axis (rf + 3);
  Eigen::Map<Eigen::Vector3f> normal (rf + 6);

  // Find every point within specified search_radius_
  std::vector<int> nn_indices;
  std::vector<float> nn_dists;
  const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
  if (neighb_cnt == 0)
  {
    for (size_t i = 0; i < desc.size (); ++i)
      desc[i] = std::numeric_limits<float>::quiet_NaN ();

    memset (rf, 0, sizeof (rf[0]) * 9);
    return (false);
  }

  float minDist = std::numeric_limits<float>::max ();
  int minIndex = -1;
  for (size_t i = 0; i < nn_indices.size (); i++)
  {
	  if (nn_dists[i] < minDist)
	  {
      minDist = nn_dists[i];
      minIndex = nn_indices[i];
	  }
  }
  
  // Get origin point
  Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();
  // Get origin normal
  // Use pre-computed normals
  normal = normals[minIndex].getNormalVector3fMap ();

  // Compute and store the RF direction
  x_axis[0] = static_cast<float> (rnd ());
  x_axis[1] = static_cast<float> (rnd ());
  x_axis[2] = static_cast<float> (rnd ());
  if (!pcl::utils::equal (normal[2], 0.0f))
    x_axis[2] = - (normal[0]*x_axis[0] + normal[1]*x_axis[1]) / normal[2];
  else if (!pcl::utils::equal (normal[1], 0.0f))
    x_axis[1] = - (normal[0]*x_axis[0] + normal[2]*x_axis[2]) / normal[1];
  else if (!pcl::utils::equal (normal[0], 0.0f))
    x_axis[0] = - (normal[1]*x_axis[1] + normal[2]*x_axis[2]) / normal[0];

  x_axis.normalize ();

  // Check if the computed x axis is orthogonal to the normal
  assert (pcl::utils::equal (x_axis[0]*normal[0] + x_axis[1]*normal[1] + x_axis[2]*normal[2], 0.0f, 1E-6f));

  // Store the 3rd frame vector
  y_axis = normal.cross (x_axis);

  // For each point within radius
  for (size_t ne = 0; ne < neighb_cnt; ne++)
  {
    if (pcl::utils::equal (nn_dists[ne], 0.0f))
		  continue;
    // Get neighbours coordinates
    Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();

    /// ----- Compute current neighbour polar coordinates -----
    /// Get distance between the neighbour and the origin
    float r = sqrt (nn_dists[ne]); 
    
    /// Project point into the tangent plane
    Eigen::Vector3f proj;
    pcl::geometry::project (neighbour, origin, normal, proj);
    proj -= origin;

    /// Normalize to compute the dot product
    proj.normalize ();
    
    /// Compute the angle between the projection and the x axis in the interval [0,360] 
    Eigen::Vector3f cross = x_axis.cross (proj);
    float phi = pcl::rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
    phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi;
    /// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180]
    Eigen::Vector3f no = neighbour - origin;
    no.normalize ();
    float theta = normal.dot (no);
    theta = pcl::rad2deg (acosf (std::min (1.0f, std::max (-1.0f, theta))));

    // Bin (j, k, l)
    size_t j = 0;
    size_t k = 0;
    size_t l = 0;

    // Compute the Bin(j, k, l) coordinates of current neighbour
    for (size_t rad = 1; rad < radius_bins_+1; rad++) 
    {
      if (r <= radii_interval_[rad]) 
      {
        j = rad-1;
        break;
//.........这里部分代码省略.........

//.........这里部分代码省略.........

	pcl::copyPointCloud(*cloud_filtered, *cloud_filteredRGB);
	if (visualizationFlag)
		{
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> color9(cloudRGB, 0, 0, 255);
	viewer->addPointCloud<pcl::PointXYZRGB>(cloud_filteredRGB,color9,"LinesFiltered",viewP(LinesFiltered));
	viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "LinesFiltered");
	viewer->addText ("LinesFiltered", 10, 10, fontsize, 1, 1, 1, "LinesFiltered text", viewP(LinesFiltered));
		}


	// Grid Minimum
//	pcl::PointCloud<pcl::PointXYZI>::Ptr gridCloud(new pcl::PointCloud<pcl::PointXYZI>);
//	pcl::GridMinimum<pcl::PointXYZI> gridm(1.0); // Set grid resolution
//	gridm.setInputCloud(cloud_filtered);
//	gridm.filter(*gridCloud);

	//*** Transform point cloud to adjust for a Ground Plane ***//
	pcl::ModelCoefficients ground_coefficients;
	pcl::PointIndices ground_indices;
	pcl::SACSegmentation<pcl::PointXYZI> ground_finder;
	ground_finder.setOptimizeCoefficients(true);
	ground_finder.setModelType(pcl::SACMODEL_PLANE);
	ground_finder.setMethodType(pcl::SAC_RANSAC);
	ground_finder.setDistanceThreshold(0.15);
	ground_finder.setInputCloud(cloud_filtered);
	ground_finder.segment(ground_indices, ground_coefficients);

	// Convert plane normal vector from type ModelCoefficients to Vector4f
	Eigen::Vector3f np = Eigen::Vector3f(ground_coefficients.values[0],ground_coefficients.values[1],ground_coefficients.values[2]);
	// Find rotation vector, u, and angle, theta
	Eigen::Vector3f u = np.cross(Eigen::Vector3f(0,0,1));
	u.normalize();
	float theta = acos(np.dot(Eigen::Vector3f(0,0,1)));
	// Construct transformation matrix (rotation matrix from axis and angle)
	Eigen::Affine3f tf = Eigen::Affine3f::Identity();
	float d = ground_coefficients.values[3];
	tf.translation() << d*np[0], d*np[1], d*np[2];
	tf.rotate (Eigen::AngleAxisf (theta, u));
	// Execute the transformation
	pcl::PointCloud<pcl::PointXYZI>::Ptr transformedCloud (new pcl::PointCloud<pcl::PointXYZI> ());
	pcl::transformPointCloud(*cloud_filtered, *transformedCloud, tf);


	// Compute statistical moments for least significant direction in neighborhood
#if (OLD_METHOD)
	pcl::NeighborhoodFeatures<pcl::PointXYZI, AllFeatures> features;
	pcl::PointCloud<AllFeatures>::Ptr featureCloud(new pcl::PointCloud<AllFeatures>);
	features.setInputCloud(transformedCloud);
	pcl::search::KdTree<pcl::PointXYZI>::Ptr search_tree5( new pcl::search::KdTree<pcl::PointXYZI>());
	features.setSearchMethod(search_tree5);
	//principalComponentsAnalysis.setRadiusSearch(0.6);
	//features.setKSearch(30);
	features.setRadiusSearch(0.01); // This value does not do anything! Look inside "NeighborgoodFeatures.h" to see adaptive radius calculation.
	features.compute(*featureCloud);

	//	ros::Time tFeatureCalculation2 = ros::Time::now();
	//	ros::Duration tFeatureCalculation = tFeatureCalculation2-tPreprocessing2;
	//	if (executionTimes==true)
	//		ROS_INFO("Feature calculation time = %i",tFeatureCalculation.nsec);



	visualizeFeature(*cloudRGB, *featureCloud, 0, viewP(GPDistMean), "GPDistMean");
	visualizeFeature(*cloudRGB, *featureCloud, 1, viewP(GPDistMin), "GPDistMin");
	visualizeFeature(*cloudRGB, *featureCloud, 2, viewP(GPDistPoint), "GPDistPoint");

template <typename PointInT, typename PointOutT> void
pcl::ROPSEstimation <PointInT, PointOutT>::computeLRF (const PointInT& point, const std::set <unsigned int>& local_triangles, Eigen::Matrix3f& lrf_matrix) const
{
  const unsigned int number_of_triangles = static_cast <unsigned int> (local_triangles.size ());

  std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > scatter_matrices (number_of_triangles);
  std::vector <float> triangle_area (number_of_triangles);
  std::vector <float> distance_weight (number_of_triangles);

  float total_area = 0.0f;
  const float coeff = 1.0f / 12.0f;
  const float coeff_1_div_3 = 1.0f / 3.0f;

  Eigen::Vector3f feature_point (point.x, point.y, point.z);

  unsigned int i_triangle = 0;
  for (auto it = local_triangles.cbegin (); it != local_triangles.cend (); it++, i_triangle++)
  {
    Eigen::Vector3f pt[3];
    for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
    {
      const unsigned int index = triangles_[*it].vertices[i_vertex];
      pt[i_vertex] (0) = surface_->points[index].x;
      pt[i_vertex] (1) = surface_->points[index].y;
      pt[i_vertex] (2) = surface_->points[index].z;
    }

    const float curr_area = ((pt[1] - pt[0]).cross (pt[2] - pt[0])).norm ();
    triangle_area[i_triangle] = curr_area;
    total_area += curr_area;

    distance_weight[i_triangle] = std::pow (support_radius_ - (feature_point - (pt[0] + pt[1] + pt[2]) * coeff_1_div_3).norm (), 2.0f);

    Eigen::Matrix3f curr_scatter_matrix;
    curr_scatter_matrix.setZero ();
    for (const auto &i_pt : pt)
    {
      Eigen::Vector3f vec = i_pt - feature_point;
      curr_scatter_matrix += vec * (vec.transpose ());
      for (const auto &j_pt : pt)
        curr_scatter_matrix += vec * ((j_pt - feature_point).transpose ());
    }
    scatter_matrices[i_triangle] = coeff * curr_scatter_matrix;
  }

  if (std::abs (total_area) < std::numeric_limits <float>::epsilon ())
    total_area = 1.0f / total_area;
  else
    total_area = 1.0f;

  Eigen::Matrix3f overall_scatter_matrix;
  overall_scatter_matrix.setZero ();
  std::vector<float> total_weight (number_of_triangles);
  const float denominator = 1.0f / 6.0f;
  for (unsigned int i_triangle = 0; i_triangle < number_of_triangles; i_triangle++)
  {
    float factor = distance_weight[i_triangle] * triangle_area[i_triangle] * total_area;
    overall_scatter_matrix += factor * scatter_matrices[i_triangle];
    total_weight[i_triangle] = factor * denominator;
  }

  Eigen::Vector3f v1, v2, v3;
  computeEigenVectors (overall_scatter_matrix, v1, v2, v3);

  float h1 = 0.0f;
  float h3 = 0.0f;
  i_triangle = 0;
  for (auto it = local_triangles.cbegin (); it != local_triangles.cend (); it++, i_triangle++)
  {
    Eigen::Vector3f pt[3];
    for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
    {
      const unsigned int index = triangles_[*it].vertices[i_vertex];
      pt[i_vertex] (0) = surface_->points[index].x;
      pt[i_vertex] (1) = surface_->points[index].y;
      pt[i_vertex] (2) = surface_->points[index].z;
    }

    float factor1 = 0.0f;
    float factor3 = 0.0f;
    for (const auto &i_pt : pt)
    {
      Eigen::Vector3f vec = i_pt - feature_point;
      factor1 += vec.dot (v1);
      factor3 += vec.dot (v3);
    }
    h1 += total_weight[i_triangle] * factor1;
    h3 += total_weight[i_triangle] * factor3;
  }

  if (h1 < 0.0f) v1 = -v1;
  if (h3 < 0.0f) v3 = -v3;

  v2 = v3.cross (v1);

  lrf_matrix.row (0) = v1;
  lrf_matrix.row (1) = v2;
  lrf_matrix.row (2) = v3;
}

// Calculate the distance between the point and the plane
static double GetDistanceToPlane(const pcl::PointXYZ& point, const Eigen::Vector3f& normalVector, double d)
{
	Eigen::Vector3f t;
	t << point.x, point.y, point.z;
	return fabs(t.dot(normalVector) + d);
}

bool SimpleRayCaster::intersectRayTriangle(const Eigen::Vector3f ray,
        const Eigen::Vector3f a, const Eigen::Vector3f b, const Eigen::Vector3f c,
        Eigen::Vector3f& isec)
{

    /* As discribed by:
     * http://geomalgorithms.com/a06-_intersect-2.html#intersect_RayTriangle%28%29 */

    const Eigen::Vector3f p(0,0,0);
    const Eigen::Vector3f u = b - a;
    const Eigen::Vector3f v = c - a;
    const Eigen::Vector3f n = u.cross(v);
    const float n_dot_ray = n.dot(ray);

    if (std::fabs(n_dot_ray) < 1e-9) {
        return false;
    }


    const float r = n.dot(a-p) / n_dot_ray;

    if (r < 0) {
        return false;
    }

    // the ray intersection point
    isec = ray * r;

    const Eigen::Vector3f w = p + r * ray - a;
    const float denominator = u.dot(v) * u.dot(v) - u.dot(u) * v.dot(v);
    const float s_numerator = u.dot(v) * w.dot(v) - v.dot(v) * w.dot(u);
    const float s = s_numerator / denominator;
    if (s < 0 || s > 1) {
        return false;
    }

    const float t_numerator = u.dot(v) * w.dot(u) - u.dot(u) * w.dot(v);
    const float t = t_numerator / denominator;
    if (t < 0 || s+t > 1) {
        return false;
    }

    return true;
}

bool is_inlier(const Eigen::Vector3f& point, const Eigen::Vector4f plane, double threshold)
{
    return fabs(point.dot(plane.segment<3>(0)) + plane(3)) < threshold;
}

//.........这里部分代码省略.........
	std::cout << "Done" << std::endl;
	//std::cout << referencePointsIndices->indices.size() << std::endl;

	//////////////
	// Votation //
	//////////////

	std::cout<< "\tVotation... ";

	omp_set_num_threads(omp_get_num_procs());
	//omp_set_num_threads(1);
	//int iteration=0;

        bestPoses.clear();
	#pragma omp parallel for private(alpha,alphaBin,alphaScene,sameFeatureIt,index,feature,si,_pointTwoTransformed) //reduction(+:iteration)  //nowait
	for(unsigned int sr=0; sr < referencePointsIndices->indices.size(); ++sr)
	{
	
		//++iteration;
		//std::cout << "iteration: " << iteration << " thread:" << omp_get_thread_num() << std::endl;
		//printf("Hello from thread %d, nthreads %d\n", omp_get_thread_num(), omp_get_num_threads());
		scenePoint=cloudWithNormalsDownSampled->points[referencePointsIndices->indices[sr]].getVector3fMap();
		sceneNormal=cloudWithNormalsDownSampled->points[referencePointsIndices->indices[sr]].getNormalVector3fMap();

		// Get transformation from scene frame to global frame
		Eigen::Vector3f cross=sceneNormal.cross (Eigen::Vector3f::UnitX ()). normalized();

		Eigen::Affine3f rotationSceneToGlobal;
		if(isnan(cross[0]))
		{
			rotationSceneToGlobal=Eigen::AngleAxisf(0.0,Eigen::Vector3f::UnitX ());
		}
		else
			rotationSceneToGlobal=Eigen::AngleAxisf(acosf (sceneNormal.dot (Eigen::Vector3f::UnitX ())),cross);

		Eigen::Affine3f transformSceneToGlobal = Eigen::Translation3f ( rotationSceneToGlobal* ((-1)*scenePoint)) * rotationSceneToGlobal;

		//////////////////////
		// Choose best pose //
		//////////////////////

		// Reset pose accumulator
		for(std::vector<std::vector<int> >::iterator accumulatorIt=accumulatorParallelAux[omp_get_thread_num()].begin();accumulatorIt < accumulatorParallelAux[omp_get_thread_num()].end(); ++accumulatorIt)
		{
			std::fill(accumulatorIt->begin(),accumulatorIt->end(),0); 
		}
		

		//std::cout << std::endl;
		for(si=cloudWithNormalsDownSampled->begin(); si < cloudWithNormalsDownSampled->end();++si)
		{
			// if same point, skip point pair
			if( (cloudWithNormalsDownSampled->points[referencePointsIndices->indices[sr]].x==si->x) && (cloudWithNormalsDownSampled->points[referencePointsIndices->indices[sr]].y==si->y) && (cloudWithNormalsDownSampled->points[referencePointsIndices->indices[sr]].z==si->z))
			{
				//std::cout << si->x << " " << si->y << " " << si->z << std::endl;
				continue;
			}	

			// Compute PPF
			pointPairSV PPF=pointPairSV(cloudWithNormalsDownSampled->points[sr],*si, transformSceneToGlobal);

			// Compute index
			index=PPF.getHash(*si,model->distanceStepInverted);

			// If distance between point pairs is bigger than the maximum for this model, skip point pair
			if(index>pointPairSV::maxHash)

int main(int argc, char *argv[]){
    if(argc>1){
        CloudPtr cloud (new Cloud);
        ColorCloudPtr color_cloud (new ColorCloud);
        if (pcl::io::loadPCDFile(argv[1], *cloud) == -1) //* load the file
        {
            PCL_ERROR ("Couldn't read file.\n");
            return -1;
        }
        for(int i = 0; i<cloud->size(); i++){
            Point p1 = cloud->points[i];
            ColorPoint p2;
            p2.x = p1.x;
            p2.y = p1.y;
            p2.z = p1.z;
            p2.r = 0;
            p2.g = 0;
            p2.b = 0;
            color_cloud->push_back(p2);
        }
        
        float centroid[3];
        calculateCentroid(cloud,centroid);


        pcl::PolygonMesh pm;
        pcl::ConvexHull<ColorPoint> chull;
        chull.setInputCloud(color_cloud);
        chull.reconstruct(pm);

        pcl::fromROSMsg(pm.cloud,*color_cloud);
        vector<float> distances(color_cloud->size(),999999);

        for(int i = 0; i<pm.polygons.size(); i++){
            int i0 = pm.polygons[i].vertices[0];
            int i1 = pm.polygons[i].vertices[1];
            int i2 = pm.polygons[i].vertices[2];
            ColorPoint p0 = color_cloud->points[i0];
            ColorPoint p1 = color_cloud->points[i1];
            ColorPoint p2 = color_cloud->points[i2];

            Eigen::Vector3f v0;
            Eigen::Vector3f v1;
            Eigen::Vector3f v2;
            v0 << p0.x,p0.y,p0.z;
            v1 << p1.x,p1.y,p1.z;
            v2 << p2.x,p2.y,p2.z;
            Eigen::Vector3f normal = (v1-v0).cross(v2-v0).normalized();

            Eigen::Vector3f p;
            p << centroid[0], centroid[1], centroid[2];

            Eigen::Vector3f to_p = p-v0;
            Eigen::Vector3f projected_p = p-(normal* normal.dot(to_p));

            float dist0 = (v1-v0).dot(projected_p-v0)/(v1-v0).norm();
            float dist1 = (v2-v0).dot(projected_p-v0)/(v2-v0).norm();
            distances[i0] = min(distances[i0],(dist0+dist1));
            distances[i1] = min(distances[i1],(dist0+dist1));
            distances[i2] = min(distances[i2],(dist0+dist1));
        }
        float max_dist = *max_element(distances.begin(),distances.end());

        float thresh = 500;
        for(int i = 0; i<color_cloud->size(); i++){
            ColorPoint* p1 = &color_cloud->points[i];
            float color = 255 - distances[i]*(255/max_dist);
            if(distances[i]>thresh)
                color = 0;
            p1->r = color;
            p1->g = 0;
            p1->b = 0;
        }

        chull.setInputCloud(color_cloud);
        chull.reconstruct(pm);

        pcl::io::saveVTKFile("hull.vtk",pm);
    }
}

void SaveClustersWorker::doWork(const QString &filename)
{
    bool is_success(false);

    QByteArray ba = filename.toLocal8Bit();
    string* strfilename = new string(ba.data());

    dataLibrary::Status = STATUS_SAVECLUSTERS;

    dataLibrary::start = clock();

    //begin of processing
    //compute centor point and normal
    float nx_all, ny_all, nz_all;
    float curvature_all;
    Eigen::Matrix3f convariance_matrix_all;
    Eigen::Vector4f xyz_centroid_all, plane_parameters_all;
    pcl::compute3DCentroid(*dataLibrary::cloudxyz, xyz_centroid_all);
    pcl::computeCovarianceMatrix(*dataLibrary::cloudxyz, xyz_centroid_all, convariance_matrix_all);
    pcl::solvePlaneParameters(convariance_matrix_all, nx_all, ny_all, nz_all, curvature_all);
    Eigen::Vector3f centroid_all;
    dataLibrary::plane_normal_all(0)=nx_all;
    dataLibrary::plane_normal_all(1)=ny_all;
    dataLibrary::plane_normal_all(2)=nz_all;
    centroid_all(0)=xyz_centroid_all(0);
    centroid_all(1)=xyz_centroid_all(1);
    centroid_all(2)=xyz_centroid_all(2);

    //calculate total surface roughness of outcrop
    float total_distance=0.0;
    for(int i=0; i<dataLibrary::cloudxyz->size(); i++)
    {
        Eigen::Vector3f Q;
        Q(0)=dataLibrary::cloudxyz->at(i).x;
        Q(1)=dataLibrary::cloudxyz->at(i).y;
        Q(2)=dataLibrary::cloudxyz->at(i).z;
        total_distance+=std::abs((Q-centroid_all).dot(dataLibrary::plane_normal_all)/std::sqrt((dataLibrary::plane_normal_all.dot(dataLibrary::plane_normal_all))));
    }
    float roughness=total_distance/dataLibrary::cloudxyz->size();

    //project all points
    pcl::ModelCoefficients::Ptr coefficients_all (new pcl::ModelCoefficients());
    coefficients_all->values.resize(4);
    coefficients_all->values[0] = nx_all;
    coefficients_all->values[1] = ny_all;
    coefficients_all->values[2] = nz_all;
    coefficients_all->values[3] = - (nx_all*xyz_centroid_all[0] + ny_all*xyz_centroid_all[1] + nz_all*xyz_centroid_all[2]);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected_all (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ProjectInliers<pcl::PointXYZ> proj_all;
    proj_all.setModelType(pcl::SACMODEL_PLANE);
    proj_all.setInputCloud(dataLibrary::cloudxyz);
    proj_all.setModelCoefficients(coefficients_all);
    proj_all.filter(*cloud_projected_all);

    //compute convex hull
    pcl::ConvexHull<pcl::PointXYZ> chull_all;
    chull_all.setInputCloud(cloud_projected_all);
    chull_all.reconstruct(*dataLibrary::cloud_hull_all);

    /*//compute concave hull
    pcl::ConcaveHull<pcl::PointXYZ> chull_all;
    chull_all.setInputCloud(cloud_projected_all);
    chull_all.setAlpha(0.1);
    chull_all.reconstruct(*dataLibrary::cloud_hull_all);*/

    //compute area
    float area_all = 0.0f;
    int num_points_all = dataLibrary::cloud_hull_all->size();
    int j = 0;
    Eigen::Vector3f va_all, vb_all, res_all;
    res_all(0) = res_all(1) = res_all(2) = 0.0f;
    for(int i = 0; i < num_points_all; i++)
    {
        j = (i+1) % num_points_all;
        va_all = dataLibrary::cloud_hull_all->at(i).getVector3fMap();
        vb_all = dataLibrary::cloud_hull_all->at(j).getVector3fMap();
        res_all += va_all.cross(vb_all);
    }
    area_all = fabs(res_all.dot(dataLibrary::plane_normal_all) * 0.5);

    //initial total length of fracture traces
    float total_length=0.0;
    //initial over estimate length of fracture traces
    float error_length=0.0;
    //initial total displacement
    float total_displacement=0.0;
    //initial mean dip2plane angle
    float total_dip2plane=0.0;
    int inside_num=0;

    string textfilename = strfilename->substr(0, strfilename->size()-4) += "_table.txt";
    string dip_dipdir_file = strfilename->substr(0, strfilename->size()-4) += "_dip_dipdir.txt";
    string dipdir_dip_file = strfilename->substr(0, strfilename->size()-4) += "_dipdir_dip.txt";
    string fracture_intensity = strfilename->substr(0, strfilename->size()-4) += "_fracture_intensity.txt";
    ofstream fout(textfilename.c_str());
    ofstream dip_dipdir_out(dip_dipdir_file.c_str());
    ofstream dipdir_dip_out(dipdir_dip_file.c_str());
    ofstream fracture_intensity_out(fracture_intensity.c_str());
    fout<<"Flag"<<"\t"<<"Number"<<"\t"<<"Points"<<"\t"<<"Direc"<<"\t"<<"Dip"<<"\t"<<"Area"<<"\t"<<"Length"<<"\t"<<"Roughness"<<"\n";
//.........这里部分代码省略.........

    bool operator() (const Item& item) {

      dirs_vox.index(0) = item.pos[0];
      dirs_vox.index(1) = item.pos[1];
      dirs_vox.index(2) = item.pos[2];

      if (check_input (item)) {
        for (auto l = Loop(3) (dirs_vox); l; ++l)
          dirs_vox.value() = NAN;
        return true;
      }

      std::vector<Direction> all_peaks;

      for (size_t i = 0; i < size_t(dirs.rows()); i++) {
        Direction p (dirs (i,0), dirs (i,1));
        p.a = Math::SH::get_peak (item.data, lmax, p.v);
        if (std::isfinite (p.a)) {
          for (size_t j = 0; j < all_peaks.size(); j++) {
            if (std::abs (p.v.dot (all_peaks[j].v)) > DOT_THRESHOLD) {
              p.a = NAN;
              break;
            }
          }
        }
        if (std::isfinite (p.a) && p.a >= threshold) 
          all_peaks.push_back (p);
      }

      if (ipeaks_vox) {
        ipeaks_vox->index(0) = item.pos[0];
        ipeaks_vox->index(1) = item.pos[1];
        ipeaks_vox->index(2) = item.pos[2];

        for (int i = 0; i < npeaks; i++) {
          Eigen::Vector3f p;
          ipeaks_vox->index(3) = 3*i;
          for (int n = 0; n < 3; n++) {
            p[n] = ipeaks_vox->value();
            ipeaks_vox->index(3)++;
          }
          p.normalize();

          value_type mdot = 0.0;
          for (size_t n = 0; n < all_peaks.size(); n++) {
            value_type f = std::abs (p.dot (all_peaks[n].v));
            if (f > mdot) {
              mdot = f;
              peaks_out[i] = all_peaks[n];
            }
          }
        }
      }
      else if (true_peaks.size()) {
        for (int i = 0; i < npeaks; i++) {
          value_type mdot = 0.0;
          for (size_t n = 0; n < all_peaks.size(); n++) {
            value_type f = std::abs (all_peaks[n].v.dot (true_peaks[i].v));
            if (f > mdot) {
              mdot = f;
              peaks_out[i] = all_peaks[n];
            }
          }
        }
      }
      else std::partial_sort_copy (all_peaks.begin(), all_peaks.end(), peaks_out.begin(), peaks_out.end());

      int actual_npeaks = std::min (npeaks, (int) all_peaks.size());
      dirs_vox.index(3) = 0;
      for (int n = 0; n < actual_npeaks; n++) {
        dirs_vox.value() = peaks_out[n].a*peaks_out[n].v[0];
        dirs_vox.index(3)++;
        dirs_vox.value() = peaks_out[n].a*peaks_out[n].v[1];
        dirs_vox.index(3)++;
        dirs_vox.value() = peaks_out[n].a*peaks_out[n].v[2];
        dirs_vox.index(3)++;
      }
      for (; dirs_vox.index(3) < 3*npeaks; dirs_vox.index(3)++) dirs_vox.value() = NAN;

      return true;
    }

//.........这里部分代码省略.........
        }
        else
        {
            eFusion->predict();
        }

        TICK("GUI");

        if(gui->followPose->Get())
        {
            pangolin::OpenGlMatrix mv;

            Eigen::Matrix4f currPose = eFusion->getCurrPose();
            Eigen::Matrix3f currRot = currPose.topLeftCorner(3, 3);

            Eigen::Quaternionf currQuat(currRot);
            Eigen::Vector3f forwardVector(0, 0, 1);
            Eigen::Vector3f upVector(0, iclnuim ? 1 : -1, 0);

            Eigen::Vector3f forward = (currQuat * forwardVector).normalized();
            Eigen::Vector3f up = (currQuat * upVector).normalized();

            Eigen::Vector3f eye(currPose(0, 3), currPose(1, 3), currPose(2, 3));

            eye -= forward;

            Eigen::Vector3f at = eye + forward;

            Eigen::Vector3f z = (eye - at).normalized();  // Forward
            Eigen::Vector3f x = up.cross(z).normalized(); // Right
            Eigen::Vector3f y = z.cross(x);

            Eigen::Matrix4d m;
            m << x(0),  x(1),  x(2),  -(x.dot(eye)),
                 y(0),  y(1),  y(2),  -(y.dot(eye)),
                 z(0),  z(1),  z(2),  -(z.dot(eye)),
                    0,     0,     0,              1;

            memcpy(&mv.m[0], m.data(), sizeof(Eigen::Matrix4d));

            gui->s_cam.SetModelViewMatrix(mv);
        }

        gui->preCall();

        std::stringstream stri;
        stri << eFusion->getModelToModel().lastICPCount;
        gui->trackInliers->Ref().Set(stri.str());

        std::stringstream stre;
        stre << (isnan(eFusion->getModelToModel().lastICPError) ? 0 : eFusion->getModelToModel().lastICPError);
        gui->trackRes->Ref().Set(stre.str());

        if(!gui->pause->Get())
        {
            gui->resLog.Log((isnan(eFusion->getModelToModel().lastICPError) ? std::numeric_limits<float>::max() : eFusion->getModelToModel().lastICPError), icpErrThresh);
            gui->inLog.Log(eFusion->getModelToModel().lastICPCount, icpCountThresh);
        }

        Eigen::Matrix4f pose = eFusion->getCurrPose();

        if(gui->drawRawCloud->Get() || gui->drawFilteredCloud->Get())
        {
            eFusion->computeFeedbackBuffers();
        }