Fix addPointCloudNormals with view point information

[VictorLamoine]

Fixes #1475

Test code

#include <pcl/console/parse.h>
#include <pcl/point_types.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/features/normal_3d.h>

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

int main(int argc, char *argv[])
{
  std::vector<int> ply_file_indices = pcl::console::parse_file_extension_argument(argc, argv, ".ply");
  if (ply_file_indices.size() != 1)
  {
    PCL_ERROR("Wrong usage: %s input.ply\n", argv[0]);
  }

  // Load mesh
  pcl::PolygonMesh cad;
  if (pcl::io::loadPolygonFilePLY(argv[ply_file_indices[0]], cad) == 0)
  {
    PCL_ERROR("Failed to read PLY file %s\n", argv[ply_file_indices[0]]);
    return -1;
  }

  // Convert to point cloud
  PointCloudT::Ptr cloud (new PointCloudT);
  pcl::fromPCLPointCloud2(cad.cloud, *cloud);

  //TODO: Comment the two following lines to test WITHOUT cloud view point
  cloud->sensor_origin_ << 0.3, 0, 0.2, 0;
  cloud->sensor_orientation_ = Eigen::Quaternionf (0, 1, 0, 1); // Deforms the cloud!

  // Estimate normals
  pcl::NormalEstimation<PointT, pcl::Normal> ne;
  ne.setInputCloud (cloud);
  pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
  ne.setSearchMethod (tree);
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
  ne.setRadiusSearch (0.08);
  ne.compute (*cloud_normals);

  // Display everything
  pcl::visualization::PCLVisualizer viewer;
  viewer.addCoordinateSystem(0.5);
  viewer.addPointCloud(cloud, "my_cloud");
  viewer.addPointCloudNormals<PointT,pcl::Normal>(cloud, cloud_normals);
  viewer.spin();
  return 0;
}

@athundt does it fix your issue? It does for me 😉

[VictorLamoine]

Ping! 🌺
@athundt

[ahundt]

sorry lost track of this, I haven't been using this part of the code recently so I'm not set up to test it right now. Sorry!

[ahundt]

Looking at the change it does seem reasonable however!