C++ Vector4f::normalize方法代码示例

template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCone<PointT, PointNT>::getDistancesToModel (
    const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
  // Check if the model is valid given the user constraints
  if (!isModelValid (model_coefficients))
  {
    distances.clear ();
    return;
  }

  distances.resize (indices_->size ());

  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);
  // Iterate through the 3d points and calculate the distances from them to the cone
  for (size_t i = 0; i  < indices_->size (); ++i)
  {
    Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
    Eigen::Vector4f n  (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);

    // Calculate the point's projection on the cone axis
    float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
    Eigen::Vector4f pt_proj = apex + k * axis_dir;
    Eigen::Vector4f dir = pt - pt_proj;
    dir.normalize ();

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

    // Calculate the cones perfect normals
    Eigen::Vector4f cone_normal = sinf (opening_angle) * height + cosf (opening_angle) * dir;

    // Aproximate the distance from the point to the cone as the difference between
    // dist(point,cone_axis) and actual cone radius
    double d_euclid = fabs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius);

    // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
    double d_normal = fabs (getAngle3D (n, cone_normal));
    d_normal = (std::min) (d_normal, M_PI - d_normal);

    distances[i] = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
  }
}

template <typename PointT> bool
pcl::SampleConsensusModelParallelPlane<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients)
{
  // Needs a valid model coefficients
  if (model_coefficients.size () != 4)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelParallelPlane::isModelValid] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
    return (false);
  }

  // Check against template, if given
  if (eps_angle_ > 0.0)
  {
    // Obtain the plane normal
    Eigen::Vector4f coeff = model_coefficients;
    coeff[3] = 0;
    coeff.normalize ();

    Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0);
    if (fabs (axis.dot (coeff)) < cos_angle_)
      return  (false);
  }

  return (true);
}

void Plane::transform(Eigen::Matrix4f t){
	Eigen::Vector4f n = t*Eigen::Vector4f(normal_x,normal_y,normal_z,0);//Only use rotational component
	n.normalize();
	normal_x = n(0);
	normal_y = n(1);
	normal_z = n(2);

	Eigen::Vector4f p = t*Eigen::Vector4f(point_x,point_y,point_z,1);
	point_x = p(0);
	point_y = p(1);
	point_z = p(2);
}

template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCylinder<PointT, PointNT>::selectWithinDistance (
      const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
  // Check if the model is valid given the user constraints
  if (!isModelValid (model_coefficients))
  {
    inliers.clear ();
    return;
  }

  int nr_p = 0;
  inliers.resize (indices_->size ());
  error_sqr_dists_.resize (indices_->size ());

  Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  float ptdotdir = line_pt.dot (line_dir);
  float dirdotdir = 1.0f / line_dir.dot (line_dir);
  // Iterate through the 3d points and calculate the distances from them to the sphere
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Approximate the distance from the point to the cylinder as the difference between
    // dist(point,cylinder_axis) and cylinder radius
    Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
    Eigen::Vector4f n  (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
    double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);

    // Calculate the point's projection on the cylinder axis
    float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
    Eigen::Vector4f pt_proj = line_pt + k * line_dir;
    Eigen::Vector4f dir = pt - pt_proj;
    dir.normalize ();

    // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
    double d_normal = fabs (getAngle3D (n, dir));
    d_normal = (std::min) (d_normal, M_PI - d_normal);

    double distance = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); 
    if (distance < threshold)
    {
      // Returns the indices of the points whose distances are smaller than the threshold
      inliers[nr_p] = (*indices_)[i];
      error_sqr_dists_[nr_p] = distance;
      ++nr_p;
    }
  }
  inliers.resize (nr_p);
  error_sqr_dists_.resize (nr_p);
}

template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCylinder<PointT, PointNT>::projectPointToCylinder (
      const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients, Eigen::Vector4f &pt_proj)
{
  Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);

  float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) * line_dir.dot (line_dir);
  pt_proj = line_pt + k * line_dir;

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

  // Calculate the projection of the point onto the cylinder
  pt_proj += dir * model_coefficients[6];
}

template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCone<PointT, PointNT>::doSamplesVerifyModel (
      const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold)
{
  // Needs a valid model coefficients
  if (model_coefficients.size () != 7)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCone::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
    return (false);
  }

  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 openning_angle = model_coefficients[6];

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

  // Iterate through the 3d points and calculate the distances from them to the cone
  for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
  {
    Eigen::Vector4f pt (input_->points[*it].x, input_->points[*it].y, input_->points[*it].z, 0);

    // Calculate the point's projection on the cone axis
    float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
    Eigen::Vector4f pt_proj = apex + k * axis_dir;
    Eigen::Vector4f dir = pt - pt_proj;
    dir.normalize ();

    // Calculate the actual radius of the cone at the level of the projected point
    Eigen::Vector4f height = apex - pt_proj;
    double actual_cone_radius = tan (openning_angle) * height.norm ();

    // Aproximate the distance from the point to the cone as the difference between
    // dist(point,cone_axis) and actual cone radius
    if (fabs (static_cast<double>(pointToAxisDistance (pt, model_coefficients) - actual_cone_radius)) > threshold)
      return (false);
  }

  return (true);
}

