C++ PointT::getVector3fMap方法代码示例

template <typename PointT> inline PointT
pcl::transformPoint (const PointT &point, const Eigen::Affine3f &transform)
{
  PointT ret = point;
  ret.getVector3fMap () = transform * point.getVector3fMap ();
  return ret;
}

template <typename PointT, typename Scalar> inline PointT
pcl::transformPoint (const PointT &point, 
                     const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
  PointT ret = point;
  ret.getVector3fMap () = transform * point.getVector3fMap ();

  return (ret);
}

template<typename PointT> inline void
pcl::PCA<PointT>::project (const PointT& input, PointT& projection)
{
    if(!compute_done_)
        initCompute ();
    if (!compute_done_)
        PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::project] PCA initCompute failed");

    Eigen::Vector3f demean_input = input.getVector3fMap () - mean_.head<3> ();
    projection.getVector3fMap () = eigenvectors_.transpose() * demean_input;
}

        /** \brief Calculate centroid of voxel.
          * \param[out] centroid_arg the resultant centroid of the voxel 
          */
        void 
        getCentroid (PointT& centroid_arg) const
        {
          using namespace pcl::common;

          if (point_counter_)
          {
            centroid_arg.getVector3fMap() = (pt_ / static_cast<double> (point_counter_)).cast<float>();
            centroid_arg.getNormalVector3fMap() = n_.normalized().cast<float>();
            centroid_arg.r = static_cast<unsigned char>(r_ / point_counter_);
            centroid_arg.g = static_cast<unsigned char>(g_ / point_counter_);
            centroid_arg.b = static_cast<unsigned char>(b_ / point_counter_);
          }
          else
          {
            centroid_arg.x = std::numeric_limits<float>::quiet_NaN();
            centroid_arg.y = std::numeric_limits<float>::quiet_NaN();
            centroid_arg.z = std::numeric_limits<float>::quiet_NaN();
            centroid_arg.normal_x = std::numeric_limits<float>::quiet_NaN();
            centroid_arg.normal_y = std::numeric_limits<float>::quiet_NaN();
            centroid_arg.normal_z = std::numeric_limits<float>::quiet_NaN();
            centroid_arg.r = 0;
            centroid_arg.g = 0;
            centroid_arg.b = 0;
          }
        }

void images_to_cloud(cv::Mat& depth, cv::Mat& rgb, CloudT::Ptr& cloud, const Eigen::Matrix3f& K)
{
    int rows = depth.rows;
    int cols = depth.cols;
    cloud->reserve(rows*cols);
    Eigen::Vector3f pe;
    PointT pc;
    cv::Vec3b prgb;
    Eigen::ColPivHouseholderQR<Eigen::Matrix3f> dec(K);
    for (int x = 0; x < cols; ++x) {
        for (int y = 0; y < rows; ++y) {
            uint16_t d = depth.at<uint16_t>(y, x);
            if (d == 0) {
                continue;
            }
            float df = float(d)/1000.0f;
            pe(0) = x;
            pe(1) = y;
            pe(2) = 1.0f;
            pe = dec.solve(pe);
            pe *= df/pe(2);
            pc.getVector3fMap() = pe;
            prgb = rgb.at<cv::Vec3b>(y, x);
            pc.r = prgb[2];
            pc.g = prgb[1];
            pc.b = prgb[0];
            cloud->push_back(pc);
            //cout << p.getVector3fMap().transpose() << endl;
        }
    }
    //object_retrieval obr("/random");
    //obr.visualize_cloud(cloud);
}

