From 779c9a0863e52408c25f9200b1822f32647360f7 Mon Sep 17 00:00:00 2001
From: nfioraio <nicola.fioraio@gmail.com>
Date: Mon, 8 Jul 2013 12:10:21 +0200
Subject: [PATCH] Removed sensor_orientation_/sensor_origin_ setting

The sensor_orientation_/sensor_origin_ were not set to identity.
---
 io/include/pcl/io/impl/lzf_image_io.hpp | 48 ++++++++++----------
 io/src/openni_grabber.cpp               | 58 ++++++++++++-------------
 2 files changed, 53 insertions(+), 53 deletions(-)

diff --git a/io/include/pcl/io/impl/lzf_image_io.hpp b/io/include/pcl/io/impl/lzf_image_io.hpp
index c1cacfb2262..a67dfef7c68 100644
--- a/io/include/pcl/io/impl/lzf_image_io.hpp
+++ b/io/include/pcl/io/impl/lzf_image_io.hpp
@@ -94,24 +94,24 @@ pcl::io::LZFDepth16ImageReader::read (
       }
 
       pt.z = static_cast<float> (val * z_multiplication_factor_);
-      pt.x = (static_cast<float> (u) - static_cast<float> (parameters_.principal_point_x)) 
+      pt.x = (static_cast<float> (u) - static_cast<float> (parameters_.principal_point_x))
         * pt.z * static_cast<float> (constant_x);
-      pt.y = (static_cast<float> (v) - static_cast<float> (parameters_.principal_point_y)) 
+      pt.y = (static_cast<float> (v) - static_cast<float> (parameters_.principal_point_y))
         * pt.z * static_cast<float> (constant_y);
     }
   }
-  cloud.sensor_origin_.setZero ();
-  cloud.sensor_orientation_.w () = 0.0f;
-  cloud.sensor_orientation_.x () = 1.0f;
-  cloud.sensor_orientation_.y () = 0.0f;
-  cloud.sensor_orientation_.z () = 0.0f;
+//  cloud.sensor_origin_.setZero ();
+//  cloud.sensor_orientation_.w () = 0.0f;
+//  cloud.sensor_orientation_.x () = 1.0f;
+//  cloud.sensor_orientation_.y () = 0.0f;
+//  cloud.sensor_orientation_.z () = 0.0f;
   return (true);
 }
-        
+
 ///////////////////////////////////////////////////////////////////////////////
 template <typename PointT> bool
-pcl::io::LZFDepth16ImageReader::readOMP (const std::string &filename, 
-                                         pcl::PointCloud<PointT> &cloud, 
+pcl::io::LZFDepth16ImageReader::readOMP (const std::string &filename,
+                                         pcl::PointCloud<PointT> &cloud,
                                          unsigned int num_threads)
 {
   uint32_t uncompressed_size;
@@ -172,17 +172,17 @@ pcl::io::LZFDepth16ImageReader::readOMP (const std::string &filename,
     }
 
     pt.z = static_cast<float> (val * z_multiplication_factor_);
-    pt.x = (static_cast<float> (u) - static_cast<float> (parameters_.principal_point_x)) 
+    pt.x = (static_cast<float> (u) - static_cast<float> (parameters_.principal_point_x))
       * pt.z * static_cast<float> (constant_x);
-    pt.y = (static_cast<float> (v) - static_cast<float> (parameters_.principal_point_y)) 
+    pt.y = (static_cast<float> (v) - static_cast<float> (parameters_.principal_point_y))
       * pt.z * static_cast<float> (constant_y);
-    
+
   }
-  cloud.sensor_origin_.setZero ();
-  cloud.sensor_orientation_.w () = 0.0f;
-  cloud.sensor_orientation_.x () = 1.0f;
-  cloud.sensor_orientation_.y () = 0.0f;
-  cloud.sensor_orientation_.z () = 0.0f;
+//  cloud.sensor_origin_.setZero ();
+//  cloud.sensor_orientation_.w () = 0.0f;
+//  cloud.sensor_orientation_.x () = 1.0f;
+//  cloud.sensor_orientation_.y () = 0.0f;
+//  cloud.sensor_orientation_.z () = 0.0f;
   return (true);
 
 }
