Method computeModel

sloam/src/objects/cylinder.cpp:75–173

}

void Cylinder::computeModel(const std::vector<TreeVertex> &landmarkVtxs,
                            const Scalar defaultTreeRadius, const Scalar featuresPerTree)
{
  TreeVertex firstVtx = landmarkVtxs[2];
  std::vector<float> validRadii;
  CloudT::Ptr tree(new CloudT);

  for (size_t i = 0; i < landmarkVtxs.size(); ++i)
  {
    TreeVertex vtx = landmarkVtxs[i];
    tree->push_back(vtx.coords);

    for (auto &vtxP : vtx.points)
    {
      vtxP.intensity = (float)firstVtx.treeId;
      features.push_back(vtxP);
    }

    model.radii.push_back(vtx.radius);
    if (vtx.points.size() > 3)
    {
      validRadii.push_back(vtx.radius);
    }
  }

  id = firstVtx.treeId;
  auto bottomPt = landmarkVtxs[1].coords;
  auto topPt = landmarkVtxs[landmarkVtxs.size() - 2].coords;

  model.root(0) = bottomPt.x;
  model.root(1) = bottomPt.y;
  model.root(2) = bottomPt.z;

  // if tree is too short, we don't use it
  if (pcl::geometry::squaredDistance(bottomPt, topPt) < 1.5 || tree->size() == 0)
  {
    model.radius = -1;
    return;
  }

  //Create a model parameter object to record the result
  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers(new pcl::PointIndices); //inliers indicate the point where the error can be tolerated, and the sequence number of the point cloud is recorded
  pcl::SACSegmentation<PointT> seg;                      // Create a splitter
  seg.setOptimizeCoefficients(true);                     // Optional, this setting can choose whether the points displayed in the result plane are split points or the remaining points.
  seg.setModelType(pcl::SACMODEL_LINE);                  // Mandatory-set the target geometry
  seg.setMethodType(pcl::SAC_RANSAC);                    //Segmentation method: random sampling method
  seg.setDistanceThreshold(0.25);                        //Set the error tolerance range, which is the threshold
  seg.setInputCloud(tree);                               //Input point cloud
  seg.segment(*inliers, *coefficients);                  //Split point cloud, get plane and normal vector

  if (inliers->indices.size() == 0)
  {
    ROS_DEBUG_STREAM("RANSAC Line Fitting failed");
    model.radius = -1;
    return;
  }