template<typename PointT> int
pcl::search::OrganizedNeighbor<PointT>::radiusSearch (const               PointT &p_q,
                                                       const double        radius,
                                                       std::vector<int>    &k_indices,
                                                       std::vector<float>  &k_sqr_distances,
                                                       unsigned int        max_nn) const
{
  if (projection_matrix_.coeffRef (0) == 0)
  {
    PCL_ERROR ("[pcl::%s::radiusSearch] Invalid projection matrix. Probably input dataset was not organized!\n", getName ().c_str ());
    return (0);
  }

  // NAN test
  if (!pcl_isfinite (p_q.x) || !pcl_isfinite (p_q.y) || !pcl_isfinite (p_q.z))
    return (0);

  // search window
  unsigned left, right, top, bottom;
  //unsigned x, y, idx;
  float squared_distance, squared_radius;

  k_indices.clear ();
  k_sqr_distances.clear ();

  squared_radius = radius * radius;

  this->getProjectedRadiusSearchBox (p_q, squared_radius, left, right, top, bottom);

  // iterate over search box
  if (max_nn == 0 || max_nn >= (unsigned int)input_->points.size ())
    max_nn = input_->points.size ();

  k_indices.reserve (max_nn);
  k_sqr_distances.reserve (max_nn);

  unsigned yEnd  = (bottom + 1) * input_->width + right + 1;
  register unsigned idx  = top * input_->width + left;
  unsigned skip = input_->width - right + left - 1;
  unsigned xEnd = idx - left + right + 1;

  for (; xEnd != yEnd; idx += skip, xEnd += input_->width)
  {
    for (; idx < xEnd; ++idx)
    {
      squared_distance = (input_->points[idx].getVector3fMap () - p_q.getVector3fMap ()).squaredNorm ();
      if (squared_distance <= squared_radius)
      {
        k_indices.push_back (idx);
        k_sqr_distances.push_back (squared_distance);
        // already done ?
        if (k_indices.size () == max_nn)
          return max_nn;
      }
    }
  }
  return (k_indices.size ());
}

void
drawBoundingBox(PointCloudT::Ptr cloud,
				boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer,
				int z)
{
	
	//Eigen::Vector4f centroid;
	pcl::compute3DCentroid(*cloud, centroid);
	
	//Eigen::Matrix3f covariance;
	computeCovarianceMatrixNormalized(*cloud, centroid, covariance);
	//Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance,
	//Eigen::ComputeEigenvectors);

	eigen_solver.compute(covariance,Eigen::ComputeEigenvectors);
	
//	eigen_solver = boost::shared_ptr<Eigen::SelfAdjointEigenSolver>
//		(covariance,Eigen::ComputeEigenvectors);

	eigDx = eigen_solver.eigenvectors();
    eigDx.col(2) = eigDx.col(0).cross(eigDx.col(1));


	//Eigen::Matrix4f p2w(Eigen::Matrix4f::Identity());
	p2w.block<3,3>(0,0) = eigDx.transpose();
	p2w.block<3,1>(0,3) = -1.f * (p2w.block<3,3>(0,0) * centroid.head<3>());
    //pcl::PointCloud<PointT> cPoints;
    pcl::transformPointCloud(*cloud, cPoints, p2w);


	//PointT min_pt, max_pt;
    pcl::getMinMax3D(cPoints, min_pt, max_pt);
    const Eigen::Vector3f mean_diag = 0.5f*(max_pt.getVector3fMap() + min_pt.getVector3fMap());

	const Eigen::Quaternionf qfinal(eigDx);
    const Eigen::Vector3f tfinal = eigDx*mean_diag + centroid.head<3>();
	
	//viewer->addPointCloud(cloud);
	viewer->addCube(tfinal, qfinal, max_pt.x - min_pt.x, max_pt.y - min_pt.y, max_pt.z - min_pt.z,boost::lexical_cast<std::string>((z+1)*200));

}

template<typename PointT> template<unsigned PlaneDim1, unsigned PlaneDim2> bool
pcl::CropHull<PointT>::isPointIn2DPolyWithVertIndices (
  const PointT& point, const Vertices& verts, const PointCloud& cloud)
{
  bool in_poly = false;
  double x1, x2, y1, y2;

  const int nr_poly_points = static_cast<const int>(verts.vertices.size ());
  double xold = cloud[verts.vertices[nr_poly_points - 1]].getVector3fMap ()[PlaneDim1];
  double yold = cloud[verts.vertices[nr_poly_points - 1]].getVector3fMap ()[PlaneDim2];
  for (int i = 0; i < nr_poly_points; i++)
  {
    const double xnew = cloud[verts.vertices[i]].getVector3fMap ()[PlaneDim1];
    const double ynew = cloud[verts.vertices[i]].getVector3fMap ()[PlaneDim2];
    if (xnew > xold)
    {
      x1 = xold;
      x2 = xnew;
      y1 = yold;
      y2 = ynew;
    }
    else
    {
      x1 = xnew;
      x2 = xold;
      y1 = ynew;
      y2 = yold;
    }

    if ((xnew < point.getVector3fMap ()[PlaneDim1]) == (point.getVector3fMap ()[PlaneDim1] <= xold) &&
        (point.getVector3fMap ()[PlaneDim2] - y1) * (x2 - x1) < (y2 - y1) * (point.getVector3fMap ()[PlaneDim1] - x1))
    {
      in_poly = !in_poly;
    }
    xold = xnew;
    yold = ynew;
  }

  return (in_poly);
}