@@ -324,7 +324,7 @@ pcl::io::LZFYUV422ImageReader::read (
   unsigned char *color_u = reinterpret_cast<unsigned char*> (&uncompressed_data[0]);
   unsigned char *color_y = reinterpret_cast<unsigned char*> (&uncompressed_data[wh2]);
   unsigned char *color_v = reinterpret_cast<unsigned char*> (&uncompressed_data[wh2 + getWidth () * getHeight ()]);
-  
+
   register int y_idx = 0;
   for (size_t i = 0; i < wh2; ++i, y_idx += 2)
   {
@@ -382,7 +382,7 @@ pcl::io::LZFYUV422ImageReader::readOMP (
   unsigned char *color_u = reinterpret_cast<unsigned char*> (&uncompressed_data[0]);
   unsigned char *color_y = reinterpret_cast<unsigned char*> (&uncompressed_data[wh2]);
   unsigned char *color_v = reinterpret_cast<unsigned char*> (&uncompressed_data[wh2 + getWidth () * getHeight ()]);
-  
+
 #ifdef _OPENMP
 #pragma omp parallel for num_threads (num_threads)
 #endif//_OPENMP
@@ -437,8 +437,8 @@ pcl::io::LZFBayer8ImageReader::read (
   // Convert Bayer8 to RGB24
   std::vector<unsigned char> rgb_buffer (getWidth () * getHeight () * 3);
   pcl::io::DeBayer i;
-  i.debayerEdgeAware (reinterpret_cast<unsigned char*> (&uncompressed_data[0]), 
-                     static_cast<unsigned char*> (&rgb_buffer[0]), 
+  i.debayerEdgeAware (reinterpret_cast<unsigned char*> (&uncompressed_data[0]),
+                     static_cast<unsigned char*> (&rgb_buffer[0]),
                      getWidth (), getHeight ());
   // Copy to PointT
   cloud.width  = getWidth ();
@@ -487,8 +487,8 @@ pcl::io::LZFBayer8ImageReader::readOMP (
   // Convert Bayer8 to RGB24
   std::vector<unsigned char> rgb_buffer (getWidth () * getHeight () * 3);
   pcl::io::DeBayer i;
-  i.debayerEdgeAware (reinterpret_cast<unsigned char*> (&uncompressed_data[0]), 
-                     static_cast<unsigned char*> (&rgb_buffer[0]), 
+  i.debayerEdgeAware (reinterpret_cast<unsigned char*> (&uncompressed_data[0]),
+                     static_cast<unsigned char*> (&rgb_buffer[0]),
                      getWidth (), getHeight ());
   // Copy to PointT
   cloud.width  = getWidth ();
diff --git a/io/src/openni_grabber.cpp b/io/src/openni_grabber.cpp
index f72c1d66ee0..ee878db25b4 100644
--- a/io/src/openni_grabber.cpp
+++ b/io/src/openni_grabber.cpp
@@ -142,7 +142,7 @@ pcl::OpenNIGrabber::~OpenNIGrabber () throw ()
 
     // release the pointer to the device object
     device_.reset ();
-    
+
     // disconnect all listeners
     disconnect_all_slots<sig_cb_openni_image> ();
     disconnect_all_slots<sig_cb_openni_depth_image> ();
@@ -402,7 +402,7 @@ pcl::OpenNIGrabber::setupDevice (const std::string& device_id, const Mode& depth
 
   depth_width_ = depth_md.nXRes;
   depth_height_ = depth_md.nYRes;
-  
+
   if (device_->hasImageStream ())
   {
     XnMapOutputMode image_md;
@@ -563,10 +563,10 @@ pcl::OpenNIGrabber::convertToXYZPointCloud (const boost::shared_ptr<openni_wrapp
 
   if (pcl_isfinite (depth_focal_length_y_))
     constant_y =  1.0f / static_cast<float> (depth_focal_length_y_);
-  
+
   if (pcl_isfinite (depth_principal_point_x_))
     centerX =  static_cast<float> (depth_principal_point_x_);
-  
+
   if (pcl_isfinite (depth_principal_point_y_))
     centerY =  static_cast<float> (depth_principal_point_y_);
 
@@ -614,11 +614,11 @@ pcl::OpenNIGrabber::convertToXYZPointCloud (const boost::shared_ptr<openni_wrapp
       pt.y = (static_cast<float> (v) - centerY) * pt.z * constant_y;
     }
   }
-  cloud->sensor_origin_.setZero ();
-  cloud->sensor_orientation_.w () = 0.0f;
-  cloud->sensor_orientation_.x () = 1.0f;
-  cloud->sensor_orientation_.y () = 0.0f;
-  cloud->sensor_orientation_.z () = 0.0f;  
+//  cloud->sensor_origin_.setZero ();
+//  cloud->sensor_orientation_.w () = 0.0f;
+//  cloud->sensor_orientation_.x () = 1.0f;
+//  cloud->sensor_orientation_.y () = 0.0f;
+//  cloud->sensor_orientation_.z () = 0.0f;
   return (cloud);
 }
 
@@ -651,10 +651,10 @@ pcl::OpenNIGrabber::convertToXYZRGBPointCloud (const boost::shared_ptr<openni_wr
 
   if (pcl_isfinite (rgb_focal_length_y_))
     constant_y =  1.0f / static_cast<float> (rgb_focal_length_y_);
-  
+
   if (pcl_isfinite (rgb_principal_point_x_))
     centerX =  static_cast<float> (rgb_principal_point_x_);
-  
+
   if (pcl_isfinite (rgb_principal_point_y_))
     centerY =  static_cast<float> (rgb_principal_point_y_);
 
@@ -684,20 +684,20 @@ pcl::OpenNIGrabber::convertToXYZRGBPointCloud (const boost::shared_ptr<openni_wr
   image->fillRGB (image_width_, image_height_, rgb_buffer, image_width_ * 3);
   float bad_point = std::numeric_limits<float>::quiet_NaN ();
 
-  // set xyz to Nan and rgb to 0 (black)  
+  // set xyz to Nan and rgb to 0 (black)
   if (image_width_ != depth_width_)
   {
     PointT pt;
     pt.x = pt.y = pt.z = bad_point;
     pt.b = pt.g = pt.r = 0;
-    pt.a = 255; // point has no color info -> alpha = max => transparent 
+    pt.a = 255; // point has no color info -> alpha = max => transparent
     cloud->points.assign (cloud->points.size (), pt);
   }
-  
+
   // fill in XYZ values
   unsigned step = cloud->width / depth_width_;
   unsigned skip = cloud->width * step - cloud->width;
-  
+
   int value_idx = 0;
   int point_idx = 0;
   for (int v = 0; v < depth_height_; ++v, point_idx += skip)
@@ -726,7 +726,7 @@ pcl::OpenNIGrabber::convertToXYZRGBPointCloud (const boost::shared_ptr<openni_wr
   // fill in the RGB values
   step = cloud->width / image_width_;
   skip = cloud->width * step - cloud->width;
-  
+
   value_idx = 0;
   point_idx = 0;
   RGBValue color;
@@ -737,19 +737,19 @@ pcl::OpenNIGrabber::convertToXYZRGBPointCloud (const boost::shared_ptr<openni_wr
     for (unsigned xIdx = 0; xIdx < image_width_; ++xIdx, point_idx += step, value_idx += 3)
     {
       PointT& pt = cloud->points[point_idx];
-      
+
       color.Red   = rgb_buffer[value_idx];
       color.Green = rgb_buffer[value_idx + 1];
       color.Blue  = rgb_buffer[value_idx + 2];
-      
+
       pt.rgba = color.long_value;
     }
   }
-  cloud->sensor_origin_.setZero ();
-  cloud->sensor_orientation_.w () = 0.0;
-  cloud->sensor_orientation_.x () = 1.0;
-  cloud->sensor_orientation_.y () = 0.0;
-  cloud->sensor_orientation_.z () = 0.0;
+//  cloud->sensor_origin_.setZero ();
+//  cloud->sensor_orientation_.w () = 0.0;
+//  cloud->sensor_orientation_.x () = 1.0;
+//  cloud->sensor_orientation_.y () = 0.0;
+//  cloud->sensor_orientation_.z () = 0.0;
   return (cloud);
 }
 
@@ -781,7 +781,7 @@ pcl::OpenNIGrabber::convertToXYZIPointCloud (const boost::shared_ptr<openni_wrap
 
   if (pcl_isfinite (rgb_principal_point_x_))
     centerX = static_cast<float>(rgb_principal_point_x_);
-  
+
   if (pcl_isfinite (rgb_principal_point_y_))
     centerY = static_cast<float>(rgb_principal_point_y_);
 
@@ -835,11 +835,11 @@ pcl::OpenNIGrabber::convertToXYZIPointCloud (const boost::shared_ptr<openni_wrap
       pt.intensity = static_cast<float> (ir_map[depth_idx]);
     }
   }
-  cloud->sensor_origin_.setZero ();
-  cloud->sensor_orientation_.w () = 0.0;
-  cloud->sensor_orientation_.x () = 1.0;
-  cloud->sensor_orientation_.y () = 0.0;
-  cloud->sensor_orientation_.z () = 0.0;
+//  cloud->sensor_origin_.setZero ();
+//  cloud->sensor_orientation_.w () = 0.0;
+//  cloud->sensor_orientation_.x () = 1.0;
+//  cloud->sensor_orientation_.y () = 0.0;
+//  cloud->sensor_orientation_.z () = 0.0;
   return (cloud);
 }