template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCylinder<PointT, PointNT>::getDistancesToModel (
      const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
  // Check if the model is valid given the user constraints
  if (!isModelValid (model_coefficients))
  {
    distances.clear ();
    return;
  }

  distances.resize (indices_->size ());

  Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  float ptdotdir = line_pt.dot (line_dir);
  float dirdotdir = 1.0f / line_dir.dot (line_dir);
  // Iterate through the 3d points and calculate the distances from them to the sphere
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Aproximate the distance from the point to the cylinder as the difference between
    // dist(point,cylinder_axis) and cylinder radius
    // @note need to revise this.
    Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
    Eigen::Vector4f n  (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);

    double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);

    // Calculate the point's projection on the cylinder axis
    float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
    Eigen::Vector4f pt_proj = line_pt + k * line_dir;
    Eigen::Vector4f dir = pt - pt_proj;
    dir.normalize ();

    // Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
    double d_normal = fabs (getAngle3D (n, dir));
    d_normal = (std::min) (d_normal, M_PI - d_normal);

    distances[i] = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
  }
}

template <typename PointInT> double 
pcl::tracking::NormalCoherence<PointInT>::computeCoherence (PointInT &source, PointInT &target)
{
    Eigen::Vector4f n = source.getNormalVector4fMap ();
    Eigen::Vector4f n_dash = target.getNormalVector4fMap ();
    if ( n.norm () <= 1e-5 || n_dash.norm () <= 1e-5 )
    {
        PCL_ERROR("norm might be ZERO!\n");
        std::cout << "source: " << source << std::endl;
        std::cout << "target: " << target << std::endl;
        exit (1);
        return 0.0;
    }
    else
    {
        n.normalize ();
        n_dash.normalize ();
        double theta = pcl::getAngle3D (n, n_dash);
        if (!pcl_isnan (theta))
            return 1.0 / (1.0 + weight_ * theta * theta);
        else
            return 0.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.matrix () = 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.matrix () = p - mc * distance_to_plane;        // mc[3] = 0, therefore the 3rd coordinate is safe
    }
  }
}

//.........这里部分代码省略.........
                point.y = centroid[1];
                point.z = centroid[2];
                //if(index!=-1 && point.x>1.2 && point.y<0.2 && point.y>-0.2)
                if(index!=-1 )//&& point.x<-1.2 && point.y<0.2 && point.y>-0.2)
                {
                    out_ray.clear();
                    ret = voxelFilter.occlusionEstimation( state,out_ray, ijk);
                    std::cout<<"State is:"<<state<<"\n";
                    /*
                    if(state == 1)
                    {
                        if(count++>=10)
                        {
                            foundBug = true;
                            break;
                        }
                        cout<<"Number of voxels in ray:"<<out_ray.size()<<"\n";
                        for(uint j=0; j< out_ray.size(); j++)
                        {
                            Eigen::Vector4f centroid = voxelFilter.getCentroidCoordinate (out_ray[j]);
                            pcl::PointXYZRGB p = pcl::PointXYZRGB(255,255,0);
                            p.x = centroid[0];
                            p.y = centroid[1];
                            p.z = centroid[2];
                            rayCloud->points.push_back(p);
                            std::cout<<"Ray X:"<<p.x<<" y:"<< p.y<<" z:"<< p.z<<"\n";
                        }
                    }
                    */
                    if(state != 1)
                    {
                        // estimate direction to target voxel
                        Eigen::Vector4f direction = centroid - cloud->sensor_origin_;
                        direction.normalize ();
                        // estimate entry point into the voxel grid
                        float tmin = voxelFilter.rayBoxIntersection (cloud->sensor_origin_, direction,p1,p2);
                        if(tmin!=-1)
                        {
                            // coordinate of the boundary of the voxel grid
                            Eigen::Vector4f start = cloud->sensor_origin_ + tmin * direction;
                            linePoint.x = cloud->sensor_origin_[0]; linePoint.y = cloud->sensor_origin_[1]; linePoint.z = cloud->sensor_origin_[2];
                            //std::cout<<"Box Min X:"<<linePoint.x<<" y:"<< linePoint.y<<" z:"<< linePoint.z<<"\n";
                            lineSegments.push_back(linePoint);

                            linePoint.x = start[0]; linePoint.y = start[1]; linePoint.z = start[2];
                            //std::cout<<"Box Max X:"<<linePoint.x<<" y:"<< linePoint.y<<" z:"<< linePoint.z<<"\n";
                            lineSegments.push_back(linePoint);

                            linePoint.x = start[0]; linePoint.y = start[1]; linePoint.z = start[2];
                            //std::cout<<"Box Max X:"<<linePoint.x<<" y:"<< linePoint.y<<" z:"<< linePoint.z<<"\n";
                            lineSegments.push_back(linePoint);

                            linePoint.x = centroid[0]; linePoint.y = centroid[1]; linePoint.z = centroid[2];
                            //std::cout<<"Box Max X:"<<linePoint.x<<" y:"<< linePoint.y<<" z:"<< linePoint.z<<"\n";
                            lineSegments.push_back(linePoint);

                            rayCloud->points.push_back(point);
                            pt.x = centroid[0];
                            pt.y = centroid[1];
                            pt.z = centroid[2];
                            occlusionFreeCloud->points.push_back(pt);
                        }
                    }
                }
            }
        }