template<typename PointT> bool
pcl::CropHull<PointT>::rayTriangleIntersect (const PointT& point,
                                             const Eigen::Vector3f& ray,
                                             const Vertices& verts,
                                             const PointCloud& cloud)
{
  // Algorithm here is adapted from:
  // http://softsurfer.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle()
  //
  // Original copyright notice:
  // Copyright 2001, softSurfer (www.softsurfer.com)
  // This code may be freely used and modified for any purpose
  // providing that this copyright notice is included with it.
  //
  assert (verts.vertices.size () == 3);

  const Eigen::Vector3f p = point.getVector3fMap ();
  const Eigen::Vector3f a = cloud[verts.vertices[0]].getVector3fMap ();
  const Eigen::Vector3f b = cloud[verts.vertices[1]].getVector3fMap ();
  const Eigen::Vector3f c = cloud[verts.vertices[2]].getVector3fMap ();
  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);

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

template<typename PointT> void
pcl::search::OrganizedNeighbor<PointT>::getProjectedRadiusSearchBox (const PointT& point,
                                                                      float squared_radius,
                                                                      unsigned &minX,
                                                                      unsigned &maxX,
                                                                      unsigned &minY,
                                                                      unsigned &maxY) const
{
  Eigen::Vector3f q = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);

  float a = squared_radius * KR_KRT_.coeff (8) - q [2] * q [2];
  float b = squared_radius * KR_KRT_.coeff (7) - q [1] * q [2];
  float c = squared_radius * KR_KRT_.coeff (4) - q [1] * q [1];
  int min, max;
  // a and c are multiplied by two already => - 4ac -> - ac
  float det = b * b - a * c;
  if (det < 0)
  {
    minY = 0;
    maxY = input_->height - 1;
  }
  else
  {
    min = std::min ((int) floor ((b - sqrt (det)) / a), (int) floor ((b + sqrt (det)) / a));
    max = std::max ((int) ceil ((b - sqrt (det)) / a), (int) ceil ((b + sqrt (det)) / a));
    minY = (unsigned) std::min ((int) input_->height - 1, std::max (0, min));
    maxY = (unsigned) std::max (std::min ((int) input_->height - 1, max), 0);
  }

  b = squared_radius * KR_KRT_.coeff (6) - q [0] * q [2];
  c = squared_radius * KR_KRT_.coeff (0) - q [0] * q [0];

  det = b * b - a * c;
  if (det < 0)
  {
    minX = 0;
    maxX = input_->width - 1;
  }
  else
  {
    min = std::min ((int) floor ((b - sqrt (det)) / a), (int) floor ((b + sqrt (det)) / a));
    max = std::max ((int) ceil ((b - sqrt (det)) / a), (int) ceil ((b + sqrt (det)) / a));
    minX = (unsigned) std::min ((int)input_->width - 1, std::max (0, min));
    maxX = (unsigned) std::max (std::min ((int)input_->width - 1, max), 0);
  }
}

 double ColorizeDistanceFromPlane::distanceToConvexes(
   const PointT& p,
   const std::vector<ConvexPolygon::Ptr>& convexes)
 {
   Eigen::Vector3f v = p.getVector3fMap();
   double min_distance = DBL_MAX;
   for (size_t i = 0; i < convexes.size(); i++) {
     ConvexPolygon::Ptr convex = convexes[i];
     if (!only_projectable_ || convex->isProjectableInside(v)) {
       double d = convex->distanceToPoint(v);
       if (d < min_distance) {
         min_distance = d;
       }
     }
   }
   return min_distance;
 }

template <typename PointT> float
pcl16::search::BruteForce<PointT>::getDistSqr (
    const PointT& point1, const PointT& point2) const
{
  return (point1.getVector3fMap () - point2.getVector3fMap ()).squaredNorm ();
}

