Merge pull request #5766 from mvieth/ply_origin_orientation

Fix reading of origin and orientation from PLY files

io/include/pcl/io/ply_io.h

@@ -88,12 +88,12 @@ namespace pcl
 
       PLYReader ()
         : origin_ (Eigen::Vector4f::Zero ())
-        , orientation_ (Eigen::Matrix3f::Zero ())
+        , orientation_ (Eigen::Matrix3f::Identity ())
       {}
 
       PLYReader (const PLYReader &p)
         : origin_ (Eigen::Vector4f::Zero ())
-        , orientation_ (Eigen::Matrix3f::Zero ())
+        , orientation_ (Eigen::Matrix3f::Identity ())
         , cloud_ ()
         , vertex_count_ (0)
         , vertex_offset_before_ (0)
@@ -129,8 +129,8 @@ namespace pcl
         *  * > 0 on success
         * \param[in] file_name the name of the file to load
         * \param[out] cloud the resultant point cloud dataset (only the header will be filled)
-        * \param[in] origin the sensor data acquisition origin (translation)
-        * \param[in] orientation the sensor data acquisition origin (rotation)
+        * \param[out] origin the sensor data acquisition origin (translation)
+        * \param[out] orientation the sensor data acquisition origin (rotation)
         * \param[out] ply_version the PLY version read from the file
         * \param[out] data_type the type of PLY data stored in the file
         * \param[out] data_idx the data index
@@ -148,8 +148,8 @@ namespace pcl
       /** \brief Read a point cloud data from a PLY file and store it into a pcl/PCLPointCloud2.
         * \param[in] file_name the name of the file containing the actual PointCloud data
         * \param[out] cloud the resultant PointCloud message read from disk
-        * \param[in] origin the sensor data acquisition origin (translation)
-        * \param[in] orientation the sensor data acquisition origin (rotation)
+        * \param[out] origin the sensor data acquisition origin (translation)
+        * \param[out] orientation the sensor data acquisition origin (rotation)
         * \param[out] ply_version the PLY version read from the file
         * \param[in] offset the offset in the file where to expect the true header to begin.
         * One usage example for setting the offset parameter is for reading
@@ -208,8 +208,8 @@ namespace pcl
         *
         * \param[in] file_name the name of the file containing the actual PointCloud data
         * \param[out] mesh the resultant PolygonMesh message read from disk
-        * \param[in] origin the sensor data acquisition origin (translation)
-        * \param[in] orientation the sensor data acquisition origin (rotation)
+        * \param[out] origin the sensor data acquisition origin (translation)
+        * \param[out] orientation the sensor data acquisition origin (rotation)
         * \param[out] ply_version the PLY version read from the file
         * \param[in] offset the offset in the file where to expect the true header to begin.
         * One usage example for setting the offset parameter is for reading
@@ -747,9 +747,9 @@ namespace pcl
 
     /** \brief Load any PLY file into a PCLPointCloud2 type.
       * \param[in] file_name the name of the file to load
-      * \param[in] cloud the resultant templated point cloud
-      * \param[in] origin the sensor acquisition origin (only for > PLY_V7 - null if not present)
-      * \param[in] orientation the sensor acquisition orientation if available, 
+      * \param[out] cloud the resultant templated point cloud
+      * \param[out] origin the sensor acquisition origin (only for > PLY_V7 - null if not present)
+      * \param[out] orientation the sensor acquisition orientation if available, 
       * identity if not present
       * \ingroup io
       */

io/src/ply_io.cpp

@@ -562,6 +562,8 @@ pcl::PLYReader::readHeader (const std::string &file_name, pcl::PCLPointCloud2 &c
   cloud_->width = cloud_->height = 0;
   origin = Eigen::Vector4f::Zero ();
   orientation = Eigen::Quaternionf::Identity ();
+  origin_ = Eigen::Vector4f::Zero ();
+  orientation_ = Eigen::Matrix3f::Identity ();
   if (!parse (file_name))
   {
     PCL_ERROR ("[pcl::PLYReader::read] problem parsing header!\n");
@@ -653,8 +655,8 @@ pcl::PLYReader::read (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
     cloud_->data.swap (data);
   }
 
-  orientation_ = Eigen::Quaternionf (orientation);
-  origin_ = origin;
+  orientation = Eigen::Quaternionf (orientation_);
+  origin = origin_;
 
   for (auto &field : cloud_->fields)
   {
@@ -752,8 +754,8 @@ pcl::PLYReader::read (const std::string &file_name, pcl::PolygonMesh &mesh,
     cloud_->data.swap (data);
   }
 
-  orientation_ = Eigen::Quaternionf (orientation);
-  origin_ = origin;
+  orientation = Eigen::Quaternionf (orientation_);
+  origin = origin_;
 
   for (auto &field : cloud_->fields)
   {

test/io/test_ply_io.cpp

@@ -80,10 +80,22 @@ TEST (PCL, PLYReaderWriter)
 
   // test for toPCLPointCloud2 ()
   pcl::PLYWriter writer;
-  writer.write ("test_pcl_io.ply", cloud_blob, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), true, true);
+  const Eigen::Vector4f origin (0.0f, 0.5f, -1.0f, 0.0f);
+  const Eigen::Quaternionf orientation(std::sqrt(0.5f), std::sqrt(0.5f), 0.0f, 0.0f);
+  writer.write ("test_pcl_io.ply", cloud_blob, origin, orientation, true, true);
 
   pcl::PLYReader reader;
-  reader.read ("test_pcl_io.ply", cloud_blob2);
+  Eigen::Vector4f origin2;
+  Eigen::Quaternionf orientation2;
+  int ply_version;
+  reader.read ("test_pcl_io.ply", cloud_blob2, origin2, orientation2, ply_version);
+  EXPECT_NEAR (origin.x(), origin2.x(), 1e-5);
+  EXPECT_NEAR (origin.y(), origin2.y(), 1e-5);
+  EXPECT_NEAR (origin.z(), origin2.z(), 1e-5);
+  EXPECT_NEAR (orientation.x(), orientation2.x(), 1e-5);
+  EXPECT_NEAR (orientation.y(), orientation2.y(), 1e-5);
+  EXPECT_NEAR (orientation.z(), orientation2.z(), 1e-5);
+  EXPECT_NEAR (orientation.w(), orientation2.w(), 1e-5);
   //PLY DOES preserve organiziation
   EXPECT_EQ (cloud_blob.width * cloud_blob.height, cloud_blob2.width * cloud_blob2.height);
   EXPECT_EQ (cloud_blob.is_dense, cloud.is_dense);