//.........这里部分代码省略.........
    pcl::VoxelGridOcclusionEstimationT voxelFilter;
    voxelFilter.setInputCloud (output);
    voxelFilter.setLeafSize (voxelRes, voxelRes, voxelRes);
    voxelFilter.initializeVoxelGrid();

    int state,ret;

    pcl::PointXYZ pt,p1,p2;
    pcl::PointXYZRGB point;
    std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > out_ray;
    //std::vector<geometry_msgs::Point> lineSegments;
    geometry_msgs::Point linePoint;

    // iterate over the entire frustum points
    for ( int i = 0; i < (int)output->points.size(); i ++ )
    {
        // Get voxel centroid corresponding to selected point
        pcl::PointXYZ ptest = output->points[i];
        Eigen::Vector3i ijk = voxelFilter.getGridCoordinates( ptest.x, ptest.y, ptest.z);
        
        if(voxelFilter.getCentroidIndexAt(ijk) == -1 ) {
			// Voxel is out of bounds
            continue;
        }

        Eigen::Vector4f centroid = voxelFilter.getCentroidCoordinate (ijk);
        point = pcl::PointXYZRGB(0,244,0);
        point.x = centroid[0];
        point.y = centroid[1];
        point.z = centroid[2];

		// >>>>>>>>>>>>>>>>>>>>
		// 2.1 Perform occlusion estimation
		// >>>>>>>>>>>>>>>>>>>>
		
		/*
		 * The way this works is it traces a line to the point and finds
		 * the distance to the first occupied voxel in its path (t_min).
		 * It then determines if the distance between the target point and
		 * the collided voxel are close (within voxelRes).
		 * 
		 * If they are, the point is visible. Otherwise, the point is behind
		 * an occluding point
		 * 
		 * 
		 * This is the expected function of the following command:
		 *   ret = voxelFilter.occlusionEstimation( state,out_ray, ijk);
		 * 
		 * However, it sometimes shows occluded points on the edge of the cloud
		 */
		
		// Direction to target voxel
		Eigen::Vector4f direction = centroid - output->sensor_origin_;
		direction.normalize ();
		
		// Estimate entry point into the voxel grid
		float tmin = voxelFilter.rayBoxIntersection (output->sensor_origin_, direction,p1,p2); //where did this 4-input syntax come from?
		
		if(tmin == -1){
			// ray does not intersect with the bounding box
			continue;
		}
		
		// Calculate coordinate of the boundary of the voxel grid
		Eigen::Vector4f start = output->sensor_origin_ + tmin * direction;
		
		// Determine distance between boundary and target voxel centroid
		Eigen::Vector4f dist_vector = centroid-start;
		float distance = (dist_vector).dot(dist_vector);

		if (distance > voxelRes*1.414){ // voxelRes/sqrt(2)
			// ray does not correspond to this point
			continue;
		}
		
		// Save point
		occlusionFreeCloud_local->points.push_back(ptest);
		occlusionFreeCloud->points.push_back(ptest);
		

		// >>>>>>>>>>>>>>>>>>>>
		// 2.2 Save line segment for visualization
		// >>>>>>>>>>>>>>>>>>>>

		linePoint.x = output->sensor_origin_[0];
		linePoint.y = output->sensor_origin_[1];
		linePoint.z = output->sensor_origin_[2];
		lineSegments.push_back(linePoint);

		linePoint.x = start[0];
		linePoint.y = start[1];
		linePoint.z = start[2];
		lineSegments.push_back(linePoint);

		occupancyGrid->points.push_back(point);
    }
    FreeCloud.points = occlusionFreeCloud_local->points;

    return FreeCloud;
}