void segmentation_callback(const std_msgs::String::ConstPtr& msg)
{
    data_summary.load(data_path);

    boost::filesystem::path sweep_xml(msg->data);
    boost::filesystem::path surfel_path = sweep_xml.parent_path() / "surfel_map.pcd";

    SurfelCloudT::Ptr surfel_cloud(new SurfelCloudT);
    pcl::io::loadPCDFile(surfel_path.string(), *surfel_cloud);

    CloudT::Ptr cloud(new CloudT);
    NormalCloudT::Ptr normals(new NormalCloudT);
    cloud->reserve(surfel_cloud->size());
    normals->reserve(surfel_cloud->size());
    for (const SurfelT& s : surfel_cloud->points) {
        if (s.confidence < threshold) {
            continue;
        }
        PointT p;
        p.getVector3fMap() = s.getVector3fMap();
        p.rgba = s.rgba;
        NormalT n;
        n.getNormalVector3fMap() = s.getNormalVector3fMap();
        cloud->push_back(p);
        normals->push_back(n);
    }

    // we might want to save the map and normals here
    boost::filesystem::path cloud_path = sweep_xml.parent_path() / "cloud.pcd";
    boost::filesystem::path normals_path = sweep_xml.parent_path() / "normals.pcd";
    pcl::io::savePCDFileBinary(cloud_path.string(), *cloud);
    pcl::io::savePCDFileBinary(normals_path.string(), *normals);

    supervoxel_segmentation ss(0.02f, 0.2f, 0.4f, false); // do not filter
    Graph* g;
    Graph* convex_g;
    vector<CloudT::Ptr> supervoxels;
    vector<CloudT::Ptr> convex_segments;
    map<size_t, size_t> indices;
    std::tie(g, convex_g, supervoxels, convex_segments, indices) = ss.compute_convex_oversegmentation(cloud, normals, false);

#if VISUALIZE
    CloudT::Ptr colored_segments(new CloudT);
    colored_segments->reserve(cloud->size());
    int counter = 0;
    for (CloudT::Ptr& c : convex_segments) {
        for (PointT p : c->points) {
            p.r = colormap[counter%24][0];
            p.g = colormap[counter%24][1];
            p.b = colormap[counter%24][2];
            colored_segments->push_back(p);
        }
        ++counter;
    }
    //dynamic_object_retrieval::visualize(colored_segments);
    sensor_msgs::PointCloud2 vis_msg;
    pcl::toROSMsg(*colored_segments, vis_msg);
    vis_msg.header.frame_id = "/map";
    vis_cloud_pub.publish(vis_msg);
#endif

    boost::filesystem::path segments_path = sweep_xml.parent_path() / "convex_segments";
    boost::filesystem::create_directory(segments_path);
    ss.save_graph(*convex_g, (segments_path / "graph.cereal").string());

    delete g;
    delete convex_g;

    dynamic_object_retrieval::sweep_summary summary;
    summary.nbr_segments = convex_segments.size();

    int i = 0;
    vector<string> segment_paths;
    for (CloudT::Ptr& c : convex_segments) {
        std::stringstream ss;
        ss << "segment" << std::setw(4) << std::setfill('0') << i;
        boost::filesystem::path segment_path = segments_path / (ss.str() + ".pcd");
        segment_paths.push_back(segment_path.string());
        pcl::io::savePCDFileBinary(segment_path.string(), *c);
        summary.segment_indices.push_back(i); // counter
        ++i;
    }

    summary.save(segments_path);

    std_msgs::String done_msg;
    done_msg.data = msg->data;
    pub.publish(done_msg);

    data_summary.nbr_sweeps++;
    data_summary.nbr_convex_segments += convex_segments.size();
    data_summary.index_convex_segment_paths.insert(data_summary.index_convex_segment_paths.end(),
                                                   segment_paths.begin(), segment_paths.end());

    data_summary.save(data_path);
}

template<typename PointT, typename LeafT, typename BranchT> float
pcl::octree::OctreePointCloudSearch<PointT, LeafT, BranchT>::pointSquaredDist (const PointT & pointA,
                                                                               const PointT & pointB) const
{
  return (pointA.getVector3fMap () - pointB.getVector3fMap ()).squaredNorm ();
}

template<typename PointT, typename LeafContainerT, typename BranchContainerT> float
pcl::octree::OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::pointSquaredDist (const PointT & point_a,
                                                                               const PointT & point_b) const
{
  return (point_a.getVector3fMap () - point_b.getVector3fMap ()).squaredNorm ();
}