diff --git a/simulation/include/pcl/simulation/camera.h b/simulation/include/pcl/simulation/camera.h
index 2b2574292f5..98b81351e50 100644
--- a/simulation/include/pcl/simulation/camera.h
+++ b/simulation/include/pcl/simulation/camera.h
@@ -1,121 +1,180 @@
 #pragma once
 
-#include <boost/shared_ptr.hpp>
 #include <Eigen/Dense>
 #include <Eigen/StdVector>
+#include <boost/shared_ptr.hpp>
 
 #include <pcl/pcl_macros.h>
 
-namespace pcl
-{
-  namespace simulation
-  {
-    class PCL_EXPORTS Camera
-    {
-    public:
-      using Ptr = boost::shared_ptr<Camera>;
-      using ConstPtr = boost::shared_ptr<const Camera>;
-
-      Camera () : x_ (0), y_ (0), z_ (0), roll_ (0), pitch_ (0), yaw_ (0)
-      {
-        updatePose ();
-        initializeCameraParameters ();
-      }
-
-      Camera (double x, double y, double z,
-              double roll, double pitch, double yaw) : x_ (x),
-                                                       y_ (y),
-                                                       z_ (z),
-                                                       roll_ (roll),
-                                                       pitch_ (pitch),
-                                                       yaw_ (yaw)
-      {
-        updatePose ();
-        initializeCameraParameters ();
-      }
-
-      void
-      setParameters (int width, int height,
-                     float fx, float fy,
-                     float cx, float cy,
-                     float z_near, float z_far);
-
-      Eigen::Matrix4f
-      getProjectionMatrix () { return projection_matrix_; }
-
-      double getX () const { return x_; }
-      void setX (double x) { x_ = x; updatePose (); }
-
-      double getY () const { return y_; }
-      void setY (double y) { y_ = y; updatePose(); }
-
-      double getZ () const { return z_; }
-      void setZ (double z) { z_ = z; updatePose(); }
-
-      double
-      getRoll () const { return roll_; }
-      void
-      setRoll (double roll) { roll_ = roll; updatePose (); }
-
-      double
-      getPitch() const {return pitch_;}
-      void
-      setPitch(double pitch) { pitch_ = pitch; updatePose (); }
-
-      double
-      getYaw () const { return yaw_; }
-      void
-      setYaw (double yaw) { yaw_ = yaw; updatePose (); }
-
-      Eigen::Isometry3d
-      getPose () const { return pose_; }
-
-      void set (double x, double y, double z, double roll, double pitch, double yaw)
-      {
-        x_ = x; y_ = y; z_ = z;
-        roll_ = roll; pitch_ = pitch; yaw_ = yaw;
-        updatePose();
-      }
-
-      void move (double vx, double vy, double vz);
-
-      // Return the pose of the camera:
-      Eigen::Vector3d getYPR ()
-      {
-        return Eigen::Vector3d (yaw_, pitch_, roll_);
-      }
-
-    private:
-      void
-      updatePose ();
-
-      void
-      initializeCameraParameters ();
-
-      void
-      updateProjectionMatrix ();
-
-      double x_,y_,z_;
-      double roll_,pitch_,yaw_;
-
-      Eigen::Isometry3d pose_;
-
-      // Camera Intrinsic Parameters
-      int width_;
-      int height_;
-      float fx_;
-      float fy_;
-      float cx_;
-      float cy_;
-
-      // min and max range of the camera
-      float z_near_;
-      float z_far_;
-
-      Eigen::Matrix4f projection_matrix_;
-
-    public:
-      PCL_MAKE_ALIGNED_OPERATOR_NEW
-    };
-  } // namespace - simulation
-} // namespace - pcl
+namespace pcl {
+namespace simulation {
+
+class PCL_EXPORTS Camera {
+public:
+  using Ptr = boost::shared_ptr<Camera>;
+  using ConstPtr = boost::shared_ptr<const Camera>;
+
+  Camera() : x_(0), y_(0), z_(0), roll_(0), pitch_(0), yaw_(0)
+  {
+    updatePose();
+    initializeCameraParameters();
+  }
+
+  Camera(double x, double y, double z, double roll, double pitch, double yaw)
+  : x_(x), y_(y), z_(z), roll_(roll), pitch_(pitch), yaw_(yaw)
+  {
+    updatePose();
+    initializeCameraParameters();
+  }
+
+  void
+  setParameters(int width,
+                int height,
+                float fx,
+                float fy,
+                float cx,
+                float cy,
+                float z_near,
+                float z_far);
+
+  Eigen::Matrix4f
+  getProjectionMatrix()
+  {
+    return projection_matrix_;
+  }
+
+  double
+  getX() const
+  {
+    return x_;
+  }
+  void
+  setX(double x)
+  {
+    x_ = x;
+    updatePose();
+  }
+
+  double
+  getY() const
+  {
+    return y_;
+  }
+  void
+  setY(double y)
+  {
+    y_ = y;
+    updatePose();
+  }
+
+  double
+  getZ() const
+  {
+    return z_;
+  }
+  void
+  setZ(double z)
+  {
+    z_ = z;
+    updatePose();
+  }
+
+  double
+  getRoll() const
+  {
+    return roll_;
+  }
+  void
+  setRoll(double roll)
+  {
+    roll_ = roll;
+    updatePose();
+  }
+
+  double
+  getPitch() const
+  {
+    return pitch_;
+  }
+  void
+  setPitch(double pitch)
+  {
+    pitch_ = pitch;
+    updatePose();
+  }
+
+  double
+  getYaw() const
+  {
+    return yaw_;
+  }
+  void
+  setYaw(double yaw)
+  {
+    yaw_ = yaw;
+    updatePose();
+  }
+
+  Eigen::Isometry3d
+  getPose() const
+  {
+    return pose_;
+  }
+
+  void
+  set(double x, double y, double z, double roll, double pitch, double yaw)
+  {
+    x_ = x;
+    y_ = y;
+    z_ = z;
+    roll_ = roll;
+    pitch_ = pitch;
+    yaw_ = yaw;
+    updatePose();
+  }
+
+  void
+  move(double vx, double vy, double vz);
+
+  // Return the pose of the camera:
+  Eigen::Vector3d
+  getYPR()
+  {
+    return Eigen::Vector3d(yaw_, pitch_, roll_);
+  }
+
+private:
+  void
+  updatePose();
+
+  void
+  initializeCameraParameters();
+
+  void
+  updateProjectionMatrix();
+
+  double x_, y_, z_;
+  double roll_, pitch_, yaw_;
+
+  Eigen::Isometry3d pose_;
+
+  // Camera Intrinsic Parameters
+  int width_;
+  int height_;
+  float fx_;
+  float fy_;
+  float cx_;
+  float cy_;
+
+  // min and max range of the camera
+  float z_near_;
+  float z_far_;
+
+  Eigen::Matrix4f projection_matrix_;
+
+public:
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+} // namespace simulation
+} // namespace pcl
diff --git a/simulation/include/pcl/simulation/glsl_shader.h b/simulation/include/pcl/simulation/glsl_shader.h
index 5f699a66e01..49f77aa8807 100644
--- a/simulation/include/pcl/simulation/glsl_shader.h
+++ b/simulation/include/pcl/simulation/glsl_shader.h
@@ -9,95 +9,116 @@
 
 #include <pcl/pcl_exports.h>
 
-#include <GL/glew.h>
 #include <Eigen/Core>
+#include <GL/glew.h>
 #include <boost/shared_ptr.hpp>
 
-namespace pcl
-{
-  namespace simulation
+namespace pcl {
+namespace simulation {
+namespace gllib {
+
+enum ShaderType {
+  VERTEX = GL_VERTEX_SHADER,
+  FRAGMENT = GL_FRAGMENT_SHADER,
+  GEOMETRY = GL_GEOMETRY_SHADER,
+  TESS_CONTROL = GL_TESS_CONTROL_SHADER,
+  TESS_EVALUATION = GL_TESS_EVALUATION_SHADER
+};
+
+/** A GLSL shader program.  */
+class PCL_EXPORTS Program {
+public:
+  using Ptr = boost::shared_ptr<Program>;
+  using ConstPtr = boost::shared_ptr<const Program>;
+
+  /** Construct an empty shader program.  */
+  Program();
+
+  /** Destruct the shader program.  */
+  ~Program();
+
+  /** Add a new shader object to the program.  */
+  bool
+  addShaderText(const std::string& text, ShaderType shader_type);
+
+  /** Add a new shader object to the program.  */
+  bool
+  addShaderFile(const std::string& text, ShaderType shader_type);
+
+  /** Link the program.  */
+  bool
+  link();
+
+  /** Return true if the program is linked.  */
+  bool
+  isLinked();
+
+  /** Use the program.  */
+  void
+  use();
+
+  // Set uniforms
+  void
+  setUniform(const std::string& name, const Eigen::Vector2f& v);
+
+  void
+  setUniform(const std::string& name, const Eigen::Vector3f& v);
+
+  void
+  setUniform(const std::string& name, const Eigen::Vector4f& v);
+
+  void
+  setUniform(const std::string& name, const Eigen::Matrix2f& v);
+
+  void
+  setUniform(const std::string& name, const Eigen::Matrix3f& v);
+
+  void
+  setUniform(const std::string& name, const Eigen::Matrix4f& v);
+
+  void
+  setUniform(const std::string& name, float v);
+
+  void
+  setUniform(const std::string& name, int v);
+
+  void
+  setUniform(const std::string& name, bool v);
+
+  int
+  getUniformLocation(const std::string& name);
+
+  void
+  printActiveUniforms();
+  void
+  printActiveAttribs();
+
+  GLuint
+  programId()
   {
-    namespace gllib
-    {
-      enum ShaderType { VERTEX = GL_VERTEX_SHADER,
-                        FRAGMENT = GL_FRAGMENT_SHADER,
-                        GEOMETRY = GL_GEOMETRY_SHADER,
-                        TESS_CONTROL = GL_TESS_CONTROL_SHADER,
-                        TESS_EVALUATION = GL_TESS_EVALUATION_SHADER };
-
-      /**
-       * A GLSL shader program.
-       */
-      class PCL_EXPORTS Program
-      {
-        public:
-          using Ptr = boost::shared_ptr<Program>;
-          using ConstPtr = boost::shared_ptr<const Program>;
-
-          /**
-           * Construct an empty shader program.
-           */
-          Program ();
-
-          /**
-           * Destruct the shader program.
-           */
-          ~Program ();
-
-          /**
-           * Add a new shader object to the program.
-           */
-          bool addShaderText (const std::string& text, ShaderType shader_type);
-
-          /**
-           * Add a new shader object to the program.
-           */
-          bool addShaderFile (const std::string& text, ShaderType shader_type);
-
-          /**
-           * Link the program.
-           */
-          bool link ();
-
-          /**
-           * Return true if the program is linked.
-           */
-          bool isLinked ();
-
-          /**
-           * Use the program.
-           */
-          void use ();
-
-          // Set uniforms
-          void setUniform (const std::string& name, const Eigen::Vector2f& v);
-          void setUniform (const std::string& name, const Eigen::Vector3f& v);
-          void setUniform (const std::string& name, const Eigen::Vector4f& v);
-          void setUniform (const std::string& name, const Eigen::Matrix2f& v);
-          void setUniform (const std::string& name, const Eigen::Matrix3f& v);
-          void setUniform (const std::string& name, const Eigen::Matrix4f& v);
-          void setUniform (const std::string& name, float v);
-          void setUniform (const std::string& name, int v);
-          void setUniform (const std::string& name, bool v);
-
-          int getUniformLocation (const std::string& name);
-
-          void printActiveUniforms ();
-          void printActiveAttribs ();
-
-          GLuint programId () { return program_id_; }
-
-          static Ptr loadProgramFromFile (const std::string& vertex_shader_file, const std::string& fragment_shader_file);
-          static Ptr loadProgramFromText (const std::string& vertex_shader_text, const std::string& fragment_shader_text);
-
-        private:
-          GLuint program_id_;
-      };
-
-      GLenum PCL_EXPORTS getGLError ();
-      void PCL_EXPORTS printShaderInfoLog (GLuint shader);
-      void PCL_EXPORTS printProgramInfoLog (GLuint program);
-
-    } // namespace - gllib
-  } // namespace - simulation
-} // namespace - pcl
+    return program_id_;
+  }
+
+  static Ptr
+  loadProgramFromFile(const std::string& vertex_shader_file,
+                      const std::string& fragment_shader_file);
+  static Ptr
+  loadProgramFromText(const std::string& vertex_shader_text,
+                      const std::string& fragment_shader_text);
+
+private:
+  GLuint program_id_;
+};
+
+GLenum PCL_EXPORTS
+getGLError();
+
+void PCL_EXPORTS
+printShaderInfoLog(GLuint shader);
+
+void PCL_EXPORTS
+printProgramInfoLog(GLuint program);
+
+} // namespace gllib
+} // namespace simulation
+} // namespace pcl
diff --git a/simulation/include/pcl/simulation/model.h b/simulation/include/pcl/simulation/model.h
index c2fceaf1ad6..8b89f5efa91 100644
--- a/simulation/include/pcl/simulation/model.h
+++ b/simulation/include/pcl/simulation/model.h
@@ -1,208 +1,193 @@
 #pragma once
 
 #if defined(_WIN32) && !defined(APIENTRY) && !defined(__CYGWIN__)
-# define WIN32_LEAN_AND_MEAN 1
-# include <windows.h>
+#define WIN32_LEAN_AND_MEAN 1
+#include <windows.h>
 #endif
 
 #include <GL/glew.h>
 
 #include <pcl/pcl_config.h>
 #ifdef OPENGL_IS_A_FRAMEWORK
-# include <OpenGL/gl.h>
-# include <OpenGL/glu.h>
+#include <OpenGL/gl.h>
+#include <OpenGL/glu.h>
 #else
-# include <GL/gl.h>
-# include <GL/glu.h>
+#include <GL/gl.h>
+#include <GL/glu.h>
 #endif
 
 #include <boost/shared_ptr.hpp>
-#include <pcl/pcl_macros.h>
+#include <pcl/PolygonMesh.h>
 #include <pcl/io/pcd_io.h>
+#include <pcl/pcl_macros.h>
 #include <pcl/point_types.h>
-#include <pcl/PolygonMesh.h>
 #include <pcl/simulation/glsl_shader.h>
 
-namespace pcl
-{
-  namespace simulation
-  {
-    struct SinglePoly
-    {
-      float* vertices_;
-      float* colors_;
-      GLenum mode_;
-      GLuint nvertices_;
-    };
-
-    struct Vertex
-    {
-      Vertex () {}
-      //Vertex(Eigen::Vector3f pos, Eigen::Vector3f norm) : pos(pos), norm(norm) {}
-      Vertex (Eigen::Vector3f pos, Eigen::Vector3f rgb) : pos (pos), rgb (rgb) {}
-      Eigen::Vector3f pos;
-      Eigen::Vector3f rgb;
-      //Eigen::Vector3f norm;
-      //Eigen::Vector2f tex;
-    };
-
-    struct Face
-    {
-      // Index int to the index list
-      unsigned int index_offset;
-      // Number of vertices on face
-      unsigned int count;
-      // Normal of face
-      Eigen::Vector3f norm;
-    };
-
-    using Vertices = std::vector<Vertex>;
-    using Indices = std::vector<GLuint>;
-
-    class Model
-    {
-      public:
-        virtual void draw () = 0;
-
-        using Ptr = boost::shared_ptr<Model>;
-        using ConstPtr = boost::shared_ptr<const Model>;
-    };
-
-    class PCL_EXPORTS TriangleMeshModel : public Model
-    {
-      public:
-        using Ptr = boost::shared_ptr<TriangleMeshModel>;
-        using ConstPtr = boost::shared_ptr<const TriangleMeshModel>;
-
-        TriangleMeshModel (pcl::PolygonMesh::Ptr plg);
-
-        virtual
-        ~TriangleMeshModel ();
-
-        void
-        draw () override;
-
-      private:
-        GLuint vbo_;
-        GLuint ibo_;
-        GLuint size_;
-        //Vertices vertices_;
-        //Indices indices_;
-    };
-
-    class PCL_EXPORTS PolygonMeshModel : public Model
-    {
-      public:
-        PolygonMeshModel(GLenum mode, pcl::PolygonMesh::Ptr plg);
-        virtual ~PolygonMeshModel();
-        void draw() override;
-
-        using Ptr = boost::shared_ptr<PolygonMeshModel>;
-        using ConstPtr = boost::shared_ptr<const PolygonMeshModel>;
-      private:
-        std::vector<SinglePoly> polygons;
-
-        /*
-          GL_POINTS;
-          GL_LINE_STRIP;
-          GL_LINE_LOOP;
-          GL_LINES;
-          GL_TRIANGLE_STRIP;
-          GL_TRIANGLE_FAN;
-          GL_TRIANGLES;
-          GL_QUAD_STRIP;
-          GL_QUADS;
-          GL_POLYGON;
-        */
-        GLenum mode_;
-    };
-
-    class PCL_EXPORTS PointCloudModel : public Model
-    {
-      public:
-        using Ptr = boost::shared_ptr<PointCloudModel>;
-        using ConstPtr = boost::shared_ptr<const PointCloudModel>;
-
-        PointCloudModel (GLenum mode, pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc);
-
-        virtual
-        ~PointCloudModel ();
-
-        void
-        draw() override;
-
-      private:
-        float* vertices_;
-        float* colors_;
-
-        /*
-          GL_POINTS;
-          GL_LINE_STRIP;
-          GL_LINE_LOOP;
-          GL_LINES;
-          GL_TRIANGLE_STRIP;
-          GL_TRIANGLE_FAN;
-          GL_TRIANGLES;
-          GL_QUAD_STRIP;
-          GL_QUADS;
-          GL_POLYGON;
-        */
-        GLenum mode_;
-        size_t nvertices_;
-    };
-
-    /**
-     * Renders a single quad providing position (-1,-1,0) - (1,1,0) and
-     * texture coordinates (0,0) - (1,1) to each vertex.
-     * Coordinates are (lower left) - (upper right).
-     * Position is set as vertex attribute 0 and the texture coordinate
-     * as vertex attribute 1.
-     */
-    class PCL_EXPORTS Quad
-    {
-      public:
-        /**
-         * Setup the vbo for the quad.
-         */
-        Quad ();
-
-        /**
-         * Release any resources.
-         */
-
-        ~Quad ();
-
-        /**
-         * Render the quad.
-         */
-        void
-        render ();
-
-      private:
-        GLuint quad_vbo_;
-    };
-
-    class PCL_EXPORTS TexturedQuad
-    {
-      public:
-        using Ptr = boost::shared_ptr<TexturedQuad>;
-        using ConstPtr = boost::shared_ptr<const TexturedQuad>;
-
-        TexturedQuad (int width, int height);
-        ~TexturedQuad ();
-
-        void
-        setTexture (const uint8_t* data);
-
-        void
-        render ();
-  
-      private:
-        int width_;
-        int height_;
-        Quad quad_;
-        GLuint texture_;
-        gllib::Program::Ptr program_;
-    };
-  } // namespace - simulation
-} // namespace - pcl
+namespace pcl {
+namespace simulation {
+
+struct SinglePoly {
+  float* vertices_;
+  float* colors_;
+  GLenum mode_;
+  GLuint nvertices_;
+};
+
+struct Vertex {
+  Vertex() {}
+  // Vertex(Eigen::Vector3f pos, Eigen::Vector3f norm) : pos(pos), norm(norm) {}
+  Vertex(Eigen::Vector3f pos, Eigen::Vector3f rgb) : pos(pos), rgb(rgb) {}
+  Eigen::Vector3f pos;
+  Eigen::Vector3f rgb;
+  // Eigen::Vector3f norm;
+  // Eigen::Vector2f tex;
+};
+
+struct Face {
+  /// Index int to the index list
+  unsigned int index_offset;
+  /// Number of vertices on face
+  unsigned int count;
+  /// Normal of face
+  Eigen::Vector3f norm;
+};
+
+using Vertices = std::vector<Vertex>;
+using Indices = std::vector<GLuint>;
+
+class Model {
+public:
+  virtual void
+  draw() = 0;
+
+  using Ptr = boost::shared_ptr<Model>;
+  using ConstPtr = boost::shared_ptr<const Model>;
+};
+
+class PCL_EXPORTS TriangleMeshModel : public Model {
+public:
+  using Ptr = boost::shared_ptr<TriangleMeshModel>;
+  using ConstPtr = boost::shared_ptr<const TriangleMeshModel>;
+
+  TriangleMeshModel(pcl::PolygonMesh::Ptr plg);
+
+  virtual ~TriangleMeshModel();
+
+  void
+  draw() override;
+
+private:
+  GLuint vbo_;
+  GLuint ibo_;
+  GLuint size_;
+  // Vertices vertices_;
+  // Indices indices_;
+};
+
+class PCL_EXPORTS PolygonMeshModel : public Model {
+public:
+  PolygonMeshModel(GLenum mode, pcl::PolygonMesh::Ptr plg);
+  virtual ~PolygonMeshModel();
+  void
+  draw() override;
+
+  using Ptr = boost::shared_ptr<PolygonMeshModel>;
+  using ConstPtr = boost::shared_ptr<const PolygonMeshModel>;
+
+private:
+  std::vector<SinglePoly> polygons;
+
+  /*
+    GL_POINTS;
+    GL_LINE_STRIP;
+    GL_LINE_LOOP;
+    GL_LINES;
+    GL_TRIANGLE_STRIP;
+    GL_TRIANGLE_FAN;
+    GL_TRIANGLES;
+    GL_QUAD_STRIP;
+    GL_QUADS;
+    GL_POLYGON;
+  */
+  GLenum mode_;
+};
+
+class PCL_EXPORTS PointCloudModel : public Model {
+public:
+  using Ptr = boost::shared_ptr<PointCloudModel>;
+  using ConstPtr = boost::shared_ptr<const PointCloudModel>;
+
+  PointCloudModel(GLenum mode, pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc);
+
+  virtual ~PointCloudModel();
+
+  void
+  draw() override;
+
+private:
+  float* vertices_;
+  float* colors_;
+
+  /*
+    GL_POINTS;
+    GL_LINE_STRIP;
+    GL_LINE_LOOP;
+    GL_LINES;
+    GL_TRIANGLE_STRIP;
+    GL_TRIANGLE_FAN;
+    GL_TRIANGLES;
+    GL_QUAD_STRIP;
+    GL_QUADS;
+    GL_POLYGON;
+  */
+  GLenum mode_;
+  size_t nvertices_;
+};
+
+/** Renders a single quad providing position (-1,-1,0) - (1,1,0) and texture coordinates
+ *  (0,0) - (1,1) to each vertex.
+ *
+ *  Coordinates are (lower left) - (upper right). Position is set as vertex attribute 0
+ *  and the texture coordinate as vertex attribute 1.
+ */
+class PCL_EXPORTS Quad {
+public:
+  /** Setup the vbo for the quad. */
+  Quad();
+
+  /** Release any resources. */
+
+  ~Quad();
+
+  /** Render the quad. */
+  void
+  render();
+
+private:
+  GLuint quad_vbo_;
+};
+
+class PCL_EXPORTS TexturedQuad {
+public:
+  using Ptr = boost::shared_ptr<TexturedQuad>;
+  using ConstPtr = boost::shared_ptr<const TexturedQuad>;
+
+  TexturedQuad(int width, int height);
+  ~TexturedQuad();
+
+  void
+  setTexture(const uint8_t* data);
+
+  void
+  render();
+
+private:
+  int width_;
+  int height_;
+  Quad quad_;
+  GLuint texture_;
+  gllib::Program::Ptr program_;
+};
+
+} // namespace simulation
+} // namespace pcl
diff --git a/simulation/include/pcl/simulation/range_likelihood.h b/simulation/include/pcl/simulation/range_likelihood.h
index 7cf4023ad98..c0746a855a3 100644
--- a/simulation/include/pcl/simulation/range_likelihood.h
+++ b/simulation/include/pcl/simulation/range_likelihood.h
@@ -4,11 +4,11 @@
 
 #include <pcl/pcl_config.h>
 #ifdef OPENGL_IS_A_FRAMEWORK
-# include <OpenGL/gl.h>
-# include <OpenGL/glu.h>
+#include <OpenGL/gl.h>
+#include <OpenGL/glu.h>
 #else
-# include <GL/gl.h>
-# include <GL/glu.h>
+#include <GL/gl.h>
+#include <GL/glu.h>
 #endif
 
 //#include <math.h>
@@ -16,250 +16,308 @@
 
 #include <pcl/pcl_macros.h>
 //#include <pcl/win32_macros.h>
-#include <pcl/range_image/range_image_planar.h>
 #include <pcl/common/transforms.h>
+#include <pcl/range_image/range_image_planar.h>
 #include <pcl/simulation/camera.h>
-#include <pcl/simulation/scene.h>
 #include <pcl/simulation/glsl_shader.h>
+#include <pcl/simulation/scene.h>
 #include <pcl/simulation/sum_reduce.h>
 
-namespace pcl
-{
-  namespace simulation
+namespace pcl {
+namespace simulation {
+class PCL_EXPORTS RangeLikelihood {
+public:
+  using Ptr = boost::shared_ptr<RangeLikelihood>;
+  using ConstPtr = boost::shared_ptr<const RangeLikelihood>;
+
+public:
+  /** Create a new object to compute range image likelihoods.
+   *
+   *  OpenGL is used to render the images. It is assumed that render buffers have
+   *  already been setup. The area used is from 0,0 to cols*col_width,rows*row_height.
+   *
+   * \param rows number of rows to use in the render buffer
+   * \param cols number of columns to use in the render buffer
+   * \param row_height height of the image for a single particle
+   * \param col_width width of the image for a single particle
+   * \param scene a pointer to the scene that should be rendered when computing
+   *        likelihoods
+   */
+  RangeLikelihood(int rows, int cols, int row_height, int col_width, Scene::Ptr scene);
+
+  /** Destroy the RangeLikelihood object and release any memory allocated. */
+  ~RangeLikelihood();
+
+  /** Computes the likelihood of reference for each of the provided poses.
+   *
+   * \param reference is a depth image
+   * \param poses is a vector of the poses to test
+   * \param scores is an output argument. The resulting log likelihoods will be written
+   *        in score
+   */
+  void
+  computeLikelihoods(
+      float* reference,
+      std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses,
+      std::vector<float>& scores);
+
+  /** Set the basic camera intrinsic parameters. */
+  void
+  setCameraIntrinsicsParameters(int camera_width_in,
+                                int camera_height_in,
+                                float camera_fx_in,
+                                float camera_fy_in,
+                                float camera_cx_in,
+                                float camera_cy_in)
+  {
+    camera_width_ = camera_width_in;
+    camera_height_ = camera_height_in;
+    camera_fx_ = camera_fx_in;
+    camera_fy_ = camera_fy_in;
+    camera_cx_ = camera_cx_in;
+    camera_cy_ = camera_cy_in;
+  }
+
+  /** Get the basic camera intrinsic parameters. */
+  void
+  getCameraIntrinsicsParameters(int& camera_width_in,
+                                int& camera_height_in,
+                                float& camera_fx_in,
+                                float& camera_fy_in,
+                                float& camera_cx_in,
+                                float& camera_cy_in) const
+  {
+    camera_width_in = camera_width_;
+    camera_height_in = camera_height_;
+    camera_fx_in = camera_fx_;
+    camera_fy_in = camera_fy_;
+    camera_cx_in = camera_cx_;
+    camera_cy_in = camera_cy_;
+  }
+
+  /** Set the cost function to be used - one of 4 hard coded currently. */
+  void
+  setCostFunction(int which_cost_function_in)
+  {
+    which_cost_function_ = which_cost_function_in;
+  }
+
+  void
+  setSigma(double sigma_in)
+  {
+    sigma_ = sigma_in;
+  }
+
+  void
+  setFloorProportion(double floor_proportion_in)
+  {
+    floor_proportion_ = floor_proportion_in;
+  }
+
+  int
+  getRows() const
+  {
+    return rows_;
+  }
+
+  int
+  getCols() const
+  {
+    return cols_;
+  }
+
+  int
+  getRowHeight() const
+  {
+    return row_height_;
+  }
+
+  int
+  getColWidth() const
+  {
+    return col_width_;
+  }
+
+  int
+  getWidth() const
   {
-    class PCL_EXPORTS RangeLikelihood
-    {
-      public:
-        using Ptr = boost::shared_ptr<RangeLikelihood>;
-        using ConstPtr = boost::shared_ptr<const RangeLikelihood>;
-
-      public:
-        /**
-         * Create a new object to compute range image likelihoods.
-         *
-         * OpenGL is used to render the images. It is assumed that
-         * render buffers have already been setup. The area used is
-         * from 0,0 to cols*col_width,rows*row_height.
-         *
-         * @param rows - number of rows to use in the render buffer.
-         * @param cols - number of columns to use in the render buffer.
-         * @param row_height - height of the image for a single particle.
-         * @param col_width  - width of the image for a single particle.
-         * @param scene - a pointer to the scene that should be rendered when
-         *                computing likelihoods.
-         *
-         */
-        RangeLikelihood (int rows,
-                         int cols,
-                         int row_height,
-                         int col_width,
-                         Scene::Ptr scene);
-
-        /**
-         * Destroy the RangeLikelihood object and release any memory allocated.
-         */
-        ~RangeLikelihood ();
-
-        /**
-         * Computes the likelihood of reference for each of the provided poses.
-         *
-         * @param reference is a depth image.
-         * @param poses is a vector of the poses to test.
-         * @param scores is an output argument. The resulting log likelihoods will be written in score.
-         *
-         */
-        void
-        computeLikelihoods (float* reference,
-                            std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses,
-                            std::vector<float> & scores);
-
-        /**
-         * Set the basic camera intrinsic parameters
-         */
-        void
-        setCameraIntrinsicsParameters (int camera_width_in,
-                                       int camera_height_in,
-                                       float camera_fx_in,
-                                       float camera_fy_in,
-                                       float camera_cx_in,
-                                       float camera_cy_in)
-        {
-          camera_width_ = camera_width_in;
-          camera_height_ = camera_height_in;
-          camera_fx_ = camera_fx_in;
-          camera_fy_ = camera_fy_in;
-          camera_cx_ = camera_cx_in;
-          camera_cy_ = camera_cy_in;
-        }
-        
-        /**
-         * Get the basic camera intrinsic parameters
-         */
-        void
-        getCameraIntrinsicsParameters (int &camera_width_in,
-                                       int &camera_height_in,
-                                       float &camera_fx_in,
-                                       float &camera_fy_in,
-                                       float &camera_cx_in,
-                                       float &camera_cy_in) const
-        {
-          camera_width_in = camera_width_;
-          camera_height_in = camera_height_;
-          camera_fx_in = camera_fx_;
-          camera_fy_in = camera_fy_;
-          camera_cx_in = camera_cx_;
-          camera_cy_in = camera_cy_;
-        }
-        
-        /**
-         * Set the cost function to be used - one of 4 hard coded currently
-         */
-        void setCostFunction (int which_cost_function_in){  which_cost_function_ = which_cost_function_in;}
-        void setSigma (double sigma_in){  sigma_ = sigma_in;	}
-        void setFloorProportion (double floor_proportion_in){  floor_proportion_ = floor_proportion_in;}
-
-        int getRows () const {return rows_;}
-        int getCols () const {return cols_;}
-        int getRowHeight () const {return row_height_;}
-        int getColWidth () const {return col_width_;}
-        int getWidth () const {return width_;}
-        int getHeight () const {return height_;}
-
-        // Convenience function to return simulated RGB-D PointCloud
-        // Two modes:
-        // global=false - PointCloud is as would be captured by an RGB-D camera [default]
-        // global=true  - PointCloud is transformed into the model/world frame using the camera pose
-        void getPointCloud (pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc,
-              bool make_global, const Eigen::Isometry3d & pose, bool organized = false) const;
-
-        // Convenience function to return RangeImagePlanar containing
-        // simulated RGB-D:
-        void getRangeImagePlanar (pcl::RangeImagePlanar &rip) const;
-
-        // Add various types of noise to simulated RGB-D data
-        void addNoise ();
-        double sampleNormal (double sigma = 1.0);
-	
-        void
-        setComputeOnCPU(bool compute_on_cpu) { compute_likelihood_on_cpu_ = compute_on_cpu; }
-
-        void
-        setSumOnCPU(bool sum_on_cpu) { aggregate_on_cpu_ = sum_on_cpu; }
-
-        void
-        setUseColor(bool use_color) { use_color_ = use_color; }
-
-        const uint8_t*
-        getColorBuffer () const;
-
-        const float*
-        getDepthBuffer () const;
-
-        const float*
-        getScoreBuffer () const;
-
-        float 
-        getZNear () const { return z_near_; }
-        
-        float 
-        getZFar () const { return z_far_; }
-        
-        void 
-        setZNear (float z){ z_near_ = z; }
-        
-        void 
-        setZFar (float z){ z_far_ = z; }
-        
-      private:
-        /**
-         * Evaluate the likelihood/score for a set of particles
-         *
-         * @param[in] reference - input Measurement depth image (raw data)
-         * @param[out] scores - output score
-         */
-        void
-        computeScores (float* reference,
-                       std::vector<float> & scores);
-
-        void
-        computeScoresShader (float* reference);
-
-        void
-        render (const std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > & poses);
-
-        void
-        drawParticles (std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses);
-
-        void
-        applyCameraTransform (const Eigen::Isometry3d & pose);
-
-        void
-        applyCameraTransform (const Camera & camera);
-
-        void
-        setupProjectionMatrix ();
-
-        Scene::Ptr scene_;
-        int rows_;
-        int cols_;
-        int row_height_;
-        int col_width_;
-        int width_;
-        int height_;
-        float* depth_buffer_;
-        uint8_t* color_buffer_;
-
-        // Camera Intrinsic Parameters
-        int camera_width_;
-        int camera_height_;
-        float camera_fx_;
-        float camera_fy_;
-        float camera_cx_;
-        float camera_cy_;
-
-        // min and max range of the rgbd sensor
-        // everything outside this doesn't appear in depth images
-        float z_near_;
-        float z_far_;
-
-        // For caching only, not part of observable state.
-        mutable bool depth_buffer_dirty_;
-        mutable bool color_buffer_dirty_;
-        mutable bool score_buffer_dirty_;
-	
-	int which_cost_function_;
-	double floor_proportion_;
-	double sigma_;
-
-        GLuint fbo_;
-        GLuint score_fbo_;
-
-        GLuint depth_render_buffer_;
-        GLuint color_render_buffer_;
-        GLuint color_texture_;
-        GLuint depth_texture_;
-        GLuint score_texture_;
-        GLuint score_summarized_texture_;
-        GLuint sensor_texture_;
-        GLuint likelihood_texture_;
-
-        bool compute_likelihood_on_cpu_;
-        bool aggregate_on_cpu_;
-        bool use_instancing_;
-        bool generate_color_image_;
-        bool use_color_;
-
-        gllib::Program::Ptr likelihood_program_;
-        GLuint quad_vbo_;
-        std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > vertices_;
-        float* score_buffer_;
-        Quad quad_;
-        SumReduce sum_reduce_;
-    };
-
-    template<class T> T
-    sqr(T val) { return val*val; }
-    
-  } // namespace - simulation
-
-} // namespace - pcl
+    return width_;
+  }
+
+  int
+  getHeight() const
+  {
+    return height_;
+  }
+
+  /** Convenience function to return simulated RGB-D PointCloud.
+   *
+   *  Two modes:
+   *    * global=false - PointCloud is as would be captured by an RGB-D camera [default]
+   *    * global=true  - PointCloud is transformed into the model/world frame using the
+   *                     camera pose
+   */
+  void
+  getPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc,
+                bool make_global,
+                const Eigen::Isometry3d& pose,
+                bool organized = false) const;
+
+  /** Convenience function to return RangeImagePlanar containing simulated RGB-D. */
+  void
+  getRangeImagePlanar(pcl::RangeImagePlanar& rip) const;
+
+  /** Add various types of noise to simulated RGB-D data. */
+  void
+  addNoise();
+
+  double
+  sampleNormal(double sigma = 1.0);
+
+  void
+  setComputeOnCPU(bool compute_on_cpu)
+  {
+    compute_likelihood_on_cpu_ = compute_on_cpu;
+  }
+
+  void
+  setSumOnCPU(bool sum_on_cpu)
+  {
+    aggregate_on_cpu_ = sum_on_cpu;
+  }
+
+  void
+  setUseColor(bool use_color)
+  {
+    use_color_ = use_color;
+  }
+
+  const uint8_t*
+  getColorBuffer() const;
+
+  const float*
+  getDepthBuffer() const;
+
+  const float*
+  getScoreBuffer() const;
+
+  float
+  getZNear() const
+  {
+    return z_near_;
+  }
+
+  float
+  getZFar() const
+  {
+    return z_far_;
+  }
+
+  void
+  setZNear(float z)
+  {
+    z_near_ = z;
+  }
+
+  void
+  setZFar(float z)
+  {
+    z_far_ = z;
+  }
+
+private:
+  /** Evaluate the likelihood/score for a set of particles.
+   *
+   * \param[in] reference input measurement depth image (raw data)
+   * \param[out] scores output score
+   */
+  void
+  computeScores(float* reference, std::vector<float>& scores);
+
+  void
+  computeScoresShader(float* reference);
+
+  void
+  render(const std::vector<Eigen::Isometry3d,
+                           Eigen::aligned_allocator<Eigen::Isometry3d>>& poses);
+
+  void
+  drawParticles(std::vector<Eigen::Isometry3d,
+                            Eigen::aligned_allocator<Eigen::Isometry3d>> poses);
+
+  void
+  applyCameraTransform(const Eigen::Isometry3d& pose);
+
+  void
+  applyCameraTransform(const Camera& camera);
+
+  void
+  setupProjectionMatrix();
+
+  Scene::Ptr scene_;
+  int rows_;
+  int cols_;
+  int row_height_;
+  int col_width_;
+  int width_;
+  int height_;
+  float* depth_buffer_;
+  uint8_t* color_buffer_;
+
+  // Camera Intrinsic Parameters
+  int camera_width_;
+  int camera_height_;
+  float camera_fx_;
+  float camera_fy_;
+  float camera_cx_;
+  float camera_cy_;
+
+  // min and max range of the rgbd sensor
+  // everything outside this doesn't appear in depth images
+  float z_near_;
+  float z_far_;
+
+  // For caching only, not part of observable state.
+  mutable bool depth_buffer_dirty_;
+  mutable bool color_buffer_dirty_;
+  mutable bool score_buffer_dirty_;
+
+  int which_cost_function_;
+  double floor_proportion_;
+  double sigma_;
+
+  GLuint fbo_;
+  GLuint score_fbo_;
+
+  GLuint depth_render_buffer_;
+  GLuint color_render_buffer_;
+  GLuint color_texture_;
+  GLuint depth_texture_;
+  GLuint score_texture_;
+  GLuint score_summarized_texture_;
+  GLuint sensor_texture_;
+  GLuint likelihood_texture_;
+
+  bool compute_likelihood_on_cpu_;
+  bool aggregate_on_cpu_;
+  bool use_instancing_;
+  bool generate_color_image_;
+  bool use_color_;
+
+  gllib::Program::Ptr likelihood_program_;
+  GLuint quad_vbo_;
+  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> vertices_;
+  float* score_buffer_;
+  Quad quad_;
+  SumReduce sum_reduce_;
+};
+
+template <class T>
+T
+sqr(T val)
+{
+  return val * val;
+}
+
+} // namespace simulation
+} // namespace pcl
diff --git a/simulation/include/pcl/simulation/scene.h b/simulation/include/pcl/simulation/scene.h
index 19b1e56628d..7c6fd503d69 100644
--- a/simulation/include/pcl/simulation/scene.h
+++ b/simulation/include/pcl/simulation/scene.h
@@ -15,31 +15,29 @@
 #include <pcl/simulation/camera.h>
 #include <pcl/simulation/model.h>
 
-namespace pcl
-{
-  namespace simulation
-  {
-    class PCL_EXPORTS Scene
-    {
-    public:
-      using Ptr = boost::shared_ptr<Scene>;
-      using ConstPtr = boost::shared_ptr<Scene>;
-
-      void
-      draw ();
-
-      void
-      add (Model::Ptr model);
-
-      void
-      addCompleteModel (std::vector<Model::Ptr> model);
-
-      void
-      clear ();
-
-    private:
-      std::vector<Model::Ptr> models_;
-    };
-  
-  } // namespace - simulation
-} // namespace - pcl
+namespace pcl {
+namespace simulation {
+
+class PCL_EXPORTS Scene {
+public:
+  using Ptr = boost::shared_ptr<Scene>;
+  using ConstPtr = boost::shared_ptr<Scene>;
+
+  void
+  draw();
+
+  void
+  add(Model::Ptr model);
+
+  void
+  addCompleteModel(std::vector<Model::Ptr> model);
+
+  void
+  clear();
+
+private:
+  std::vector<Model::Ptr> models_;
+};
+
+} // namespace simulation
+} // namespace pcl
diff --git a/simulation/include/pcl/simulation/sum_reduce.h b/simulation/include/pcl/simulation/sum_reduce.h
index 3864b980b43..4424c92518f 100644
--- a/simulation/include/pcl/simulation/sum_reduce.h
+++ b/simulation/include/pcl/simulation/sum_reduce.h
@@ -11,53 +11,54 @@
 
 #include <pcl/pcl_config.h>
 #ifdef OPENGL_IS_A_FRAMEWORK
-# include <OpenGL/gl.h>
+#include <OpenGL/gl.h>
 #else
-# include <GL/gl.h>
+#include <GL/gl.h>
 #endif
 
 #include <pcl/simulation/glsl_shader.h>
 #include <pcl/simulation/model.h>
 
-namespace pcl
-{
-  namespace simulation
-  {
-    /** \brief Implements a parallel summation of float arrays using GLSL.
-     * The input array is provided as a float texture and the summation
-     * is performed over set number of levels, where each level halves each
-     * dimension.
-     *
-     * \author Hordur Johannsson
-     * \ingroup simulation
-     */
-    class PCL_EXPORTS SumReduce
-    {
-      public:
-        /** \brief Construct a new summation object for an array of given size.
-         * \param width[in] the width of the input array.
-         * \param width[in] the height of the input array.
-         * \param levels[in] the number of levels to carry out the reduction.
-         */
-        SumReduce (int width, int height, int levels);
-
-        /** \brief Release any allocated resources. */
-        ~SumReduce ();
-
-        /** \brief Reduce the array with summation over set number of levels.
-         *  \param[in] input_array name of the input texture.
-         *  \param[out] a pointer to an array that can store the summation result.
-         */
-        void sum (GLuint input_array, float* output_array);
-
-      private:
-        GLuint fbo_;
-        GLuint* arrays_;
-        Quad quad_;
-        int levels_;
-        int width_;
-        int height_;
-        gllib::Program::Ptr sum_program_;
-    };
-  } // namespace - simulation
-} // namespace - pcl
+namespace pcl {
+namespace simulation {
+
+/** Implements a parallel summation of float arrays using GLSL.
+ *  The input array is provided as a float texture and the summation is performed over
+ *  set number of levels, where each level halves each dimension.
+ *
+ * \author Hordur Johannsson
+ * \ingroup simulation
+ */
+class PCL_EXPORTS SumReduce {
+public:
+  /** Construct a new summation object for an array of given size.
+   *
+   * \param width[in] the width of the input array
+   * \param width[in] the height of the input array
+   * \param levels[in] the number of levels to carry out the reduction
+   */
+  SumReduce(int width, int height, int levels);
+
+  /** Release any allocated resources. */
+  ~SumReduce();
+
+  /** Reduce the array with summation over set number of levels.
+   *
+   * \param[in] input_array name of the input texture
+   * \param[out] a pointer to an array that can store the summation result
+   */
+  void
+  sum(GLuint input_array, float* output_array);
+
+private:
+  GLuint fbo_;
+  GLuint* arrays_;
+  Quad quad_;
+  int levels_;
+  int width_;
+  int height_;
+  gllib::Program::Ptr sum_program_;
+};
+
+} // namespace simulation
+} // namespace pcl
diff --git a/simulation/src/camera.cpp b/simulation/src/camera.cpp
index 0d51b435edd..f0244281e07 100644
--- a/simulation/src/camera.cpp
+++ b/simulation/src/camera.cpp
@@ -5,37 +5,40 @@ using namespace Eigen;
 using namespace pcl::simulation;
 
 void
-pcl::simulation::Camera::move (double vx, double vy, double vz)
+pcl::simulation::Camera::move(double vx, double vy, double vz)
 {
   Vector3d v;
   v << vx, vy, vz;
-  pose_.pretranslate (pose_.rotation ()*v);
-  x_ = pose_.translation ().x ();
-  y_ = pose_.translation ().y ();
-  z_ = pose_.translation ().z ();
+  pose_.pretranslate(pose_.rotation() * v);
+  x_ = pose_.translation().x();
+  y_ = pose_.translation().y();
+  z_ = pose_.translation().z();
 }
 
 void
-pcl::simulation::Camera::updatePose ()
+pcl::simulation::Camera::updatePose()
 {
   Matrix3d m;
-  m = AngleAxisd (yaw_, Vector3d::UnitZ ())
-    * AngleAxisd (pitch_, Vector3d::UnitY ())
-    * AngleAxisd (roll_, Vector3d::UnitX ());
+  m = AngleAxisd(yaw_, Vector3d::UnitZ()) * AngleAxisd(pitch_, Vector3d::UnitY()) *
+      AngleAxisd(roll_, Vector3d::UnitX());
 
-  pose_.setIdentity ();
+  pose_.setIdentity();
   pose_ *= m;
-  
+
   Vector3d v;
   v << x_, y_, z_;
-  pose_.translation () = v;
+  pose_.translation() = v;
 }
 
 void
-pcl::simulation::Camera::setParameters (int width, int height,
-                                        float fx, float fy,
-                                        float cx, float cy,
-                                        float z_near, float z_far)
+pcl::simulation::Camera::setParameters(int width,
+                                       int height,
+                                       float fx,
+                                       float fy,
+                                       float cx,
+                                       float cy,
+                                       float z_near,
+                                       float z_far)
 {
   width_ = width;
   height_ = height;
@@ -46,18 +49,24 @@ pcl::simulation::Camera::setParameters (int width, int height,
   z_near_ = z_near;
   z_far_ = z_far;
 
+  // clang-format off
   float z_nf = (z_near_-z_far_);
-  projection_matrix_ <<  2.0f*fx_/width_,  0,                 1.0f-(2.0f*cx_/width_),     0,
-                         0,                2.0f*fy_/height_,  1.0f-(2.0f*cy_/height_),    0,
-                         0,                0,                (z_far_+z_near_)/z_nf,  2.0f*z_near_*z_far_/z_nf,
-                         0,                0,                -1.0f,                  0;
+  projection_matrix_ <<  2.0f*fx_/width_,  0,                 1.0f-(2.0f*cx_/width_),   0,
+                         0,                2.0f*fy_/height_,  1.0f-(2.0f*cy_/height_),  0,
+                         0,                0,                 (z_far_+z_near_)/z_nf,    2.0f*z_near_*z_far_/z_nf,
+                         0,                0,                 -1.0f,                    0;
+  // clang-format on
 }
 
 void
-pcl::simulation::Camera::initializeCameraParameters ()
+pcl::simulation::Camera::initializeCameraParameters()
 {
-  setParameters (640, 480,
-                 576.09757860f, 576.09757860f,
-                 321.06398107f, 242.97676897f,
-                 0.7f, 20.0f);
+  setParameters(640,
+                480,
+                576.09757860f,
+                576.09757860f,
+                321.06398107f,
+                242.97676897f,
+                0.7f,
+                20.0f);
 }
diff --git a/simulation/src/glsl_shader.cpp b/simulation/src/glsl_shader.cpp
index 99ccc980b83..d32b650fcd2 100644
--- a/simulation/src/glsl_shader.cpp
+++ b/simulation/src/glsl_shader.cpp
@@ -5,240 +5,243 @@
  *      Author: hordurj
  */
 
-#include <pcl/simulation/glsl_shader.h>
-#include <iostream>
 #include <fstream>
+#include <iostream>
+#include <pcl/simulation/glsl_shader.h>
 
 using namespace pcl::simulation::gllib;
 
 char*
-readTextFile (const char* filename)
+readTextFile(const char* filename)
 {
   using namespace std;
   char* buf = nullptr;
   ifstream file;
-  file.open (filename, ios::in|ios::binary|ios::ate);
-  if (file.is_open ())
-  {
+  file.open(filename, ios::in | ios::binary | ios::ate);
+  if (file.is_open()) {
     ifstream::pos_type size;
     size = file.tellg();
-    buf = new char[size + static_cast<ifstream::pos_type> (1)];
-    file.seekg (0, ios::beg);
-    file.read (buf, size);
-    file.close ();
+    buf = new char[size + static_cast<ifstream::pos_type>(1)];
+    file.seekg(0, ios::beg);
+    file.read(buf, size);
+    file.close();
     buf[size] = 0;
   }
   return buf;
 }
 
-pcl::simulation::gllib::Program::Program ()
-{
-  program_id_ = glCreateProgram ();
-}
+pcl::simulation::gllib::Program::Program() { program_id_ = glCreateProgram(); }
 
-pcl::simulation::gllib::Program::~Program ()
-{
-}
+pcl::simulation::gllib::Program::~Program() {}
 
 int
-pcl::simulation::gllib::Program::getUniformLocation (const std::string& name)
+pcl::simulation::gllib::Program::getUniformLocation(const std::string& name)
 {
-  return glGetUniformLocation (program_id_, name.c_str ());
+  return glGetUniformLocation(program_id_, name.c_str());
 }
 
 void
-pcl::simulation::gllib::Program::setUniform (const std::string& name, const Eigen::Vector2f& v)
+pcl::simulation::gllib::Program::setUniform(const std::string& name,
+                                            const Eigen::Vector2f& v)
 {
-  GLuint loc = getUniformLocation (name.c_str ());
-  glUniform2f(loc, v (0), v (1));
+  GLuint loc = getUniformLocation(name.c_str());
+  glUniform2f(loc, v(0), v(1));
 }
 
 void
-pcl::simulation::gllib::Program::setUniform (const std::string& name, const Eigen::Vector3f& v)
+pcl::simulation::gllib::Program::setUniform(const std::string& name,
+                                            const Eigen::Vector3f& v)
 {
-  GLuint loc = getUniformLocation (name.c_str ());
-  glUniform3f(loc, v (0), v (1), v (2));
+  GLuint loc = getUniformLocation(name.c_str());
+  glUniform3f(loc, v(0), v(1), v(2));
 }
 
 void
-pcl::simulation::gllib::Program::setUniform(const std::string& name, const Eigen::Vector4f& v)
+pcl::simulation::gllib::Program::setUniform(const std::string& name,
+                                            const Eigen::Vector4f& v)
 {
-  GLuint loc = getUniformLocation (name.c_str ());
-  glUniform4f (loc, v (0), v (1), v (2), v (4));
+  GLuint loc = getUniformLocation(name.c_str());
+  glUniform4f(loc, v(0), v(1), v(2), v(4));
 }
 
 void
-pcl::simulation::gllib::Program::setUniform (const std::string& name, const Eigen::Matrix3f& v)
+pcl::simulation::gllib::Program::setUniform(const std::string& name,
+                                            const Eigen::Matrix3f& v)
 {
-  GLuint loc = getUniformLocation (name.c_str ());
-  glUniformMatrix3fv (loc, 1, false, v.data ());
+  GLuint loc = getUniformLocation(name.c_str());
+  glUniformMatrix3fv(loc, 1, false, v.data());
 }
 
 void
-pcl::simulation::gllib::Program::setUniform (const std::string& name, const Eigen::Matrix4f& v)
+pcl::simulation::gllib::Program::setUniform(const std::string& name,
+                                            const Eigen::Matrix4f& v)
 {
-  GLuint loc = getUniformLocation (name.c_str ());
-  glUniformMatrix4fv (loc, 1, false, v.data ());
+  GLuint loc = getUniformLocation(name.c_str());
+  glUniformMatrix4fv(loc, 1, false, v.data());
 }
 
 void
-pcl::simulation::gllib::Program::setUniform (const std::string& name, float v)
+pcl::simulation::gllib::Program::setUniform(const std::string& name, float v)
 {
-  GLuint loc = getUniformLocation (name.c_str ());
-  glUniform1f (loc, v);
+  GLuint loc = getUniformLocation(name.c_str());
+  glUniform1f(loc, v);
 }
 
 void
-pcl::simulation::gllib::Program::setUniform (const std::string& name, int v)
+pcl::simulation::gllib::Program::setUniform(const std::string& name, int v)
 {
-  GLuint loc = getUniformLocation (name.c_str ());
-  glUniform1i (loc, v);
+  GLuint loc = getUniformLocation(name.c_str());
+  glUniform1i(loc, v);
 }
 
 void
-pcl::simulation::gllib::Program::setUniform (const std::string& name, bool v)
+pcl::simulation::gllib::Program::setUniform(const std::string& name, bool v)
 {
-  GLuint loc = getUniformLocation (name.c_str ());
-  glUniform1i(loc, (v?1:0));
+  GLuint loc = getUniformLocation(name.c_str());
+  glUniform1i(loc, (v ? 1 : 0));
 }
 
 bool
-pcl::simulation::gllib::Program::addShaderText (const std::string& text, ShaderType shader_type)
+pcl::simulation::gllib::Program::addShaderText(const std::string& text,
+                                               ShaderType shader_type)
 {
   GLuint id;
   GLint compile_status;
-  id = glCreateShader (shader_type);
-  const char* source_list = text.c_str ();
+  id = glCreateShader(shader_type);
+  const char* source_list = text.c_str();
 
-  glShaderSource (id, 1, &source_list, nullptr);
+  glShaderSource(id, 1, &source_list, nullptr);
 
-  glCompileShader (id);
-  printShaderInfoLog (id);
-  glGetShaderiv (id, GL_COMPILE_STATUS, &compile_status);
-  if (compile_status == GL_FALSE) return false;
+  glCompileShader(id);
+  printShaderInfoLog(id);
+  glGetShaderiv(id, GL_COMPILE_STATUS, &compile_status);
+  if (compile_status == GL_FALSE)
+    return false;
 
-  if (getGLError () != GL_NO_ERROR) return false;
+  if (getGLError() != GL_NO_ERROR)
+    return false;
 
-  glAttachShader (program_id_, id);
+  glAttachShader(program_id_, id);
   return true;
 }
 
 bool
-pcl::simulation::gllib::Program::addShaderFile (const std::string& filename, ShaderType shader_type)
+pcl::simulation::gllib::Program::addShaderFile(const std::string& filename,
+                                               ShaderType shader_type)
 {
-  char* text = readTextFile (filename.c_str ());
-  if(text == nullptr)  return (false);
+  char* text = readTextFile(filename.c_str());
+  if (text == nullptr)
+    return (false);
 
-  bool rval = addShaderText (text, shader_type);
-  delete [] text;
+  bool rval = addShaderText(text, shader_type);
+  delete[] text;
   return rval;
 }
 
 bool
-pcl::simulation::gllib::Program::link ()
+pcl::simulation::gllib::Program::link()
 {
-  glLinkProgram (program_id_);
-  printProgramInfoLog (program_id_);
+  glLinkProgram(program_id_);
+  printProgramInfoLog(program_id_);
 
-  return getGLError () == GL_NO_ERROR;
+  return getGLError() == GL_NO_ERROR;
 }
 
-void pcl::simulation::gllib::Program::use ()
+void
+pcl::simulation::gllib::Program::use()
 {
-  glUseProgram (program_id_);
+  glUseProgram(program_id_);
 }
 
-GLenum pcl::simulation::gllib::getGLError ()
+GLenum
+pcl::simulation::gllib::getGLError()
 {
   GLenum last_error = GL_NO_ERROR;
-  GLenum error = glGetError ();
-  while (error != GL_NO_ERROR)
-  {
+  GLenum error = glGetError();
+  while (error != GL_NO_ERROR) {
     last_error = error;
     std::cout << "Error: OpenGL: " << gluErrorString(error) << std::endl;
-    error = glGetError ();
+    error = glGetError();
   }
   return last_error;
 }
 
 void
-pcl::simulation::gllib::printShaderInfoLog (GLuint shader)
+pcl::simulation::gllib::printShaderInfoLog(GLuint shader)
 {
   GLsizei max_length;
   GLsizei length;
   GLchar* info_log;
 
-  glGetShaderiv (shader, GL_INFO_LOG_LENGTH, &length);
+  glGetShaderiv(shader, GL_INFO_LOG_LENGTH, &length);
   max_length = length;
-  info_log = new GLchar[length+1];
+  info_log = new GLchar[length + 1];
 
-  glGetShaderInfoLog (shader, max_length, &length, info_log);
+  glGetShaderInfoLog(shader, max_length, &length, info_log);
 
   info_log[max_length] = 0;
 
   std::cout << "Shader info log: " << std::endl << info_log << std::endl;
 
-  delete [] info_log;
+  delete[] info_log;
 }
 
 void
-pcl::simulation::gllib::printProgramInfoLog (GLuint program)
+pcl::simulation::gllib::printProgramInfoLog(GLuint program)
 {
   GLsizei max_length;
   GLsizei length;
   GLchar* info_log;
 
-  glGetProgramiv (program, GL_INFO_LOG_LENGTH, &length);
+  glGetProgramiv(program, GL_INFO_LOG_LENGTH, &length);
   max_length = length;
-  info_log = new GLchar[length+1];
+  info_log = new GLchar[length + 1];
 
-  glGetProgramInfoLog (program, max_length, &length, info_log);
+  glGetProgramInfoLog(program, max_length, &length, info_log);
 
   info_log[max_length] = 0;
 
   std::cout << "Program info log: " << std::endl << info_log << std::endl;
 
-  delete [] info_log;
+  delete[] info_log;
 }
 
 Program::Ptr
-pcl::simulation::gllib::Program::loadProgramFromText (const std::string& vertex_shader_text, const std::string& fragment_shader_text)
+pcl::simulation::gllib::Program::loadProgramFromText(
+    const std::string& vertex_shader_text, const std::string& fragment_shader_text)
 {
   // Load shader
-  Program::Ptr program = gllib::Program::Ptr (new gllib::Program ());
-  if (!program->addShaderText (vertex_shader_text, gllib::VERTEX))
-  {
+  Program::Ptr program = gllib::Program::Ptr(new gllib::Program());
+  if (!program->addShaderText(vertex_shader_text, gllib::VERTEX)) {
     std::cerr << "Failed loading vertex shader" << std::endl;
   }
 
   // TODO: to remove file dependency include the shader source in the binary
-  if (!program->addShaderFile (fragment_shader_text, gllib::FRAGMENT))
-  {
+  if (!program->addShaderFile(fragment_shader_text, gllib::FRAGMENT)) {
     std::cerr << "Failed loading fragment shader" << std::endl;
   }
 
-  program->link ();
+  program->link();
 
   return program;
 }
 
 Program::Ptr
-pcl::simulation::gllib::Program::loadProgramFromFile (const std::string& vertex_shader_file, const std::string& fragment_shader_file)
+pcl::simulation::gllib::Program::loadProgramFromFile(
+    const std::string& vertex_shader_file, const std::string& fragment_shader_file)
 {
   // Load shader
-  Program::Ptr program = gllib::Program::Ptr (new gllib::Program ());
-  if (!program->addShaderFile (vertex_shader_file, gllib::VERTEX))
-  {
+  Program::Ptr program = gllib::Program::Ptr(new gllib::Program());
+  if (!program->addShaderFile(vertex_shader_file, gllib::VERTEX)) {
     std::cerr << "Failed loading vertex shader" << std::endl;
   }
 
   // TODO: to remove file dependency include the shader source in the binary
-  if (!program->addShaderFile (fragment_shader_file, gllib::FRAGMENT))
-  {
+  if (!program->addShaderFile(fragment_shader_file, gllib::FRAGMENT)) {
     std::cerr << "Failed loading fragment shader" << std::endl;
   }
 
-  program->link ();
+  program->link();
 
   return program;
 }
diff --git a/simulation/src/model.cpp b/simulation/src/model.cpp
index 2c397bac2c9..fece4e56110 100644
--- a/simulation/src/model.cpp
+++ b/simulation/src/model.cpp
@@ -1,338 +1,333 @@
 #include <pcl/simulation/model.h>
 using namespace pcl::simulation;
 
-pcl::simulation::TriangleMeshModel::TriangleMeshModel (pcl::PolygonMesh::Ptr plg)
+pcl::simulation::TriangleMeshModel::TriangleMeshModel(pcl::PolygonMesh::Ptr plg)
 {
   Vertices vertices;
   Indices indices;
 
   bool found_rgb = false;
-  for (const auto &field : plg->cloud.fields)
+  for (const auto& field : plg->cloud.fields)
     if (field.name == "rgb")
       found_rgb = true;
 
-  if (found_rgb)
-  {
+  if (found_rgb) {
     pcl::PointCloud<pcl::PointXYZRGB> newcloud;
-    pcl::fromPCLPointCloud2 (plg->cloud, newcloud);
+    pcl::fromPCLPointCloud2(plg->cloud, newcloud);
 
     PCL_DEBUG("RGB Triangle mesh: ");
-    PCL_DEBUG("Mesh polygons: %ld", plg->polygons.size ());
-    PCL_DEBUG("Mesh points: %ld", newcloud.points.size ());
+    PCL_DEBUG("Mesh polygons: %ld", plg->polygons.size());
+    PCL_DEBUG("Mesh points: %ld", newcloud.points.size());
 
     Eigen::Vector4f tmp;
-    for(const auto &polygon : plg->polygons)
-    {
-      for(const unsigned int &point : polygon.vertices)
-      {
+    for (const auto& polygon : plg->polygons) {
+      for (const unsigned int& point : polygon.vertices) {
         tmp = newcloud.points[point].getVector4fMap();
-        vertices.push_back (Vertex (Eigen::Vector3f (tmp (0), tmp (1), tmp (2)),
-                                    Eigen::Vector3f (newcloud.points[point].r/255.0f,
-                                                     newcloud.points[point].g/255.0f,
-                                                     newcloud.points[point].b/255.0f)));
-        indices.push_back (indices.size ());
+        vertices.push_back(Vertex(Eigen::Vector3f(tmp(0), tmp(1), tmp(2)),
+                                  Eigen::Vector3f(newcloud.points[point].r / 255.0f,
+                                                  newcloud.points[point].g / 255.0f,
+                                                  newcloud.points[point].b / 255.0f)));
+        indices.push_back(indices.size());
       }
     }
   }
-  else
-  {
+  else {
     pcl::PointCloud<pcl::PointXYZ> newcloud;
-    pcl::fromPCLPointCloud2 (plg->cloud, newcloud);
+    pcl::fromPCLPointCloud2(plg->cloud, newcloud);
     Eigen::Vector4f tmp;
-    for(const auto &polygon : plg->polygons)
-    {
-      for(const unsigned int &point : polygon.vertices)
-      {
+    for (const auto& polygon : plg->polygons) {
+      for (const unsigned int& point : polygon.vertices) {
         tmp = newcloud.points[point].getVector4fMap();
-        vertices.push_back (Vertex (Eigen::Vector3f (tmp (0), tmp (1), tmp (2)),
-                                    Eigen::Vector3f (1.0, 1.0, 1.0)));
-        indices.push_back (indices.size ());
+        vertices.push_back(Vertex(Eigen::Vector3f(tmp(0), tmp(1), tmp(2)),
+                                  Eigen::Vector3f(1.0, 1.0, 1.0)));
+        indices.push_back(indices.size());
       }
     }
   }
 
-  PCL_DEBUG("Vertices: %ld", vertices.size ());
-  PCL_DEBUG("Indices: %ld", indices.size ());
-
-  glGenBuffers (1, &vbo_);
-  glBindBuffer (GL_ARRAY_BUFFER, vbo_);
-  glBufferData (GL_ARRAY_BUFFER, vertices.size () * sizeof (vertices[0]), &(vertices[0]), GL_STATIC_DRAW);
-  glBindBuffer (GL_ARRAY_BUFFER, 0);
-
-  glGenBuffers (1, &ibo_);
-  glBindBuffer (GL_ELEMENT_ARRAY_BUFFER, ibo_);
-  glBufferData (GL_ELEMENT_ARRAY_BUFFER, indices.size () * sizeof (indices[0]), &(indices[0]), GL_STATIC_DRAW);
-  glBindBuffer (GL_ELEMENT_ARRAY_BUFFER, 0);
-
-  if (indices.size () > std::numeric_limits<GLuint>::max ())
+  PCL_DEBUG("Vertices: %ld", vertices.size());
+  PCL_DEBUG("Indices: %ld", indices.size());
+
+  glGenBuffers(1, &vbo_);
+  glBindBuffer(GL_ARRAY_BUFFER, vbo_);
+  glBufferData(GL_ARRAY_BUFFER,
+               vertices.size() * sizeof(vertices[0]),
+               &(vertices[0]),
+               GL_STATIC_DRAW);
+  glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+  glGenBuffers(1, &ibo_);
+  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, ibo_);
+  glBufferData(GL_ELEMENT_ARRAY_BUFFER,
+               indices.size() * sizeof(indices[0]),
+               &(indices[0]),
+               GL_STATIC_DRAW);
+  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
+
+  if (indices.size() > std::numeric_limits<GLuint>::max())
     PCL_THROW_EXCEPTION(PCLException, "Too many vertices");
 
-  size_ = static_cast<GLuint>(indices.size ());
+  size_ = static_cast<GLuint>(indices.size());
 }
 
 void
-pcl::simulation::TriangleMeshModel::draw ()
+pcl::simulation::TriangleMeshModel::draw()
 {
-  glEnable (GL_DEPTH_TEST);
+  glEnable(GL_DEPTH_TEST);
 
-  glEnableClientState (GL_VERTEX_ARRAY);
-  glEnableClientState (GL_COLOR_ARRAY);
-  //glEnableClientState(GL_NORMAL_ARRAY);
-  glBindBuffer (GL_ARRAY_BUFFER, vbo_);
-  glBindBuffer (GL_ELEMENT_ARRAY_BUFFER, ibo_);
+  glEnableClientState(GL_VERTEX_ARRAY);
+  glEnableClientState(GL_COLOR_ARRAY);
+  // glEnableClientState(GL_NORMAL_ARRAY);
+  glBindBuffer(GL_ARRAY_BUFFER, vbo_);
+  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, ibo_);
 
-  glVertexPointer (3, GL_FLOAT, sizeof (Vertex), nullptr);
-  glColorPointer (3, GL_FLOAT, sizeof (Vertex), reinterpret_cast<GLvoid*> (12));
+  glVertexPointer(3, GL_FLOAT, sizeof(Vertex), nullptr);
+  glColorPointer(3, GL_FLOAT, sizeof(Vertex), reinterpret_cast<GLvoid*>(12));
 
-  // glNormalPointer(GL_FLOAT, sizeof(Vertex), (GLvoid*)((char*)&(vertices_[0].norm)-(char*)&(vertices_[0].pos)));
+  // glNormalPointer(GL_FLOAT, sizeof(Vertex),
+  // (GLvoid*)((char*)&(vertices_[0].norm)-(char*)&(vertices_[0].pos)));
   //  glDrawElements(GL_TRIANGLES, indices_.size(), GL_UNSIGNED_INT, 0);
-  glDrawElements (GL_TRIANGLES, size_, GL_UNSIGNED_INT, nullptr);
-  glDisableClientState (GL_VERTEX_ARRAY);
-  glDisableClientState (GL_COLOR_ARRAY);
-  //glDisableClientState(GL_NORMAL_ARRAY);
-  glVertexPointer (3, GL_FLOAT, 0, nullptr);
-  glColorPointer (3, GL_FLOAT, 0, nullptr);
-  //glNormalPointer(GL_FLOAT, 0, 0);
-
-  glBindBuffer (GL_ARRAY_BUFFER, 0);
-  glBindBuffer (GL_ELEMENT_ARRAY_BUFFER, 0);
+  glDrawElements(GL_TRIANGLES, size_, GL_UNSIGNED_INT, nullptr);
+  glDisableClientState(GL_VERTEX_ARRAY);
+  glDisableClientState(GL_COLOR_ARRAY);
+  // glDisableClientState(GL_NORMAL_ARRAY);
+  glVertexPointer(3, GL_FLOAT, 0, nullptr);
+  glColorPointer(3, GL_FLOAT, 0, nullptr);
+  // glNormalPointer(GL_FLOAT, 0, 0);
+
+  glBindBuffer(GL_ARRAY_BUFFER, 0);
+  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
 }
 
-pcl::simulation::TriangleMeshModel::~TriangleMeshModel ()
+pcl::simulation::TriangleMeshModel::~TriangleMeshModel()
 {
-  if (glIsBuffer (vbo_) == GL_TRUE)
-    glDeleteBuffers (1, &vbo_);
+  if (glIsBuffer(vbo_) == GL_TRUE)
+    glDeleteBuffers(1, &vbo_);
 
-  if (glIsBuffer (ibo_) == GL_TRUE)
-    glDeleteBuffers (1, &ibo_);
+  if (glIsBuffer(ibo_) == GL_TRUE)
+    glDeleteBuffers(1, &ibo_);
 }
 
 // Create a PolygonMeshModel by converting the PolygonMesh to our format
-pcl::simulation::PolygonMeshModel::PolygonMeshModel (GLenum mode, pcl::PolygonMesh::Ptr plg) : mode_ (mode)
+pcl::simulation::PolygonMeshModel::PolygonMeshModel(GLenum mode,
+                                                    pcl::PolygonMesh::Ptr plg)
+: mode_(mode)
 {
-  bool found_rgb=false;
-  for (const auto &field : plg->cloud.fields)
-    if ((field.name == "rgb") || (field.name == "rgba"))
-    {
+  bool found_rgb = false;
+  for (const auto& field : plg->cloud.fields)
+    if ((field.name == "rgb") || (field.name == "rgba")) {
       found_rgb = true;
       break;
     }
 
-  if (found_rgb)
-  {
-    pcl::PointCloud<pcl::PointXYZRGB> newcloud;  
-    pcl::fromPCLPointCloud2 (plg->cloud, newcloud);
+  if (found_rgb) {
+    pcl::PointCloud<pcl::PointXYZRGB> newcloud;
+    pcl::fromPCLPointCloud2(plg->cloud, newcloud);
     Eigen::Vector4f tmp;
-    for(const auto &apoly_in : plg->polygons)
-    { // each triangle/polygon
+    for (const auto& apoly_in : plg->polygons) { // each triangle/polygon
       SinglePoly apoly;
-      apoly.nvertices_ = apoly_in.vertices.size ();
-      apoly.vertices_ = new float[3*apoly_in.vertices.size ()];
-      apoly.colors_ = new float[4*apoly_in.vertices.size ()]; 
-
-      for(size_t j=0; j< apoly_in.vertices.size (); j++)
-      { // each point
-	uint32_t pt = apoly_in.vertices[j];
-	tmp = newcloud.points[pt].getVector4fMap ();
-	// x,y,z
-	apoly.vertices_[3*j + 0] = tmp (0);
-	apoly.vertices_[3*j + 1] = tmp (1);
-	apoly.vertices_[3*j + 2] = tmp (2);
-	// r,g,b: input is ints 0->255, opengl wants floats 0->1
-	apoly.colors_[4*j + 0] = newcloud.points[pt].r/255.0f; // Red
-	apoly.colors_[4*j + 1] = newcloud.points[pt].g/255.0f; // Green
-	apoly.colors_[4*j + 2] = newcloud.points[pt].b/255.0f; // Blue
-	apoly.colors_[4*j + 3] = 1.0f; // transparency? unnecessary?
+      apoly.nvertices_ = apoly_in.vertices.size();
+      apoly.vertices_ = new float[3 * apoly_in.vertices.size()];
+      apoly.colors_ = new float[4 * apoly_in.vertices.size()];
+
+      for (size_t j = 0; j < apoly_in.vertices.size(); j++) { // each point
+        uint32_t pt = apoly_in.vertices[j];
+        tmp = newcloud.points[pt].getVector4fMap();
+        // x,y,z
+        apoly.vertices_[3 * j + 0] = tmp(0);
+        apoly.vertices_[3 * j + 1] = tmp(1);
+        apoly.vertices_[3 * j + 2] = tmp(2);
+        // r,g,b: input is ints 0->255, opengl wants floats 0->1
+        apoly.colors_[4 * j + 0] = newcloud.points[pt].r / 255.0f; // Red
+        apoly.colors_[4 * j + 1] = newcloud.points[pt].g / 255.0f; // Green
+        apoly.colors_[4 * j + 2] = newcloud.points[pt].b / 255.0f; // Blue
+        apoly.colors_[4 * j + 3] = 1.0f; // transparency? unnecessary?
       }
-      polygons.push_back (apoly);
+      polygons.push_back(apoly);
     }
   }
-  else
-  {
-    pcl::PointCloud<pcl::PointXYZ> newcloud;  
-    pcl::fromPCLPointCloud2 (plg->cloud, newcloud);
+  else {
+    pcl::PointCloud<pcl::PointXYZ> newcloud;
+    pcl::fromPCLPointCloud2(plg->cloud, newcloud);
     Eigen::Vector4f tmp;
-    for(const auto &apoly_in : plg->polygons)
-    { // each triangle/polygon
+    for (const auto& apoly_in : plg->polygons) { // each triangle/polygon
       SinglePoly apoly;
-      apoly.nvertices_ = apoly_in.vertices.size ();
-      apoly.vertices_ = new float [3*apoly_in.vertices.size ()];
-      apoly.colors_ = new float [4*apoly_in.vertices.size ()];
-
-      for(size_t j=0; j< apoly_in.vertices.size (); j++)
-      { // each point
-	uint32_t pt = apoly_in.vertices[j];
-	tmp = newcloud.points[pt].getVector4fMap ();
-	// x,y,z
-	apoly.vertices_[3*j + 0] = tmp (0);
-	apoly.vertices_[3*j + 1] = tmp (1);
-	apoly.vertices_[3*j + 2] = tmp (2);
-	// r,g,b: input is ints 0->255, opengl wants floats 0->1
-	apoly.colors_[4*j + 0] = 1.0f; // Red
-	apoly.colors_[4*j + 1] = 0.0f; // Green
-	apoly.colors_[4*j + 2] = 0.0f; // Blue
-	apoly.colors_[4*j + 3] = 1.0;
+      apoly.nvertices_ = apoly_in.vertices.size();
+      apoly.vertices_ = new float[3 * apoly_in.vertices.size()];
+      apoly.colors_ = new float[4 * apoly_in.vertices.size()];
+
+      for (size_t j = 0; j < apoly_in.vertices.size(); j++) { // each point
+        uint32_t pt = apoly_in.vertices[j];
+        tmp = newcloud.points[pt].getVector4fMap();
+        // x,y,z
+        apoly.vertices_[3 * j + 0] = tmp(0);
+        apoly.vertices_[3 * j + 1] = tmp(1);
+        apoly.vertices_[3 * j + 2] = tmp(2);
+        // r,g,b: input is ints 0->255, opengl wants floats 0->1
+        apoly.colors_[4 * j + 0] = 1.0f; // Red
+        apoly.colors_[4 * j + 1] = 0.0f; // Green
+        apoly.colors_[4 * j + 2] = 0.0f; // Blue
+        apoly.colors_[4 * j + 3] = 1.0;
       }
-      polygons.push_back (apoly);
+      polygons.push_back(apoly);
     }
   }
 }
 
-pcl::simulation::PolygonMeshModel::~PolygonMeshModel ()
+pcl::simulation::PolygonMeshModel::~PolygonMeshModel()
 {
   // TODO: memory management!
-  for (auto &polygon : polygons)
-  {
-    delete [] polygon.vertices_;
-    delete [] polygon.colors_;
+  for (auto& polygon : polygons) {
+    delete[] polygon.vertices_;
+    delete[] polygon.colors_;
   }
 }
 
 void
-pcl::simulation::PolygonMeshModel::draw ()
+pcl::simulation::PolygonMeshModel::draw()
 {
   // This might be a little quicker than drawing using individual polygons
   // TODO: test by how much
-  glEnable (GL_DEPTH_TEST);
-  glEnableClientState (GL_VERTEX_ARRAY);
-  glEnableClientState (GL_COLOR_ARRAY);
-
-  for (const auto &polygon : polygons)
-  {
-    glVertexPointer (3, GL_FLOAT, 0, polygon.vertices_);
-    glColorPointer (4, GL_FLOAT, 0, polygon.colors_);
-    glDrawArrays (mode_, 0, polygon.nvertices_);
+  glEnable(GL_DEPTH_TEST);
+  glEnableClientState(GL_VERTEX_ARRAY);
+  glEnableClientState(GL_COLOR_ARRAY);
+
+  for (const auto& polygon : polygons) {
+    glVertexPointer(3, GL_FLOAT, 0, polygon.vertices_);
+    glColorPointer(4, GL_FLOAT, 0, polygon.colors_);
+    glDrawArrays(mode_, 0, polygon.nvertices_);
   }
-  glDisableClientState (GL_COLOR_ARRAY);
-  glDisableClientState (GL_VERTEX_ARRAY);
+  glDisableClientState(GL_COLOR_ARRAY);
+  glDisableClientState(GL_VERTEX_ARRAY);
 }
 
-pcl::simulation::PointCloudModel::PointCloudModel (GLenum mode, pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc) : mode_ (mode)
+pcl::simulation::PointCloudModel::PointCloudModel(
+    GLenum mode, pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc)
+: mode_(mode)
 {
-  nvertices_ = pc->points.size ();
-  vertices_ = new float[3*nvertices_];
-  colors_ = new float[4*nvertices_];
-
-  for (size_t i = 0; i < pc->points.size (); ++i)
-  {
-    vertices_[3*i + 0] = pc->points[i].x;
-    vertices_[3*i + 1] = pc->points[i].y;
-    vertices_[3*i + 2] = pc->points[i].z;
-
-    colors_[4*i + 0] = pc->points[i].r / 255.0f;
-    colors_[4*i + 1] = pc->points[i].g / 255.0f;
-    colors_[4*i + 2] = pc->points[i].b / 255.0f;
-    colors_[4*i + 3] = 1.0;
+  nvertices_ = pc->points.size();
+  vertices_ = new float[3 * nvertices_];
+  colors_ = new float[4 * nvertices_];
+
+  for (size_t i = 0; i < pc->points.size(); ++i) {
+    vertices_[3 * i + 0] = pc->points[i].x;
+    vertices_[3 * i + 1] = pc->points[i].y;
+    vertices_[3 * i + 2] = pc->points[i].z;
+
+    colors_[4 * i + 0] = pc->points[i].r / 255.0f;
+    colors_[4 * i + 1] = pc->points[i].g / 255.0f;
+    colors_[4 * i + 2] = pc->points[i].b / 255.0f;
+    colors_[4 * i + 3] = 1.0;
   }
 }
 
-pcl::simulation::PointCloudModel::~PointCloudModel ()
+pcl::simulation::PointCloudModel::~PointCloudModel()
 {
   delete vertices_;
   delete colors_;
 }
 
 void
-pcl::simulation::PointCloudModel::draw ()
+pcl::simulation::PointCloudModel::draw()
 {
-  glEnable (GL_DEPTH_TEST);
+  glEnable(GL_DEPTH_TEST);
 
-  glEnableClientState (GL_VERTEX_ARRAY);
-  glEnableClientState (GL_COLOR_ARRAY);
+  glEnableClientState(GL_VERTEX_ARRAY);
+  glEnableClientState(GL_COLOR_ARRAY);
 
   float att[3] = {0.0f, 0.25f, 0.0f};
   glPointParameterf(GL_POINT_SIZE_MIN, 1.0f);
-  glPointParameterf(GL_POINT_SIZE_MAX, 500.0f); 
+  glPointParameterf(GL_POINT_SIZE_MAX, 500.0f);
   glPointParameterfv(GL_POINT_DISTANCE_ATTENUATION, att);
   glEnable(GL_POINT_SPRITE);
 
-  glVertexPointer (3, GL_FLOAT, 0, vertices_);
-  glColorPointer (4, GL_FLOAT, 0, colors_);
+  glVertexPointer(3, GL_FLOAT, 0, vertices_);
+  glColorPointer(4, GL_FLOAT, 0, colors_);
 
-  glDrawArrays (mode_, 0, nvertices_);
+  glDrawArrays(mode_, 0, nvertices_);
 
-  glDisableClientState (GL_COLOR_ARRAY);
-  glDisableClientState (GL_VERTEX_ARRAY);
+  glDisableClientState(GL_COLOR_ARRAY);
+  glDisableClientState(GL_VERTEX_ARRAY);
 
   glDisable(GL_POINT_SPRITE);
 }
 
-pcl::simulation::Quad::Quad ()
+pcl::simulation::Quad::Quad()
 {
   // vertex pos: xyz , texture coord: uv
+  // clang-format off
   const static float vertices[20] = {-1.0, -1.0, 0.0, 0.0, 0.0,
                                      -1.0,  1.0, 0.0, 0.0, 1.0,
                                       1.0,  1.0, 0.0, 1.0, 1.0,
-                                      1.0, -1.0, 0.0, 1.0, 0.0 };
+                                      1.0, -1.0, 0.0, 1.0, 0.0};
+  // clang-format on
 
-  glGenBuffers (1, &quad_vbo_);
-  glBindBuffer (GL_ARRAY_BUFFER, quad_vbo_);
-  glBufferData (GL_ARRAY_BUFFER, sizeof (vertices), vertices, GL_STATIC_DRAW);
-  glBindBuffer (GL_ARRAY_BUFFER, 0);
+  glGenBuffers(1, &quad_vbo_);
+  glBindBuffer(GL_ARRAY_BUFFER, quad_vbo_);
+  glBufferData(GL_ARRAY_BUFFER, sizeof(vertices), vertices, GL_STATIC_DRAW);
+  glBindBuffer(GL_ARRAY_BUFFER, 0);
 }
 
-pcl::simulation::Quad::~Quad ()
-{
-  glDeleteBuffers (1, &quad_vbo_);
-}
+pcl::simulation::Quad::~Quad() { glDeleteBuffers(1, &quad_vbo_); }
 
 void
-pcl::simulation::Quad::render ()
+pcl::simulation::Quad::render()
 {
-  glBindBuffer (GL_ARRAY_BUFFER, quad_vbo_);
-  glEnableVertexAttribArray (0);
-  glEnableVertexAttribArray (1);
-  glVertexAttribPointer (0, 3, GL_FLOAT, GL_FALSE, sizeof (float)*5, nullptr);
-  glVertexAttribPointer (1, 2, GL_FLOAT, GL_FALSE, sizeof (float)*5, (const GLvoid*)12);
+  glBindBuffer(GL_ARRAY_BUFFER, quad_vbo_);
+  glEnableVertexAttribArray(0);
+  glEnableVertexAttribArray(1);
+  glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, sizeof(float) * 5, nullptr);
+  glVertexAttribPointer(1, 2, GL_FLOAT, GL_FALSE, sizeof(float) * 5, (const GLvoid*)12);
 
-  glDrawArrays (GL_QUADS, 0, 4);
+  glDrawArrays(GL_QUADS, 0, 4);
 
-  glDisableVertexAttribArray (1);
-  glDisableVertexAttribArray (0);
-  glBindBuffer (GL_ARRAY_BUFFER, 0);
+  glDisableVertexAttribArray(1);
+  glDisableVertexAttribArray(0);
+  glBindBuffer(GL_ARRAY_BUFFER, 0);
 }
 
-pcl::simulation::TexturedQuad::TexturedQuad (int width, int height) : width_ (width), height_ (height)
+pcl::simulation::TexturedQuad::TexturedQuad(int width, int height)
+: width_(width), height_(height)
 {
-  program_ = gllib::Program::loadProgramFromFile ("single_texture.vert", "single_texture.frag");
-  program_->use ();
+  program_ =
+      gllib::Program::loadProgramFromFile("single_texture.vert", "single_texture.frag");
+  program_->use();
   Eigen::Matrix<float, 4, 4> MVP;
   MVP.setIdentity();
-  program_->setUniform ("MVP", MVP);
-  glUseProgram (0);
-
-  glGenTextures (1, &texture_);
-  glBindTexture (GL_TEXTURE_2D, texture_);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
-  glTexImage2D (GL_TEXTURE_2D, 0, GL_RGB, width_, height_, 0, GL_RGB, GL_UNSIGNED_BYTE, nullptr);
-  glBindTexture (GL_TEXTURE_2D, 0);
+  program_->setUniform("MVP", MVP);
+  glUseProgram(0);
+
+  glGenTextures(1, &texture_);
+  glBindTexture(GL_TEXTURE_2D, texture_);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
+  glTexImage2D(
+      GL_TEXTURE_2D, 0, GL_RGB, width_, height_, 0, GL_RGB, GL_UNSIGNED_BYTE, nullptr);
+  glBindTexture(GL_TEXTURE_2D, 0);
 }
 
-pcl::simulation::TexturedQuad::~TexturedQuad ()
-{
-  glDeleteTextures (1, &texture_);
-}
+pcl::simulation::TexturedQuad::~TexturedQuad() { glDeleteTextures(1, &texture_); }
 
 void
-pcl::simulation::TexturedQuad::setTexture (const uint8_t* data)
+pcl::simulation::TexturedQuad::setTexture(const uint8_t* data)
 {
-  glBindTexture (GL_TEXTURE_2D, texture_);
-  glTexImage2D (GL_TEXTURE_2D, 0, GL_RGB, width_, height_, 0, GL_RGB, GL_UNSIGNED_BYTE, data);
-  glBindTexture (GL_TEXTURE_2D, 0);
+  glBindTexture(GL_TEXTURE_2D, texture_);
+  glTexImage2D(
+      GL_TEXTURE_2D, 0, GL_RGB, width_, height_, 0, GL_RGB, GL_UNSIGNED_BYTE, data);
+  glBindTexture(GL_TEXTURE_2D, 0);
 }
 
 void
-pcl::simulation::TexturedQuad::render ()
+pcl::simulation::TexturedQuad::render()
 {
-  program_->use ();
-  glActiveTexture (GL_TEXTURE0);
-  glBindTexture (GL_TEXTURE_2D, texture_);
-  quad_.render ();
-  glBindTexture (GL_TEXTURE_2D, 0);
-  glUseProgram (0);
+  program_->use();
+  glActiveTexture(GL_TEXTURE0);
+  glBindTexture(GL_TEXTURE_2D, texture_);
+  quad_.render();
+  glBindTexture(GL_TEXTURE_2D, 0);
+  glUseProgram(0);
 }
diff --git a/simulation/src/range_likelihood.cpp b/simulation/src/range_likelihood.cpp
index c7c3a618674..9ef61f144a2 100644
--- a/simulation/src/range_likelihood.cpp
+++ b/simulation/src/range_likelihood.cpp
@@ -7,18 +7,18 @@
 
 #include <pcl/pcl_config.h>
 #ifdef OPENGL_IS_A_FRAMEWORK
-# include <OpenGL/gl.h>
-# include <OpenGL/glu.h>
+#include <OpenGL/gl.h>
+#include <OpenGL/glu.h>
 #else
-# include <GL/gl.h>
-# include <GL/glu.h>
+#include <GL/gl.h>
+#include <GL/glu.h>
 #endif
 
 #include <pcl/common/time.h>
 #include <pcl/simulation/range_likelihood.h>
 
 // For adding noise:
-static std::minstd_rand rng ([] {std::random_device rd; return rd(); } ()); // seed
+static std::minstd_rand rng(std::random_device{}());
 
 //#define SIMULATION_DEBUG 1
 #define DO_TIMING_PROFILE 0
@@ -26,89 +26,221 @@ static std::minstd_rand rng ([] {std::random_device rd; return rd(); } ()); // s
 using namespace std;
 
 // 301 values, 0.0 uniform  1.0 normal. properly truncated/normalized
-float normal_sigma0x5_normal1x0_range0to3_step0x01[] = {1.59576912f, 1.59545000f, 1.59449302f, 1.59289932f, 1.59067083f,
-                                                        1.58781019f, 1.58432085f, 1.58020697f, 1.57547345f, 1.57012594f,
-                                                        1.56417078f, 1.55761504f, 1.55046646f, 1.54273348f, 1.53442517f,
-                                                        1.52555126f, 1.51612211f, 1.50614865f, 1.49564242f, 1.48461552f,
-                                                        1.47308056f, 1.46105069f, 1.44853953f, 1.43556117f, 1.42213012f,
-                                                        1.40826131f, 1.39397005f, 1.37927201f, 1.36418316f, 1.34871978f,
-                                                        1.33289841f, 1.31673585f, 1.30024906f, 1.28345522f, 1.26637163f,
-                                                        1.24901574f, 1.23140504f, 1.21355714f, 1.19548963f, 1.17722012f,
-                                                        1.15876621f, 1.14014544f, 1.12137525f, 1.10247299f, 1.08345589f,
-                                                        1.06434100f, 1.04514521f, 1.02588518f, 1.00657737f, 0.98723796f,
-                                                        0.96788290f, 0.94852781f, 0.92918802f, 0.90987853f, 0.89061400f,
-                                                        0.87140871f, 0.85227659f, 0.83323116f, 0.81428555f, 0.79545248f,
-                                                        0.77674422f, 0.75817263f, 0.73974913f, 0.72148465f, 0.70338972f,
-                                                        0.68547437f, 0.66774817f, 0.65022022f, 0.63289916f, 0.61579315f,
-                                                        0.59890986f, 0.58225652f, 0.56583986f, 0.54966616f, 0.53374122f,
-                                                        0.51807038f, 0.50265855f, 0.48751015f, 0.47262918f, 0.45801920f,
-                                                        0.44368334f, 0.42962430f, 0.41584438f, 0.40234547f, 0.38912908f,
-                                                        0.37619631f, 0.36354792f, 0.35118428f, 0.33910545f, 0.32731110f,
-                                                        0.31580063f, 0.30457310f, 0.29362725f, 0.28296157f, 0.27257426f,
-                                                        0.26246326f, 0.25262625f, 0.24306068f, 0.23376378f, 0.22473257f,
-                                                        0.21596387f, 0.20745431f, 0.19920035f, 0.19119830f, 0.18344431f,
-                                                        0.17593438f, 0.16866443f, 0.16163022f, 0.15482742f, 0.14825164f,
-                                                        0.14189837f, 0.13576305f, 0.12984106f, 0.12412773f, 0.11861834f,
-                                                        0.11330815f, 0.10819240f, 0.10326630f, 0.09852508f, 0.09396394f,
-                                                        0.08957812f, 0.08536286f, 0.08131342f, 0.07742511f, 0.07369324f,
-                                                        0.07011320f, 0.06668040f, 0.06339032f, 0.06023847f, 0.05722044f,
-                                                        0.05433188f, 0.05156850f, 0.04892611f, 0.04640054f, 0.04398775f,
-                                                        0.04168374f, 0.03948462f, 0.03738655f, 0.03538582f, 0.03347876f,
-                                                        0.03166181f, 0.02993149f, 0.02828442f, 0.02671730f, 0.02522691f,
-                                                        0.02381013f, 0.02246393f, 0.02118538f, 0.01997160f, 0.01881983f,
-                                                        0.01772739f, 0.01669169f, 0.01571021f, 0.01478053f, 0.01390031f,
-                                                        0.01306728f, 0.01227925f, 0.01153414f, 0.01082990f, 0.01016460f,
-                                                        0.00953635f, 0.00894336f, 0.00838388f, 0.00785626f, 0.00735890f,
-                                                        0.00689028f, 0.00644891f, 0.00603340f, 0.00564241f, 0.00527464f,
-                                                        0.00492888f, 0.00460393f, 0.00429869f, 0.00401209f, 0.00374309f,
-                                                        0.00349073f, 0.00325408f, 0.00303227f, 0.00282444f, 0.00262981f,
-                                                        0.00244761f, 0.00227712f, 0.00211766f, 0.00196858f, 0.00182926f,
-                                                        0.00169912f, 0.00157761f, 0.00146420f, 0.00135840f, 0.00125975f,
-                                                        0.00116779f, 0.00108211f, 0.00100231f, 0.00092803f, 0.00085891f,
-                                                        0.00079462f, 0.00073485f, 0.00067930f, 0.00062770f, 0.00057979f,
-                                                        0.00053532f, 0.00049406f, 0.00045581f, 0.00042034f, 0.00038748f,
-                                                        0.00035705f, 0.00032887f, 0.00030280f, 0.00027868f, 0.00025638f,
-                                                        0.00023577f, 0.00021673f, 0.00019915f, 0.00018292f, 0.00016795f,
-                                                        0.00015414f, 0.00014141f, 0.00012968f, 0.00011887f, 0.00010893f,
-                                                        0.00009977f, 0.00009135f, 0.00008360f, 0.00007648f, 0.00006994f,
-                                                        0.00006393f, 0.00005842f, 0.00005336f, 0.00004872f, 0.00004446f,
-                                                        0.00004056f, 0.00003699f, 0.00003372f, 0.00003072f, 0.00002798f,
-                                                        0.00002548f, 0.00002319f, 0.00002110f, 0.00001918f, 0.00001744f,
-                                                        0.00001585f, 0.00001439f, 0.00001307f, 0.00001186f, 0.00001076f,
-                                                        0.00000976f, 0.00000884f, 0.00000801f, 0.00000726f, 0.00000657f,
-                                                        0.00000595f, 0.00000538f, 0.00000486f, 0.00000440f, 0.00000397f,
-                                                        0.00000359f, 0.00000324f, 0.00000292f, 0.00000264f, 0.00000238f,
-                                                        0.00000214f, 0.00000193f, 0.00000174f, 0.00000157f, 0.00000141f,
-                                                        0.00000127f, 0.00000114f, 0.00000103f, 0.00000092f, 0.00000083f,
-                                                        0.00000074f, 0.00000067f, 0.00000060f, 0.00000054f, 0.00000048f,
-                                                        0.00000043f, 0.00000039f, 0.00000035f, 0.00000031f, 0.00000028f,
-                                                        0.00000025f, 0.00000022f, 0.00000020f, 0.00000018f, 0.00000016f,
-                                                        0.00000014f, 0.00000013f, 0.00000011f, 0.00000010f, 0.00000009f,
-                                                        0.00000008f, 0.00000007f, 0.00000006f, 0.00000006f, 0.00000005f,
-                                                        0.00000004f, 0.00000004f, 0.00000003f, 0.00000003f, 0.00000003f,
-                                                        0.00000002f};
+float normal_sigma0x5_normal1x0_range0to3_step0x01[] = {
+    1.59576912f, 1.59545000f, 1.59449302f, 1.59289932f, 1.59067083f, 1.58781019f,
+    1.58432085f, 1.58020697f, 1.57547345f, 1.57012594f, 1.56417078f, 1.55761504f,
+    1.55046646f, 1.54273348f, 1.53442517f, 1.52555126f, 1.51612211f, 1.50614865f,
+    1.49564242f, 1.48461552f, 1.47308056f, 1.46105069f, 1.44853953f, 1.43556117f,
+    1.42213012f, 1.40826131f, 1.39397005f, 1.37927201f, 1.36418316f, 1.34871978f,
+    1.33289841f, 1.31673585f, 1.30024906f, 1.28345522f, 1.26637163f, 1.24901574f,
+    1.23140504f, 1.21355714f, 1.19548963f, 1.17722012f, 1.15876621f, 1.14014544f,
+    1.12137525f, 1.10247299f, 1.08345589f, 1.06434100f, 1.04514521f, 1.02588518f,
+    1.00657737f, 0.98723796f, 0.96788290f, 0.94852781f, 0.92918802f, 0.90987853f,
+    0.89061400f, 0.87140871f, 0.85227659f, 0.83323116f, 0.81428555f, 0.79545248f,
+    0.77674422f, 0.75817263f, 0.73974913f, 0.72148465f, 0.70338972f, 0.68547437f,
+    0.66774817f, 0.65022022f, 0.63289916f, 0.61579315f, 0.59890986f, 0.58225652f,
+    0.56583986f, 0.54966616f, 0.53374122f, 0.51807038f, 0.50265855f, 0.48751015f,
+    0.47262918f, 0.45801920f, 0.44368334f, 0.42962430f, 0.41584438f, 0.40234547f,
+    0.38912908f, 0.37619631f, 0.36354792f, 0.35118428f, 0.33910545f, 0.32731110f,
+    0.31580063f, 0.30457310f, 0.29362725f, 0.28296157f, 0.27257426f, 0.26246326f,
+    0.25262625f, 0.24306068f, 0.23376378f, 0.22473257f, 0.21596387f, 0.20745431f,
+    0.19920035f, 0.19119830f, 0.18344431f, 0.17593438f, 0.16866443f, 0.16163022f,
+    0.15482742f, 0.14825164f, 0.14189837f, 0.13576305f, 0.12984106f, 0.12412773f,
+    0.11861834f, 0.11330815f, 0.10819240f, 0.10326630f, 0.09852508f, 0.09396394f,
+    0.08957812f, 0.08536286f, 0.08131342f, 0.07742511f, 0.07369324f, 0.07011320f,
+    0.06668040f, 0.06339032f, 0.06023847f, 0.05722044f, 0.05433188f, 0.05156850f,
+    0.04892611f, 0.04640054f, 0.04398775f, 0.04168374f, 0.03948462f, 0.03738655f,
+    0.03538582f, 0.03347876f, 0.03166181f, 0.02993149f, 0.02828442f, 0.02671730f,
+    0.02522691f, 0.02381013f, 0.02246393f, 0.02118538f, 0.01997160f, 0.01881983f,
+    0.01772739f, 0.01669169f, 0.01571021f, 0.01478053f, 0.01390031f, 0.01306728f,
+    0.01227925f, 0.01153414f, 0.01082990f, 0.01016460f, 0.00953635f, 0.00894336f,
+    0.00838388f, 0.00785626f, 0.00735890f, 0.00689028f, 0.00644891f, 0.00603340f,
+    0.00564241f, 0.00527464f, 0.00492888f, 0.00460393f, 0.00429869f, 0.00401209f,
+    0.00374309f, 0.00349073f, 0.00325408f, 0.00303227f, 0.00282444f, 0.00262981f,
+    0.00244761f, 0.00227712f, 0.00211766f, 0.00196858f, 0.00182926f, 0.00169912f,
+    0.00157761f, 0.00146420f, 0.00135840f, 0.00125975f, 0.00116779f, 0.00108211f,
+    0.00100231f, 0.00092803f, 0.00085891f, 0.00079462f, 0.00073485f, 0.00067930f,
+    0.00062770f, 0.00057979f, 0.00053532f, 0.00049406f, 0.00045581f, 0.00042034f,
+    0.00038748f, 0.00035705f, 0.00032887f, 0.00030280f, 0.00027868f, 0.00025638f,
+    0.00023577f, 0.00021673f, 0.00019915f, 0.00018292f, 0.00016795f, 0.00015414f,
+    0.00014141f, 0.00012968f, 0.00011887f, 0.00010893f, 0.00009977f, 0.00009135f,
+    0.00008360f, 0.00007648f, 0.00006994f, 0.00006393f, 0.00005842f, 0.00005336f,
+    0.00004872f, 0.00004446f, 0.00004056f, 0.00003699f, 0.00003372f, 0.00003072f,
+    0.00002798f, 0.00002548f, 0.00002319f, 0.00002110f, 0.00001918f, 0.00001744f,
+    0.00001585f, 0.00001439f, 0.00001307f, 0.00001186f, 0.00001076f, 0.00000976f,
+    0.00000884f, 0.00000801f, 0.00000726f, 0.00000657f, 0.00000595f, 0.00000538f,
+    0.00000486f, 0.00000440f, 0.00000397f, 0.00000359f, 0.00000324f, 0.00000292f,
+    0.00000264f, 0.00000238f, 0.00000214f, 0.00000193f, 0.00000174f, 0.00000157f,
+    0.00000141f, 0.00000127f, 0.00000114f, 0.00000103f, 0.00000092f, 0.00000083f,
+    0.00000074f, 0.00000067f, 0.00000060f, 0.00000054f, 0.00000048f, 0.00000043f,
+    0.00000039f, 0.00000035f, 0.00000031f, 0.00000028f, 0.00000025f, 0.00000022f,
+    0.00000020f, 0.00000018f, 0.00000016f, 0.00000014f, 0.00000013f, 0.00000011f,
+    0.00000010f, 0.00000009f, 0.00000008f, 0.00000007f, 0.00000006f, 0.00000006f,
+    0.00000005f, 0.00000004f, 0.00000004f, 0.00000003f, 0.00000003f, 0.00000003f,
+    0.00000002f};
 
 // Where the above if lhoodf, this a hard coded/optimized version:
-//ratio = 0.99; r_min =0;  	r_max = 3;
-//lhood = ratio/(r_max -r_min)  + (1-ratio)*lhood ; hard_coded_log_lhood=std::log(lhood)
-float hard_coded_log_lhood[] = {-1.0614388f, -1.0614480f, -1.0614757f, -1.0615217f, -1.0615862f, -1.0616689f, -1.0617698f, -1.0618887f, -1.0620256f, -1.0621803f, -1.0623526f, -1.0625423f, -1.0627491f, -1.0629730f, -1.0632135f, -1.0634705f, -1.0637437f, -1.0640327f, -1.0643372f, -1.0646569f, -1.0649914f, -1.0653405f, -1.0657036f, -1.0660804f, -1.0664705f, -1.0668735f, -1.0672889f, -1.0677164f, -1.0681554f, -1.0686054f, -1.0690662f, -1.0695370f, -1.0700176f, -1.0705073f, -1.0710057f, -1.0715124f, -1.0720267f, -1.0725482f, -1.0730764f, -1.0736108f, -1.0741509f, -1.0746962f, -1.0752462f, -1.0758003f, -1.0763581f, -1.0769191f, -1.0774827f, -1.0780486f, -1.0786162f, -1.0791851f, -1.0797547f, -1.0803247f, -1.0808945f, -1.0814638f, -1.0820321f, -1.0825989f, -1.0831639f, -1.0837267f, -1.0842868f, -1.0848439f, -1.0853977f, -1.0859476f, -1.0864935f, -1.0870350f, -1.0875718f, -1.0881035f, -1.0886298f, -1.0891506f, -1.0896655f, -1.0901742f, -1.0906766f, -1.0911723f, -1.0916613f, -1.0921433f, -1.0926181f, -1.0930855f, -1.0935454f, -1.0939976f, -1.0944421f, -1.0948787f, -1.0953073f, -1.0957277f, -1.0961400f, -1.0965441f, -1.0969398f, -1.0973272f, -1.0977063f, -1.0980769f, -1.0984391f, -1.0987930f, -1.0991384f, -1.0994755f, -1.0998042f, -1.1001246f, -1.1004367f, -1.1007407f, -1.1010364f, -1.1013241f, -1.1016038f, -1.1018756f, -1.1021396f, -1.1023958f, -1.1026444f, -1.1028855f, -1.1031191f, -1.1033454f, -1.1035646f, -1.1037767f, -1.1039819f, -1.1041802f, -1.1043719f, -1.1045570f, -1.1047358f, -1.1049082f, -1.1050746f, -1.1052349f, -1.1053894f, -1.1055382f, -1.1056815f, -1.1058193f, -1.1059518f, -1.1060792f, -1.1062016f, -1.1063192f, -1.1064320f, -1.1065402f, -1.1066440f, -1.1067435f, -1.1068389f, -1.1069302f, -1.1070176f, -1.1071012f, -1.1071811f, -1.1072575f, -1.1073306f, -1.1074003f, -1.1074668f, -1.1075303f, -1.1075909f, -1.1076486f, -1.1077036f, -1.1077560f, -1.1078059f, -1.1078533f, -1.1078985f, -1.1079414f, -1.1079821f, -1.1080208f, -1.1080576f, -1.1080925f, -1.1081256f, -1.1081569f, -1.1081867f, -1.1082148f, -1.1082415f, -1.1082667f, -1.1082906f, -1.1083132f, -1.1083345f, -1.1083547f, -1.1083737f, -1.1083917f, -1.1084086f, -1.1084246f, -1.1084397f, -1.1084538f, -1.1084672f, -1.1084798f, -1.1084917f, -1.1085028f, -1.1085133f, -1.1085231f, -1.1085324f, -1.1085411f, -1.1085492f, -1.1085569f, -1.1085640f, -1.1085707f, -1.1085770f, -1.1085829f, -1.1085885f, -1.1085936f, -1.1085985f, -1.1086030f, -1.1086072f, -1.1086111f, -1.1086148f, -1.1086183f, -1.1086215f, -1.1086245f, -1.1086272f, -1.1086298f, -1.1086323f, -1.1086345f, -1.1086366f, -1.1086385f, -1.1086404f, -1.1086420f, -1.1086436f, -1.1086451f, -1.1086464f, -1.1086477f, -1.1086488f, -1.1086499f, -1.1086509f, -1.1086518f, -1.1086527f, -1.1086534f, -1.1086542f, -1.1086549f, -1.1086555f, -1.1086561f, -1.1086566f, -1.1086571f, -1.1086575f, -1.1086580f, -1.1086583f, -1.1086587f, -1.1086590f, -1.1086593f, -1.1086596f, -1.1086599f, -1.1086601f, -1.1086603f, -1.1086605f, -1.1086607f, -1.1086609f, -1.1086610f, -1.1086611f, -1.1086613f, -1.1086614f, -1.1086615f, -1.1086616f, -1.1086617f, -1.1086618f, -1.1086619f, -1.1086619f, -1.1086620f, -1.1086620f, -1.1086621f, -1.1086621f, -1.1086622f, -1.1086622f, -1.1086623f, -1.1086623f, -1.1086623f, -1.1086624f, -1.1086624f, -1.1086624f, -1.1086624f, -1.1086624f, -1.1086625f, -1.1086625f, -1.1086625f, -1.1086625f, -1.1086625f, -1.1086625f, -1.1086625f, -1.1086625f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f};
+// ratio = 0.99; r_min =0;  	r_max = 3;
+// lhood = ratio/(r_max -r_min)  + (1-ratio)*lhood ;
+// hard_coded_log_lhood=std::log(lhood)
+float hard_coded_log_lhood[] = {
+    -1.0614388f, -1.0614480f, -1.0614757f, -1.0615217f, -1.0615862f, -1.0616689f,
+    -1.0617698f, -1.0618887f, -1.0620256f, -1.0621803f, -1.0623526f, -1.0625423f,
+    -1.0627491f, -1.0629730f, -1.0632135f, -1.0634705f, -1.0637437f, -1.0640327f,
+    -1.0643372f, -1.0646569f, -1.0649914f, -1.0653405f, -1.0657036f, -1.0660804f,
+    -1.0664705f, -1.0668735f, -1.0672889f, -1.0677164f, -1.0681554f, -1.0686054f,
+    -1.0690662f, -1.0695370f, -1.0700176f, -1.0705073f, -1.0710057f, -1.0715124f,
+    -1.0720267f, -1.0725482f, -1.0730764f, -1.0736108f, -1.0741509f, -1.0746962f,
+    -1.0752462f, -1.0758003f, -1.0763581f, -1.0769191f, -1.0774827f, -1.0780486f,
+    -1.0786162f, -1.0791851f, -1.0797547f, -1.0803247f, -1.0808945f, -1.0814638f,
+    -1.0820321f, -1.0825989f, -1.0831639f, -1.0837267f, -1.0842868f, -1.0848439f,
+    -1.0853977f, -1.0859476f, -1.0864935f, -1.0870350f, -1.0875718f, -1.0881035f,
+    -1.0886298f, -1.0891506f, -1.0896655f, -1.0901742f, -1.0906766f, -1.0911723f,
+    -1.0916613f, -1.0921433f, -1.0926181f, -1.0930855f, -1.0935454f, -1.0939976f,
+    -1.0944421f, -1.0948787f, -1.0953073f, -1.0957277f, -1.0961400f, -1.0965441f,
+    -1.0969398f, -1.0973272f, -1.0977063f, -1.0980769f, -1.0984391f, -1.0987930f,
+    -1.0991384f, -1.0994755f, -1.0998042f, -1.1001246f, -1.1004367f, -1.1007407f,
+    -1.1010364f, -1.1013241f, -1.1016038f, -1.1018756f, -1.1021396f, -1.1023958f,
+    -1.1026444f, -1.1028855f, -1.1031191f, -1.1033454f, -1.1035646f, -1.1037767f,
+    -1.1039819f, -1.1041802f, -1.1043719f, -1.1045570f, -1.1047358f, -1.1049082f,
+    -1.1050746f, -1.1052349f, -1.1053894f, -1.1055382f, -1.1056815f, -1.1058193f,
+    -1.1059518f, -1.1060792f, -1.1062016f, -1.1063192f, -1.1064320f, -1.1065402f,
+    -1.1066440f, -1.1067435f, -1.1068389f, -1.1069302f, -1.1070176f, -1.1071012f,
+    -1.1071811f, -1.1072575f, -1.1073306f, -1.1074003f, -1.1074668f, -1.1075303f,
+    -1.1075909f, -1.1076486f, -1.1077036f, -1.1077560f, -1.1078059f, -1.1078533f,
+    -1.1078985f, -1.1079414f, -1.1079821f, -1.1080208f, -1.1080576f, -1.1080925f,
+    -1.1081256f, -1.1081569f, -1.1081867f, -1.1082148f, -1.1082415f, -1.1082667f,
+    -1.1082906f, -1.1083132f, -1.1083345f, -1.1083547f, -1.1083737f, -1.1083917f,
+    -1.1084086f, -1.1084246f, -1.1084397f, -1.1084538f, -1.1084672f, -1.1084798f,
+    -1.1084917f, -1.1085028f, -1.1085133f, -1.1085231f, -1.1085324f, -1.1085411f,
+    -1.1085492f, -1.1085569f, -1.1085640f, -1.1085707f, -1.1085770f, -1.1085829f,
+    -1.1085885f, -1.1085936f, -1.1085985f, -1.1086030f, -1.1086072f, -1.1086111f,
+    -1.1086148f, -1.1086183f, -1.1086215f, -1.1086245f, -1.1086272f, -1.1086298f,
+    -1.1086323f, -1.1086345f, -1.1086366f, -1.1086385f, -1.1086404f, -1.1086420f,
+    -1.1086436f, -1.1086451f, -1.1086464f, -1.1086477f, -1.1086488f, -1.1086499f,
+    -1.1086509f, -1.1086518f, -1.1086527f, -1.1086534f, -1.1086542f, -1.1086549f,
+    -1.1086555f, -1.1086561f, -1.1086566f, -1.1086571f, -1.1086575f, -1.1086580f,
+    -1.1086583f, -1.1086587f, -1.1086590f, -1.1086593f, -1.1086596f, -1.1086599f,
+    -1.1086601f, -1.1086603f, -1.1086605f, -1.1086607f, -1.1086609f, -1.1086610f,
+    -1.1086611f, -1.1086613f, -1.1086614f, -1.1086615f, -1.1086616f, -1.1086617f,
+    -1.1086618f, -1.1086619f, -1.1086619f, -1.1086620f, -1.1086620f, -1.1086621f,
+    -1.1086621f, -1.1086622f, -1.1086622f, -1.1086623f, -1.1086623f, -1.1086623f,
+    -1.1086624f, -1.1086624f, -1.1086624f, -1.1086624f, -1.1086624f, -1.1086625f,
+    -1.1086625f, -1.1086625f, -1.1086625f, -1.1086625f, -1.1086625f, -1.1086625f,
+    -1.1086625f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f,
+    -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f,
+    -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f,
+    -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f,
+    -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f,
+    -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f,
+    -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f, -1.1086626f,
+    -1.1086626f};
 
 // Disparity:
 // sigma 0.025
-double top_lookup[]={15.9577, 15.8165, 15.4003, 14.7308, 13.8422, 12.7779, 11.5877, 10.3231, 9.0345, 7.7674, 6.5604, 5.4433, 4.4368, 3.5527, 2.7947, 2.1596, 1.6395, 1.2227, 0.89578, 0.64471, 0.45584, 0.31662, 0.21604, 0.14482, 0.095364, 0.061691, 0.039205, 0.024476, 0.015011, 0.0090444, 0.0053532, 0.0031126, 0.001778, 0.0009977, 0.00054999, 0.00029784, 0.00015845, 8.2811e-05, 4.2517e-05, 2.1444e-05, 1.0625e-05, 5.1718e-06, 2.473e-06, 1.1617e-06, 5.361e-07, 2.4304e-07, 1.0824e-07, 4.7354e-08, 2.0353e-08, 8.5933e-09, 3.5644e-09, 1.4524e-09, 5.8138e-10, 2.2862e-10, 8.832e-11, 3.3518e-11, 1.2496e-11, 4.5766e-12, 1.6466e-12, 5.8201e-13, 2.0209e-13, 6.8935e-14, 2.31e-14, 7.6043e-15, 2.4592e-15, 7.8127e-16, 2.4383e-16, 7.4758e-17, 2.2517e-17, 6.6624e-18, 1.9366e-18, 5.5299e-19, 1.5512e-19, 4.2749e-20, 1.1573e-20, 3.0778e-21, 8.0413e-22, 2.0639e-22, 5.2038e-23, 1.289e-23, 3.1365e-24, 7.4975e-25, 1.7606e-25, 4.0617e-26, 9.2049e-27, 2.0493e-27, 4.4821e-28, 9.6302e-29, 2.0327e-29, 4.2148e-30, 8.5855e-31, 1.718e-31, 3.3774e-32, 6.5224e-33, 1.2374e-33, 2.3062e-34, 4.2225e-35, 7.5947e-36, 1.3419e-36, 2.3294e-37, 3.9721e-38, 6.6539e-39, 1.095e-39, 1.7703e-40, 2.8115e-41, 4.3864e-42, 6.7231e-43, 1.0123e-43, 1.4973e-44, 2.1758e-45, 3.1059e-46, 4.3555e-47, 6.0003e-48, 8.1205e-49, 1.0796e-49, 1.4101e-50, 1.8092e-51, 2.2804e-52, 2.8237e-53, 3.4349e-54, 4.1047e-55, 4.8186e-56, 5.5571e-57, 6.2958e-58, 7.007e-59, 7.6611e-60, 8.2287e-61, 8.6827e-62, 9.0002e-63, 9.165e-64, 9.1683e-65, 9.01e-66, 8.6984e-67, 8.2497e-68, 7.6862e-69, 7.035e-70, 6.3255e-71, 5.5874e-72, 4.8484e-73, 4.133e-74, 3.4611e-75, 2.8474e-76, 2.3012e-77, 1.827e-78, 1.425e-79, 1.0918e-80, 8.2183e-82, 6.077e-83, 4.4144e-84, 3.1502e-85, 2.2084e-86, 1.5209e-87, 1.029e-88, 6.8387e-90, 4.4651e-91, 2.864e-92, 1.8046e-93, 1.1171e-94, 6.793e-96, 4.058e-97, 2.3815e-98, 1.373e-99, 7.7759e-101, 4.3264e-102, 2.3647e-103, 1.2697e-104, 6.6975e-106, 3.4706e-107, 1.7667e-108, 8.8352e-110, 4.3405e-111, 2.0948e-112, 9.9319e-114, 4.6259e-115, 2.1166e-116, 9.514e-118, 4.2011e-119, 1.8224e-120, 7.7661e-122, 3.2512e-123, 1.3371e-124, 5.402e-126, 2.144e-127, 8.3597e-129, 3.202e-130, 1.2049e-131, 4.4538e-133, 1.6173e-134, 5.7697e-136, 2.022e-137, 6.9614e-139, 2.3544e-140, 7.8227e-142, 2.5533e-143, 8.1871e-145, 2.5789e-146, 7.9803e-148, 2.426e-149, 7.2448e-151, 2.1255e-152, 6.1257e-154, 1.7343e-155, 4.8239e-157, 1.3181e-158, 3.538e-160, 9.3294e-162, 2.4167e-163, 6.1502e-165, 1.5375e-166, 3.7761e-168, 9.1103e-170, 2.1593e-171, 5.0276e-173, 1.15e-174, 2.5841e-176, 5.7042e-178, 1.237e-179, 2.6352e-181, 5.5149e-183, 1.1338e-184, 2.29e-186, 4.5436e-188, 8.8561e-190, 1.6958e-191, 3.1899e-193, 5.8946e-195, 1.0701e-196, 1.9083e-198, 3.3433e-200, 5.7541e-202, 9.7287e-204, 1.6159e-205, 2.6366e-207, 4.2263e-209, 6.6552e-211, 1.0295e-212, 1.5645e-214, 2.3357e-216, 3.4256e-218, 4.9354e-220, 6.9855e-222, 9.7128e-224, 1.3267e-225, 1.7803e-227, 2.3468e-229, 3.039e-231, 3.8662e-233, 4.8318e-235, 5.9321e-237, 7.1548e-239, 8.4773e-241, 9.8673e-243, 1.1283e-244, 1.2674e-246, 1.3986e-248, 1.5162e-250, 1.6147e-252, 1.6893e-254, 1.7362e-256, 1.753e-258, 1.7388e-260, 1.6942e-262, 1.6218e-264, 1.525e-266, 1.4088e-268, 1.2785e-270, 1.1398e-272, 9.9826e-275, 8.5888e-277, 7.2594e-279, 6.0276e-281, 4.9167e-283, 3.9398e-285, 3.1014e-287, 2.3984e-289, 1.8221e-291, 1.3598e-293, 9.9699e-296, 7.1808e-298, 5.0808e-300, 3.5316e-302, 2.4115e-304, 1.6177e-306, 1.066e-308, 6.9011e-311, 4.3889e-313, 2.742e-315, 1.6829e-317, 1.0147e-319, 6.324e-322, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
-float bottom_lookup[]={0.5f, 0.55304f, 0.60514f, 0.65542f, 0.7031f, 0.74751f, 0.78814f, 0.82468f, 0.85694f, 0.88493f, 0.90879f, 0.92877f, 0.9452f, 0.95848f, 0.96903f, 0.97725f, 0.98355f, 0.98829f, 0.9918f, 0.99435f, 0.99617f, 0.99744f, 0.99832f, 0.99892f, 0.99931f, 0.99957f, 0.99974f, 0.99984f, 0.99991f, 0.99994f, 0.99997f, 0.99998f, 0.99999f, 0.99999f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.99999f, 0.99999f, 0.99998f, 0.99997f, 0.99994f, 0.99991f, 0.99984f, 0.99974f, 0.99957f, 0.99931f, 0.99892f, 0.99832f, 0.99744f, 0.99617f, 0.99435f, 0.9918f, 0.98829f, 0.98355f, 0.97725f, 0.96903f, 0.95848f, 0.9452f, 0.92877f, 0.90879f, 0.88493f, 0.85694f, 0.82468f, 0.78814f, 0.74751f, 0.7031f, 0.65542f, 0.60514f, 0.55304f, 0.5f};
+double top_lookup[] = {
+    15.9577,     15.8165,     15.4003,     14.7308,     13.8422,     12.7779,
+    11.5877,     10.3231,     9.0345,      7.7674,      6.5604,      5.4433,
+    4.4368,      3.5527,      2.7947,      2.1596,      1.6395,      1.2227,
+    0.89578,     0.64471,     0.45584,     0.31662,     0.21604,     0.14482,
+    0.095364,    0.061691,    0.039205,    0.024476,    0.015011,    0.0090444,
+    0.0053532,   0.0031126,   0.001778,    0.0009977,   0.00054999,  0.00029784,
+    0.00015845,  8.2811e-05,  4.2517e-05,  2.1444e-05,  1.0625e-05,  5.1718e-06,
+    2.473e-06,   1.1617e-06,  5.361e-07,   2.4304e-07,  1.0824e-07,  4.7354e-08,
+    2.0353e-08,  8.5933e-09,  3.5644e-09,  1.4524e-09,  5.8138e-10,  2.2862e-10,
+    8.832e-11,   3.3518e-11,  1.2496e-11,  4.5766e-12,  1.6466e-12,  5.8201e-13,
+    2.0209e-13,  6.8935e-14,  2.31e-14,    7.6043e-15,  2.4592e-15,  7.8127e-16,
+    2.4383e-16,  7.4758e-17,  2.2517e-17,  6.6624e-18,  1.9366e-18,  5.5299e-19,
+    1.5512e-19,  4.2749e-20,  1.1573e-20,  3.0778e-21,  8.0413e-22,  2.0639e-22,
+    5.2038e-23,  1.289e-23,   3.1365e-24,  7.4975e-25,  1.7606e-25,  4.0617e-26,
+    9.2049e-27,  2.0493e-27,  4.4821e-28,  9.6302e-29,  2.0327e-29,  4.2148e-30,
+    8.5855e-31,  1.718e-31,   3.3774e-32,  6.5224e-33,  1.2374e-33,  2.3062e-34,
+    4.2225e-35,  7.5947e-36,  1.3419e-36,  2.3294e-37,  3.9721e-38,  6.6539e-39,
+    1.095e-39,   1.7703e-40,  2.8115e-41,  4.3864e-42,  6.7231e-43,  1.0123e-43,
+    1.4973e-44,  2.1758e-45,  3.1059e-46,  4.3555e-47,  6.0003e-48,  8.1205e-49,
+    1.0796e-49,  1.4101e-50,  1.8092e-51,  2.2804e-52,  2.8237e-53,  3.4349e-54,
+    4.1047e-55,  4.8186e-56,  5.5571e-57,  6.2958e-58,  7.007e-59,   7.6611e-60,
+    8.2287e-61,  8.6827e-62,  9.0002e-63,  9.165e-64,   9.1683e-65,  9.01e-66,
+    8.6984e-67,  8.2497e-68,  7.6862e-69,  7.035e-70,   6.3255e-71,  5.5874e-72,
+    4.8484e-73,  4.133e-74,   3.4611e-75,  2.8474e-76,  2.3012e-77,  1.827e-78,
+    1.425e-79,   1.0918e-80,  8.2183e-82,  6.077e-83,   4.4144e-84,  3.1502e-85,
+    2.2084e-86,  1.5209e-87,  1.029e-88,   6.8387e-90,  4.4651e-91,  2.864e-92,
+    1.8046e-93,  1.1171e-94,  6.793e-96,   4.058e-97,   2.3815e-98,  1.373e-99,
+    7.7759e-101, 4.3264e-102, 2.3647e-103, 1.2697e-104, 6.6975e-106, 3.4706e-107,
+    1.7667e-108, 8.8352e-110, 4.3405e-111, 2.0948e-112, 9.9319e-114, 4.6259e-115,
+    2.1166e-116, 9.514e-118,  4.2011e-119, 1.8224e-120, 7.7661e-122, 3.2512e-123,
+    1.3371e-124, 5.402e-126,  2.144e-127,  8.3597e-129, 3.202e-130,  1.2049e-131,
+    4.4538e-133, 1.6173e-134, 5.7697e-136, 2.022e-137,  6.9614e-139, 2.3544e-140,
+    7.8227e-142, 2.5533e-143, 8.1871e-145, 2.5789e-146, 7.9803e-148, 2.426e-149,
+    7.2448e-151, 2.1255e-152, 6.1257e-154, 1.7343e-155, 4.8239e-157, 1.3181e-158,
+    3.538e-160,  9.3294e-162, 2.4167e-163, 6.1502e-165, 1.5375e-166, 3.7761e-168,
+    9.1103e-170, 2.1593e-171, 5.0276e-173, 1.15e-174,   2.5841e-176, 5.7042e-178,
+    1.237e-179,  2.6352e-181, 5.5149e-183, 1.1338e-184, 2.29e-186,   4.5436e-188,
+    8.8561e-190, 1.6958e-191, 3.1899e-193, 5.8946e-195, 1.0701e-196, 1.9083e-198,
+    3.3433e-200, 5.7541e-202, 9.7287e-204, 1.6159e-205, 2.6366e-207, 4.2263e-209,
+    6.6552e-211, 1.0295e-212, 1.5645e-214, 2.3357e-216, 3.4256e-218, 4.9354e-220,
+    6.9855e-222, 9.7128e-224, 1.3267e-225, 1.7803e-227, 2.3468e-229, 3.039e-231,
+    3.8662e-233, 4.8318e-235, 5.9321e-237, 7.1548e-239, 8.4773e-241, 9.8673e-243,
+    1.1283e-244, 1.2674e-246, 1.3986e-248, 1.5162e-250, 1.6147e-252, 1.6893e-254,
+    1.7362e-256, 1.753e-258,  1.7388e-260, 1.6942e-262, 1.6218e-264, 1.525e-266,
+    1.4088e-268, 1.2785e-270, 1.1398e-272, 9.9826e-275, 8.5888e-277, 7.2594e-279,
+    6.0276e-281, 4.9167e-283, 3.9398e-285, 3.1014e-287, 2.3984e-289, 1.8221e-291,
+    1.3598e-293, 9.9699e-296, 7.1808e-298, 5.0808e-300, 3.5316e-302, 2.4115e-304,
+    1.6177e-306, 1.066e-308,  6.9011e-311, 4.3889e-313, 2.742e-315,  1.6829e-317,
+    1.0147e-319, 6.324e-322,  0.0,         0.0,         0.0,         0.0,
+    0.0,         0.0,         0.0,         0.0,         0.0,         0.0,
+    0.0};
+float bottom_lookup[] = {
+    0.5f,     0.55304f, 0.60514f, 0.65542f, 0.7031f,  0.74751f, 0.78814f, 0.82468f,
+    0.85694f, 0.88493f, 0.90879f, 0.92877f, 0.9452f,  0.95848f, 0.96903f, 0.97725f,
+    0.98355f, 0.98829f, 0.9918f,  0.99435f, 0.99617f, 0.99744f, 0.99832f, 0.99892f,
+    0.99931f, 0.99957f, 0.99974f, 0.99984f, 0.99991f, 0.99994f, 0.99997f, 0.99998f,
+    0.99999f, 0.99999f, 1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,     1.0f,
+    1.0f,     1.0f,     1.0f,     0.99999f, 0.99999f, 0.99998f, 0.99997f, 0.99994f,
+    0.99991f, 0.99984f, 0.99974f, 0.99957f, 0.99931f, 0.99892f, 0.99832f, 0.99744f,
+    0.99617f, 0.99435f, 0.9918f,  0.98829f, 0.98355f, 0.97725f, 0.96903f, 0.95848f,
+    0.9452f,  0.92877f, 0.90879f, 0.88493f, 0.85694f, 0.82468f, 0.78814f, 0.74751f,
+    0.7031f,  0.65542f, 0.60514f, 0.55304f, 0.5f};
 
 using namespace pcl::simulation;
 
 // Finds the maximum level n so a and b are still
 // divisible by 2^n
 int
-max_level (int a, int b)
+max_level(int a, int b)
 {
   int level = 0;
-  while (true)
-  {
-    if (a%2 || b%2) return level;
+  while (true) {
+    if (a % 2 || b % 2)
+      return level;
     a /= 2;
     b /= 2;
     level++;
@@ -119,54 +251,59 @@ max_level (int a, int b)
 // timestamps and displays the elapsed time between them as
 // a fraction and time used [for profiling]
 void
-display_tic_toc (vector<double> &tic_toc,const string &fun_name)
+display_tic_toc(vector<double>& tic_toc, const string& fun_name)
 {
-  size_t tic_toc_size = tic_toc.size ();
+  size_t tic_toc_size = tic_toc.size();
 
   double percent_tic_toc_last = 0;
-  double dtime = tic_toc[tic_toc_size-1] - tic_toc[0];
+  double dtime = tic_toc[tic_toc_size - 1] - tic_toc[0];
   std::cout << "fraction_" << fun_name << ",";
-  for (size_t i = 0; i < tic_toc_size; i++)
-  {
-    double percent_tic_toc =  (tic_toc[i] - tic_toc[0])/(tic_toc[tic_toc_size-1] - tic_toc[0]);
-    std::cout <<  percent_tic_toc - percent_tic_toc_last << ", ";
+  for (size_t i = 0; i < tic_toc_size; i++) {
+    double percent_tic_toc =
+        (tic_toc[i] - tic_toc[0]) / (tic_toc[tic_toc_size - 1] - tic_toc[0]);
+    std::cout << percent_tic_toc - percent_tic_toc_last << ", ";
     percent_tic_toc_last = percent_tic_toc;
   }
   std::cout << "\ntime_" << fun_name << ",";
   double time_tic_toc_last = 0;
-  for (size_t i = 0; i < tic_toc_size; i++)
-  {
-    double percent_tic_toc = (tic_toc[i] - tic_toc[0])/(tic_toc[tic_toc_size-1] - tic_toc[0]);
-    std::cout <<  percent_tic_toc*dtime - time_tic_toc_last << ", ";
-    time_tic_toc_last = percent_tic_toc*dtime;
+  for (size_t i = 0; i < tic_toc_size; i++) {
+    double percent_tic_toc =
+        (tic_toc[i] - tic_toc[0]) / (tic_toc[tic_toc_size - 1] - tic_toc[0]);
+    std::cout << percent_tic_toc * dtime - time_tic_toc_last << ", ";
+    time_tic_toc_last = percent_tic_toc * dtime;
   }
   std::cout << "\ntotal_time_" << fun_name << " " << dtime << "\n";
 }
 
-pcl::simulation::RangeLikelihood::RangeLikelihood (int rows, int cols, int row_height, int col_width, Scene::Ptr scene) :
-      scene_(scene), rows_(rows), cols_(cols), row_height_(row_height), col_width_(col_width),
-      depth_buffer_dirty_(true),
-      color_buffer_dirty_(true),
-      score_buffer_dirty_(true),
-      fbo_ (0),
-      depth_render_buffer_ (0),
-      color_render_buffer_ (0),
-      depth_texture_ (0),
-      score_texture_ (0),
-      score_summarized_texture_ (0),
-      sensor_texture_ (0),
-      likelihood_texture_ (0),
-      compute_likelihood_on_cpu_ (false),
-      aggregate_on_cpu_ (false),
-      use_instancing_ (false),
-      use_color_ (true),
-      sum_reduce_ (cols * col_width, rows * row_height, max_level (col_width, row_height))
+pcl::simulation::RangeLikelihood::RangeLikelihood(
+    int rows, int cols, int row_height, int col_width, Scene::Ptr scene)
+: scene_(scene)
+, rows_(rows)
+, cols_(cols)
+, row_height_(row_height)
+, col_width_(col_width)
+, depth_buffer_dirty_(true)
+, color_buffer_dirty_(true)
+, score_buffer_dirty_(true)
+, fbo_(0)
+, depth_render_buffer_(0)
+, color_render_buffer_(0)
+, depth_texture_(0)
+, score_texture_(0)
+, score_summarized_texture_(0)
+, sensor_texture_(0)
+, likelihood_texture_(0)
+, compute_likelihood_on_cpu_(false)
+, aggregate_on_cpu_(false)
+, use_instancing_(false)
+, use_color_(true)
+, sum_reduce_(cols * col_width, rows * row_height, max_level(col_width, row_height))
 {
   height_ = rows_ * row_height;
   width_ = cols_ * col_width;
 
-  depth_buffer_ = new float[width_*height_];
-  color_buffer_ = new uint8_t[width_*height_*3];
+  depth_buffer_ = new float[width_ * height_];
+  color_buffer_ = new uint8_t[width_ * height_ * 3];
 
   // Set Default Camera Intrinstic Parameters. techquad
   // Correspond closely to those stated here:
@@ -180,336 +317,356 @@ pcl::simulation::RangeLikelihood::RangeLikelihood (int rows, int cols, int row_h
 
   z_near_ = 0.7f;
   z_far_ = 20.0f;
-  
+
   which_cost_function_ = 2; // default to commonly used meter based function
-  
+
   // default lhood parameters - these should always be set by the user
   // so might want to add to constructor eventually:
   sigma_ = 0.1;
-  floor_proportion_ = 0.9; 
+  floor_proportion_ = 0.9;
 
   int height = rows * row_height;
   int width = cols * col_width;
 
   // For now we only support a limited size texture
-  assert (height >0 && height <= 8192 && width > 0 && width <= 8192);
+  assert(height > 0 && height <= 8192 && width > 0 && width <= 8192);
   // throw std::runtime_error "
 
   // Allocate framebuffer
-  glGenRenderbuffers (1, &depth_render_buffer_);
-  glBindRenderbuffer (GL_RENDERBUFFER, depth_render_buffer_);
-  glRenderbufferStorage (GL_RENDERBUFFER, GL_DEPTH_COMPONENT32, width, height);
-  glBindRenderbuffer (GL_RENDERBUFFER, 0);
+  glGenRenderbuffers(1, &depth_render_buffer_);
+  glBindRenderbuffer(GL_RENDERBUFFER, depth_render_buffer_);
+  glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT32, width, height);
+  glBindRenderbuffer(GL_RENDERBUFFER, 0);
 
-  glGenRenderbuffers (1, &color_render_buffer_);
-  glBindRenderbuffer (GL_RENDERBUFFER, color_render_buffer_);
-  glRenderbufferStorage (GL_RENDERBUFFER, GL_RGB8UI, width, height);
-  glBindRenderbuffer (GL_RENDERBUFFER, 0);
+  glGenRenderbuffers(1, &color_render_buffer_);
+  glBindRenderbuffer(GL_RENDERBUFFER, color_render_buffer_);
+  glRenderbufferStorage(GL_RENDERBUFFER, GL_RGB8UI, width, height);
+  glBindRenderbuffer(GL_RENDERBUFFER, 0);
 
   // Setup texture to store depth image
-  glGenTextures (1, &depth_texture_);
-  glBindTexture (GL_TEXTURE_2D, depth_texture_);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
-  glTexImage2D (GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT32, width, height, 0, GL_DEPTH_COMPONENT, GL_FLOAT, nullptr);
-  glBindTexture (GL_TEXTURE_2D, 0);
-
-  glGenTextures (1, &color_texture_);
-  glBindTexture (GL_TEXTURE_2D, color_texture_);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
-  glTexImage2D (GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, nullptr);
-  glBindTexture (GL_TEXTURE_2D, 0);
+  glGenTextures(1, &depth_texture_);
+  glBindTexture(GL_TEXTURE_2D, depth_texture_);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
+  glTexImage2D(GL_TEXTURE_2D,
+               0,
+               GL_DEPTH_COMPONENT32,
+               width,
+               height,
+               0,
+               GL_DEPTH_COMPONENT,
+               GL_FLOAT,
+               nullptr);
+  glBindTexture(GL_TEXTURE_2D, 0);
+
+  glGenTextures(1, &color_texture_);
+  glBindTexture(GL_TEXTURE_2D, color_texture_);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
+  glTexImage2D(
+      GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, nullptr);
+  glBindTexture(GL_TEXTURE_2D, 0);
 
   // Setup texture for incoming image
-  glGenTextures (1, &sensor_texture_);
-  glBindTexture (GL_TEXTURE_2D, sensor_texture_);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
-  glTexImage2D (GL_TEXTURE_2D, 0, GL_R32F, col_width, row_height, 0, GL_RED, GL_FLOAT, nullptr);
-  glBindTexture (GL_TEXTURE_2D, 0);
+  glGenTextures(1, &sensor_texture_);
+  glBindTexture(GL_TEXTURE_2D, sensor_texture_);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
+  glTexImage2D(
+      GL_TEXTURE_2D, 0, GL_R32F, col_width, row_height, 0, GL_RED, GL_FLOAT, nullptr);
+  glBindTexture(GL_TEXTURE_2D, 0);
 
   // Texture for to score on each pixel
-  glGenTextures (1, &score_texture_);
-  glBindTexture (GL_TEXTURE_2D, score_texture_);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
-  glTexImage2D (GL_TEXTURE_2D, 0, GL_R32F, width_, height_, 0, GL_RED, GL_FLOAT, nullptr);
-  glBindTexture (GL_TEXTURE_2D, 0);
+  glGenTextures(1, &score_texture_);
+  glBindTexture(GL_TEXTURE_2D, score_texture_);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
+  glTexImage2D(
+      GL_TEXTURE_2D, 0, GL_R32F, width_, height_, 0, GL_RED, GL_FLOAT, nullptr);
+  glBindTexture(GL_TEXTURE_2D, 0);
 
   // Setup texture for likelihood function
   // size of likelihood texture
   int likelihood_size = 301;
-  glActiveTexture (GL_TEXTURE2);
-  glGenTextures (1, &likelihood_texture_);
-  glBindTexture (GL_TEXTURE_2D, likelihood_texture_);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_R, GL_CLAMP_TO_EDGE);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
-  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_R, GL_RED);
-  glTexImage2D (GL_TEXTURE_2D, 0, GL_R32F, likelihood_size, 1, 0, GL_RED, GL_FLOAT, normal_sigma0x5_normal1x0_range0to3_step0x01);
-  glBindTexture (GL_TEXTURE_2D, 0);
+  glActiveTexture(GL_TEXTURE2);
+  glGenTextures(1, &likelihood_texture_);
+  glBindTexture(GL_TEXTURE_2D, likelihood_texture_);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_R, GL_CLAMP_TO_EDGE);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_R, GL_RED);
+  glTexImage2D(GL_TEXTURE_2D,
+               0,
+               GL_R32F,
+               likelihood_size,
+               1,
+               0,
+               GL_RED,
+               GL_FLOAT,
+               normal_sigma0x5_normal1x0_range0to3_step0x01);
+  glBindTexture(GL_TEXTURE_2D, 0);
 
   // Setup the framebuffer object for rendering
-  glGenFramebuffers (1, &fbo_);
-  glBindFramebuffer (GL_FRAMEBUFFER, fbo_);
-  glFramebufferTexture2D (GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, depth_texture_, 0);
-  glFramebufferTexture2D (GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, color_texture_, 0);
-  glBindFramebuffer (GL_FRAMEBUFFER, 0);
-
-  if (gllib::getGLError() != GL_NO_ERROR)
-  {
-    std::cerr << "RangeLikelihoodGLSL::RangeLikelihoodGLSL: Failed initializing OpenGL buffers" << std::endl;
+  glGenFramebuffers(1, &fbo_);
+  glBindFramebuffer(GL_FRAMEBUFFER, fbo_);
+  glFramebufferTexture2D(
+      GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, depth_texture_, 0);
+  glFramebufferTexture2D(
+      GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, color_texture_, 0);
+  glBindFramebuffer(GL_FRAMEBUFFER, 0);
+
+  if (gllib::getGLError() != GL_NO_ERROR) {
+    std::cerr << "RangeLikelihoodGLSL::RangeLikelihoodGLSL: Failed initializing OpenGL "
+                 "buffers"
+              << std::endl;
     exit(-1);
   }
 
-  glGenFramebuffers (1, &score_fbo_);
-  glBindFramebuffer (GL_FRAMEBUFFER, score_fbo_);
-  glFramebufferRenderbuffer (GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, 0);
-  glFramebufferTexture2D (GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, score_texture_, 0);
-  glBindFramebuffer (GL_FRAMEBUFFER, 0);
+  glGenFramebuffers(1, &score_fbo_);
+  glBindFramebuffer(GL_FRAMEBUFFER, score_fbo_);
+  glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, 0);
+  glFramebufferTexture2D(
+      GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, score_texture_, 0);
+  glBindFramebuffer(GL_FRAMEBUFFER, 0);
 
   // Load shader
-  likelihood_program_ = gllib::Program::Ptr (new gllib::Program ());
+  likelihood_program_ = gllib::Program::Ptr(new gllib::Program());
   // TODO: to remove file dependency include the shader source in the binary
-  if (!likelihood_program_->addShaderFile ("compute_score.vert", gllib::VERTEX))
-  {
+  if (!likelihood_program_->addShaderFile("compute_score.vert", gllib::VERTEX)) {
     std::cout << "Failed loading vertex shader" << std::endl;
-    exit (-1);
+    exit(-1);
   }
 
-  if (!likelihood_program_->addShaderFile ("compute_score.frag", gllib::FRAGMENT))
-  {
+  if (!likelihood_program_->addShaderFile("compute_score.frag", gllib::FRAGMENT)) {
     std::cout << "Failed loading fragment shader" << std::endl;
-    exit (-1);
+    exit(-1);
   }
 
-  likelihood_program_->link ();
+  likelihood_program_->link();
 
-  vertices_.emplace_back(-1.0,  1.0, 0.0);
-  vertices_.emplace_back( 1.0,  1.0, 0.0);
-  vertices_.emplace_back( 1.0, -1.0, 0.0);
+  vertices_.emplace_back(-1.0, 1.0, 0.0);
+  vertices_.emplace_back(1.0, 1.0, 0.0);
+  vertices_.emplace_back(1.0, -1.0, 0.0);
   vertices_.emplace_back(-1.0, -1.0, 0.0);
 
-  glGenBuffers (1, &quad_vbo_);
-  glBindBuffer (GL_ARRAY_BUFFER, quad_vbo_);
-  glBufferData (GL_ARRAY_BUFFER, sizeof (Eigen::Vector3f) * vertices_.size (), &(vertices_[0]), GL_STATIC_DRAW);
+  glGenBuffers(1, &quad_vbo_);
+  glBindBuffer(GL_ARRAY_BUFFER, quad_vbo_);
+  glBufferData(GL_ARRAY_BUFFER,
+               sizeof(Eigen::Vector3f) * vertices_.size(),
+               &(vertices_[0]),
+               GL_STATIC_DRAW);
   glBindBuffer(GL_ARRAY_BUFFER, 0);
 
-  gllib::getGLError ();
+  gllib::getGLError();
 
   // Go back to the default pipeline
-  glUseProgram (0);
-
-  score_buffer_ = new float[width_*height_];
+  glUseProgram(0);
 
+  score_buffer_ = new float[width_ * height_];
 }
 
-pcl::simulation::RangeLikelihood::~RangeLikelihood ()
+pcl::simulation::RangeLikelihood::~RangeLikelihood()
 {
-  glDeleteBuffers (1, &quad_vbo_);
-  glDeleteTextures (1, &depth_texture_);
-  glDeleteTextures (1, &color_texture_);
-  glDeleteTextures (1, &score_texture_);
-  glDeleteTextures (1, &score_summarized_texture_);
-  glDeleteTextures (1, &sensor_texture_);
-  glDeleteTextures (1, &likelihood_texture_);
-  glDeleteFramebuffers (1, &fbo_);
-  glDeleteFramebuffers (1, &score_fbo_);
-  glDeleteRenderbuffers (1, &depth_render_buffer_);
-  glDeleteRenderbuffers (1, &color_render_buffer_);
-
-  delete [] depth_buffer_;
-  delete [] color_buffer_;
-  delete [] score_buffer_;
+  glDeleteBuffers(1, &quad_vbo_);
+  glDeleteTextures(1, &depth_texture_);
+  glDeleteTextures(1, &color_texture_);
+  glDeleteTextures(1, &score_texture_);
+  glDeleteTextures(1, &score_summarized_texture_);
+  glDeleteTextures(1, &sensor_texture_);
+  glDeleteTextures(1, &likelihood_texture_);
+  glDeleteFramebuffers(1, &fbo_);
+  glDeleteFramebuffers(1, &score_fbo_);
+  glDeleteRenderbuffers(1, &depth_render_buffer_);
+  glDeleteRenderbuffers(1, &color_render_buffer_);
+
+  delete[] depth_buffer_;
+  delete[] color_buffer_;
+  delete[] score_buffer_;
 }
 
 double
-pcl::simulation::RangeLikelihood::sampleNormal (double sigma)
+pcl::simulation::RangeLikelihood::sampleNormal(double sigma)
 {
-  std::normal_distribution<> dist (0.0, sigma);
-  return (dist (rng));
+  std::normal_distribution<> dist(0.0, sigma);
+  return (dist(rng));
 }
 
 void
-pcl::simulation::RangeLikelihood::setupProjectionMatrix ()
+pcl::simulation::RangeLikelihood::setupProjectionMatrix()
 {
-  glMatrixMode (GL_PROJECTION);
-  glLoadIdentity ();
+  glMatrixMode(GL_PROJECTION);
+  glLoadIdentity();
 
   // Prepare scaled simulated camera projection matrix
-  float sx = static_cast<float> (camera_width_) / static_cast<float> (col_width_);
-  float sy = static_cast<float> (camera_height_) / static_cast<float> (row_height_);
-  float width = static_cast<float> (col_width_);
-  float height = static_cast<float> (row_height_);
-
-  float fx = camera_fx_/sx;
-  float fy = camera_fy_/sy;
-  float cx = camera_cx_/sx;
-  float cy = camera_cy_/sy;
-  float m[16];
-  float z_nf = (z_near_-z_far_);
-
-  m[0] = 2.0f*fx/width;  m[4] = 0;              m[ 8] = 1.0f-(2*cx/width);      m[12] = 0;
-  m[1] = 0;              m[5] = 2.0f*fy/height; m[ 9] = 1.0f-(2*cy/height);     m[13] = 0;
-  m[2] = 0;              m[6] = 0;              m[10] = (z_far_+z_near_)/z_nf;  m[14] = 2.0f*z_near_*z_far_/z_nf;
-  m[3] = 0;              m[7] = 0;              m[11] = -1.0f;                  m[15] = 0;
-  glMultMatrixf (m);
+  float sx = static_cast<float>(camera_width_) / static_cast<float>(col_width_);
+  float sy = static_cast<float>(camera_height_) / static_cast<float>(row_height_);
+  float width = static_cast<float>(col_width_);
+  float height = static_cast<float>(row_height_);
+
+  float fx = camera_fx_ / sx;
+  float fy = camera_fy_ / sy;
+  float cx = camera_cx_ / sx;
+  float cy = camera_cy_ / sy;
+  float z_nf = (z_near_ - z_far_);
+
+  // clang-format off
+  float m[16] = {2.0f * fx / width       , 0                        , 0                              , 0     ,
+                 0                       , 2.0f * fy / height       , 0                              , 0     ,
+                 1.0f - (2 * cx / width) , 1.0f - (2 * cy / height) , (z_far_ + z_near_) / z_nf      , -1.0f ,
+                 0                       , 0                        , 2.0f * z_near_ * z_far_ / z_nf , 0};
+  // clang-format on
+
+  glMultMatrixf(m);
 }
 
 void
-pcl::simulation::RangeLikelihood::applyCameraTransform (const Eigen::Isometry3d & pose)
+pcl::simulation::RangeLikelihood::applyCameraTransform(const Eigen::Isometry3d& pose)
 {
-  float T[16];
-  Eigen::Matrix4f m = (pose.matrix ().inverse ()).cast<float> ();
-  T[0] = m(0,0); T[4] = m(0,1); T[8] = m(0,2); T[12] = m(0,3);
-  T[1] = m(1,0); T[5] = m(1,1); T[9] = m(1,2); T[13] = m(1,3);
-  T[2] = m(2,0); T[6] = m(2,1); T[10] = m(2,2); T[14] = m(2,3);
-  T[3] = m(3,0); T[7] = m(3,1); T[11] = m(3,2); T[15] = m(3,3);
+  Eigen::Matrix4f m = (pose.matrix().inverse()).cast<float>();
+  // clang-format off
+  float T[16] = {
+      m(0, 0), m(1, 0), m(2, 0), m(3, 0),
+      m(0, 1), m(1, 1), m(2, 1), m(3, 1),
+      m(0, 2), m(1, 2), m(2, 2), m(3, 2),
+      m(0, 3), m(1, 3), m(2, 3), m(3, 3),
+  };
+  // clang-format on
   glMultMatrixf(T);
 }
 
 void
-pcl::simulation::RangeLikelihood::drawParticles (std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses)
+pcl::simulation::RangeLikelihood::drawParticles(
+    std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses)
 {
   int n = 0;
-  for (int i=0; i<rows_; ++i)
-  {
-    for (int j=0; j<cols_; ++j)
-    {
-      glMatrixMode (GL_MODELVIEW);
-      glLoadIdentity ();
+  for (int i = 0; i < rows_; ++i) {
+    for (int j = 0; j < cols_; ++j) {
+      glMatrixMode(GL_MODELVIEW);
+      glLoadIdentity();
 
-      glViewport(j*col_width_, i*row_height_, col_width_, row_height_);
+      glViewport(j * col_width_, i * row_height_, col_width_, row_height_);
 
       // Go from Z-up, X-forward coordinate frame
       // to OpenGL Z-out,Y-up [both Right Handed]
-      float T[16];
-      T[0] =  0;   T[4] = -1.0; T[8] =  0; T[12] = 0;
-      T[1] =  0;   T[5] = 0;    T[9] =  1; T[13] = 0;
-      T[2] = -1.0; T[6] = 0;    T[10] = 0; T[14] = 0;
-      T[3] =  0;   T[7] = 0;    T[11] = 0; T[15] = 1;
-      glMultMatrixf (T);
+      // clang-format off
+      float T[16] = {
+           0, 0, -1, 0,
+          -1, 0,  0, 0,
+           0, 1,  0, 0,
+           0, 0,  0, 1,
+      };
+      // clang-format on
+      glMultMatrixf(T);
 
       // Apply camera transformation
-      applyCameraTransform (poses[n++]);
+      applyCameraTransform(poses[n++]);
 
       // Draw the planes in each location:
-      scene_->draw ();
+      scene_->draw();
     }
   }
 }
 
-
 /////////////////////////////////////////////////////////////////
 // Below are 4 previously used cost functions:
 // 0 original scoring method
 float
-costFunction0 (float ref_val,float depth_val)
+costFunction0(float ref_val, float depth_val)
 {
-  return  (sqr(ref_val - depth_val));
+  return (sqr(ref_val - depth_val));
 }
 
 // 1st working cost function:
 // Empirical reverse mapping between depthbuffer and true depth:
 // Version 0: [25 aug 2011]
 // TRUEDEPTH =  1/(1.33 -(DEPTHBUFFER)*1.29)
-//float cost = sqr(ref[col%col_width] - 1/(1.33 -(*depth)*1.29));
+// float cost = sqr(ref[col%col_width] - 1/(1.33 -(*depth)*1.29));
 // Version 1: [29 aug 2011] Exact version using correct mappings:
 float
-costFunction1 (float ref_val, float depth_val)
+costFunction1(float ref_val, float depth_val)
 {
-  float cost = sqr (ref_val - 1/(1.4285f - (depth_val)*1.3788f));
-  //std::cout << " [" << ref_val << "," << 1/(1.4285 -(depth_val)*1.3788) << "] ";
-  if (ref_val < 0)
-  { // all images pixels with no range
-    cost =1;
+  float cost = sqr(ref_val - 1 / (1.4285f - (depth_val)*1.3788f));
+  // std::cout << " [" << ref_val << "," << 1/(1.4285 -(depth_val)*1.3788) << "] ";
+  if (ref_val < 0) { // all images pixels with no range
+    cost = 1;
   }
-  if (cost > 10)
-  { // required to lessen the effect of modelpixel with no range (ie holes in the model)
+  if (cost > 10) {
+    // required to lessen the effect of modelpixel with no range (ie holes in the model)
     cost = 10;
   }
-  //return std::log (cost);
+  // return std::log (cost);
   return (cost);
 }
 
 // 1st working likelihood function (by far most commonly used)
 float
-costFunction2 (float ref_val, float depth_val)
+costFunction2(float ref_val, float depth_val)
 {
-  float min_dist = std::abs(ref_val - 1/(1.4285f - (depth_val)*1.3788f));
-  int lup = static_cast<int> (std::ceil (min_dist*100)); // has resolution of 0.01m
+  float min_dist = std::abs(ref_val - 1 / (1.4285f - (depth_val)*1.3788f));
+  int lup = static_cast<int>(std::ceil(min_dist * 100)); // has resolution of 0.01m
 
-  if (lup > 300)
-  { // implicitly this caps the cost if there is a hole in the model
+  if (lup > 300) { // implicitly this caps the cost if there is a hole in the model
     lup = 300;
   }
-  
+
   double lhood = 1;
-  if (std::isnan (depth_val))
-  { // pixels with nan depth - for openNI null points
-    lhood = 1; // std::log(1) = 0 ---> has no effect
+  if (std::isnan(depth_val)) { // pixels with nan depth - for openNI null points
+    lhood = 1;                 // std::log(1) = 0 ---> has no effect
   }
-  else if(ref_val < 0)
-  { // all RGB pixels with no depth - for freenect null points
-    lhood = 1; // std::log(1) = 0 ---> has no effect
+  else if (ref_val < 0) { // all RGB pixels with no depth - for freenect null points
+    lhood = 1;            // std::log(1) = 0 ---> has no effect
   }
-  else
-  {
+  else {
     lhood = normal_sigma0x5_normal1x0_range0to3_step0x01[lup];
     // add a ground floor:
     // increasing this will mean that the likelihood is less peaked
     // but you need more particles to do this...
     // with ~90particles user 0.999, for example in the quad dataset
     // ratio of uniform to	normal
-    double ratio = 0.99;//was always 0.99; 
-    double r_min = 0; // metres
-    double r_max = 3; // metres
-    lhood = ratio/(r_max -r_min) + (1-ratio)*lhood ;
+    double ratio = 0.99; // was always 0.99;
+    double r_min = 0;    // metres
+    double r_max = 3;    // metres
+    lhood = ratio / (r_max - r_min) + (1 - ratio) * lhood;
   }
-  return static_cast<float> (std::log (lhood));
+  return static_cast<float>(std::log(lhood));
 }
 
 float
-costFunction3 (float ref_val,float depth_val)
+costFunction3(float ref_val, float depth_val)
 {
-  float log_lhood=0;
+  float log_lhood = 0;
   // std::log(1) = 0 ---> has no effect
-  if (ref_val < 0)
-  {
+  if (ref_val < 0) {
     // all images pixels with no range
   }
-  else if (ref_val > 7)
-  {
+  else if (ref_val > 7) {
     // ignore long ranges... for now
   }
-  else
-  { // working range
-    float min_dist = std::abs (ref_val - 0.7253f/(1.0360f - (depth_val)));
+  else { // working range
+    float min_dist = std::abs(ref_val - 0.7253f / (1.0360f - (depth_val)));
 
-    int lup = static_cast<int> (std::ceil (min_dist*100)); // has resolution of 0.01m
-    if (lup > 300)
-    { // implicitly this caps the cost if there is a hole in the model
+    int lup = static_cast<int>(std::ceil(min_dist * 100)); // has resolution of 0.01m
+    if (lup > 300) { // implicitly this caps the cost if there is a hole in the model
       lup = 300;
     }
     log_lhood = hard_coded_log_lhood[lup];
@@ -518,39 +675,38 @@ costFunction3 (float ref_val,float depth_val)
 }
 
 float
-costFunction4(float ref_val,float depth_val)
+costFunction4(float ref_val, float depth_val)
 {
-  float disparity_diff = std::abs( ( -0.7253f/ref_val +1.0360f ) -  depth_val );
+  float disparity_diff = std::abs((-0.7253f / ref_val + 1.0360f) - depth_val);
 
-  int top_lup = static_cast<int> (std::ceil (disparity_diff*300)); // has resolution of 0.001m
-  if (top_lup > 300)
-  {
-    top_lup =300;
+  int top_lup =
+      static_cast<int>(std::ceil(disparity_diff * 300)); // has resolution of 0.001m
+  if (top_lup > 300) {
+    top_lup = 300;
   }
-  float top = static_cast<float> (top_lookup[top_lup]);// round( std::abs(x-mu) *1000+1) );
+  float top =
+      static_cast<float>(top_lookup[top_lup]); // round( std::abs(x-mu) *1000+1) );
 
   // bottom:
-  //bottom = bottom_lookup(   round(mu*1000+1));
-  int bottom_lup = static_cast<int> (std::ceil( (depth_val) * 300)); // has resolution of 0.001m
-  if (bottom_lup > 300)
-  {
-    bottom_lup =300;
+  // bottom = bottom_lookup(   round(mu*1000+1));
+  int bottom_lup =
+      static_cast<int>(std::ceil((depth_val)*300)); // has resolution of 0.001m
+  if (bottom_lup > 300) {
+    bottom_lup = 300;
   }
-  float bottom = bottom_lookup[bottom_lup];// round( std::abs(x-mu) *1000+1) );
+  float bottom = bottom_lookup[bottom_lup]; // round( std::abs(x-mu) *1000+1) );
 
   float proportion = 0.999f;
-  float lhood = proportion + (1-proportion)*(top/bottom);
+  float lhood = proportion + (1 - proportion) * (top / bottom);
 
   // safety fix that seems to be required due to opengl asynchronization
   // ask hordur about this
-  if (bottom == 0)
-  {
+  if (bottom == 0) {
     lhood = proportion;
   }
 
-  if (ref_val< 0)
-  { // all images pixels with no range
-    lhood = 1; // std::log(1) = 0 ---> has no effect
+  if (ref_val < 0) { // all images pixels with no range
+    lhood = 1;       // std::log(1) = 0 ---> has no effect
   }
   return std::log(lhood);
 }
@@ -558,36 +714,43 @@ costFunction4(float ref_val,float depth_val)
 // TODO: WHEN WE'RE HAPPY THIS SHOULD BE "THE" LIKELIHOOD FUNCTION
 // add these global variables into the class
 // abd use sigma and floor_proportion directly from class also
-using boost::math::normal; // typedef provides default type is double.
-normal unit_norm_dist(0,1); // (default mean = zero, and standard deviation = unity)
-double costFunction5(double measured_depth, double model_disp, double sigma, double floor_proportion)
+using boost::math::normal;   // typedef provides default type is double.
+normal unit_norm_dist(0, 1); // (default mean = zero, and standard deviation = unity)
+double
+costFunction5(double measured_depth,
+              double model_disp,
+              double sigma,
+              double floor_proportion)
 {
   // NEED TO CONVERT MEASURED TO DISPARITY
-  double measured_disp =  (-0.7253/measured_depth + 1.0360 );
+  double measured_disp = (-0.7253 / measured_depth + 1.0360);
 
   // measured_depth = ref_val  [m]
   // model_disp = depth_val [0-1]
   // upper and lower bound on depth buffer:
-  double lower_bound =0;
-  double upper_bound =1;
+  double lower_bound = 0;
+  double upper_bound = 1;
+
+  double gaussian_part =
+      pdf(unit_norm_dist, (measured_disp - model_disp) / sigma) / sigma;
+  double truncation = 1 / cdf(unit_norm_dist, (upper_bound - model_disp) / sigma) -
+                      cdf(unit_norm_dist, (lower_bound - model_disp) / sigma);
 
-  double gaussian_part = pdf(unit_norm_dist, (measured_disp-model_disp)/sigma)/sigma;
-  double truncation = 1/cdf(unit_norm_dist,(upper_bound-model_disp)/sigma) -   cdf(unit_norm_dist, (lower_bound-model_disp)/sigma);
-	
-  double trunc_gaussian_part = truncation*gaussian_part;
+  double trunc_gaussian_part = truncation * gaussian_part;
 
-  double lhood= (floor_proportion/(upper_bound-lower_bound) + (1-floor_proportion)*trunc_gaussian_part);
+  double lhood = (floor_proportion / (upper_bound - lower_bound) +
+                  (1 - floor_proportion) * trunc_gaussian_part);
 
-  if (measured_depth< 0){ // all images pixels with no range
-    lhood = 1; // std::log(1) = 0 ---> has no effect
+  if (measured_depth < 0) { // all images pixels with no range
+    lhood = 1;              // std::log(1) = 0 ---> has no effect
   }
 
-  return std::log (lhood);
+  return std::log(lhood);
 }
 
 void
-pcl::simulation::RangeLikelihood::computeScores (float* reference,
-                                                 std::vector<float> & scores)
+pcl::simulation::RangeLikelihood::computeScores(float* reference,
+                                                std::vector<float>& scores)
 {
   const float* depth = getDepthBuffer();
   // Mapping between disparity and range:
@@ -605,66 +768,62 @@ pcl::simulation::RangeLikelihood::computeScores (float* reference,
 
   // ref[col%col_width] - z/depth value in metres,   0-> ~20
   // depth_val - contents of depth buffer [0->1]
-  
+
   // for row across each image in a row of model images
-  for (int row = 0; row < rows_*row_height_; row++)
-  {
-    float* ref = reference + col_width_*(row % row_height_);
+  for (int row = 0; row < rows_ * row_height_; row++) {
+    float* ref = reference + col_width_ * (row % row_height_);
     // for each column: across each image in a column of model images
-    for (int col = 0; col < cols_*col_width_; col++)
-    {
-      float depth_val = (*depth++); // added jan 2012 - check this is not a breaking fix later mfallon
+    for (int col = 0; col < cols_ * col_width_; col++) {
+      float depth_val =
+          (*depth++); // added jan 2012 - check this is not a breaking fix later mfallon
       float score = 0;
-      if (which_cost_function_ == 0)
-      {
-        score = costFunction0 (ref[col%col_width_],depth_val);
+      if (which_cost_function_ == 0) {
+        score = costFunction0(ref[col % col_width_], depth_val);
       }
-      else if (which_cost_function_ == 1)
-      {
-	score = costFunction1 (ref[col%col_width_],depth_val);
+      else if (which_cost_function_ == 1) {
+        score = costFunction1(ref[col % col_width_], depth_val);
       }
-      else if (which_cost_function_ == 2)
-      {
-        score = costFunction2 (ref[col%col_width_],depth_val);
+      else if (which_cost_function_ == 2) {
+        score = costFunction2(ref[col % col_width_], depth_val);
       }
-      else if(which_cost_function_==3)
-      {
-	score = costFunction3 (ref[col%col_width_],depth_val);
+      else if (which_cost_function_ == 3) {
+        score = costFunction3(ref[col % col_width_], depth_val);
       }
-      else if (which_cost_function_ == 4)
-      {
-        score = costFunction4 (ref[col%col_width_],depth_val);
+      else if (which_cost_function_ == 4) {
+        score = costFunction4(ref[col % col_width_], depth_val);
       }
-      else if (which_cost_function_ == 5)
-      {
-	//double sigma = 0.025;
-	//double floor_proportion_ = 0.999;
-        score = static_cast<float> (costFunction5 (ref[col%col_width_],depth_val,sigma_,floor_proportion_));
+      else if (which_cost_function_ == 5) {
+        // double sigma = 0.025;
+        // double floor_proportion_ = 0.999;
+        score = static_cast<float>(
+            costFunction5(ref[col % col_width_], depth_val, sigma_, floor_proportion_));
       }
-      scores[row/row_height_ * cols_ + col/col_width_] += score;
-      //std::cout << "(" << scores[row/row_height_ * cols_ + col/col_width_] <<"," << score << "," << ref[col%col_width_] << "," << depth_val << ") ";
-      score_buffer_[row*width_ + col] =  score;
+      scores[row / row_height_ * cols_ + col / col_width_] += score;
+      // std::cout << "(" << scores[row/row_height_ * cols_ + col/col_width_] <<"," <<
+      // score << "," << ref[col%col_width_] << "," << depth_val << ") ";
+      score_buffer_[row * width_ + col] = score;
     }
   }
   score_buffer_dirty_ = false;
 }
 
 void
-pcl::simulation::RangeLikelihood::getPointCloud (pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc,
-  bool make_global,
-  const Eigen::Isometry3d & pose,
-  bool organized) const
+pcl::simulation::RangeLikelihood::getPointCloud(
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc,
+    bool make_global,
+    const Eigen::Isometry3d& pose,
+    bool organized) const
 {
   // TODO: check if this works for for rows/cols >1  and for width&height != 640x480
   // i.e. multiple tiled images
-  pc->width    = col_width_;
-  pc->height   = row_height_;
+  pc->width = col_width_;
+  pc->height = row_height_;
   // Was:
-  //pc->width    = camera_width_;
-  //pc->height   = camera_height_;
+  // pc->width    = camera_width_;
+  // pc->height   = camera_height_;
 
   pc->is_dense = true;
-  pc->points.resize (pc->width*pc->height);
+  pc->points.resize(pc->width * pc->height);
 
   int points_added = 0;
 
@@ -678,101 +837,110 @@ pcl::simulation::RangeLikelihood::getPointCloud (pcl::PointCloud<pcl::PointXYZRG
   // TODO: support decimation
   // Copied the format of RangeImagePlanar::setDepthImage()
   // Use this as a template for decimation
-  for (int y = 0; y < row_height_ ; ++y) //camera_height_
+  for (int y = 0; y < row_height_; ++y) // camera_height_
   {
-    for (int x = 0; x < col_width_ ; ++x)  // camera_width_
+    for (int x = 0; x < col_width_; ++x) // camera_width_
     {
       // Find XYZ from normalized 0->1 mapped disparity
       int idx;
-      if (organized) idx = y*col_width_ + x;
-      else idx = points_added; // y*camera_width_ + x;
+      if (organized)
+        idx = y * col_width_ + x;
+      else
+        idx = points_added; // y*camera_width_ + x;
 
-      float d = depth_buffer_[y*camera_width_ + x] ;
+      float d = depth_buffer_[y * camera_width_ + x];
 
       if (d < 1.0) // only add points with depth buffer less than max (20m) range
       {
-        float z = zf*zn/((zf-zn)*(d - zf/(zf-zn)));
+        float z = zf * zn / ((zf - zn) * (d - zf / (zf - zn)));
 
         // TODO: add mode to ignore points with no return i.e. depth_buffer_ ==1
-        // NB: OpenGL uses a Right Hand system with +X right, +Y up, +Z back out of the screen,
-        // The Z-buffer is natively -1 (far) to 1 (near)
-        // But in this class we invert this to be 0 (near, 0.7m) and 1 (far, 20m)
-        // ... so by negating y we get to a right-hand computer vision system
-        // which is also used by PCL and OpenNi
+        // NB: OpenGL uses a Right Hand system with +X right, +Y up, +Z back out of the
+        // screen, The Z-buffer is natively -1 (far) to 1 (near). But in this class we
+        // invert this to be 0 (near, 0.7m) and 1 (far, 20m), so by negating y we get to
+        // a right-hand computer vision system which is also used by PCL and OpenNi.
         pc->points[idx].z = z;
-        pc->points[idx].x = (static_cast<float> (x)-camera_cx_) * z * (-camera_fx_reciprocal_);
-        pc->points[idx].y = (static_cast<float> (y)-camera_cy_) * z * (-camera_fy_reciprocal_);
-
-        int rgb_idx = y*col_width_ + x;  //camera_width_
-        pc->points[idx].b = color_buffer[rgb_idx*3+2]; // blue
-        pc->points[idx].g = color_buffer[rgb_idx*3+1]; // green
-        pc->points[idx].r = color_buffer[rgb_idx*3]; // red
+        pc->points[idx].x =
+            (static_cast<float>(x) - camera_cx_) * z * (-camera_fx_reciprocal_);
+        pc->points[idx].y =
+            (static_cast<float>(y) - camera_cy_) * z * (-camera_fy_reciprocal_);
+
+        int rgb_idx = y * col_width_ + x;                  // camera_width_
+        pc->points[idx].b = color_buffer[rgb_idx * 3 + 2]; // blue
+        pc->points[idx].g = color_buffer[rgb_idx * 3 + 1]; // green
+        pc->points[idx].r = color_buffer[rgb_idx * 3];     // red
         points_added++;
       }
-      else if (organized)
-      {
+      else if (organized) {
         pc->is_dense = false;
-        pc->points[idx].z = std::numeric_limits<float>::quiet_NaN ();
-        pc->points[idx].x = std::numeric_limits<float>::quiet_NaN ();
-        pc->points[idx].y = std::numeric_limits<float>::quiet_NaN ();
+        pc->points[idx].z = std::numeric_limits<float>::quiet_NaN();
+        pc->points[idx].x = std::numeric_limits<float>::quiet_NaN();
+        pc->points[idx].y = std::numeric_limits<float>::quiet_NaN();
         pc->points[idx].rgba = 0;
       }
     }
   }
 
-  if (!organized)
-  {
-    pc->width    = 1;
-    pc->height   = points_added;
-    pc->points.resize (points_added);
+  if (!organized) {
+    pc->width = 1;
+    pc->height = points_added;
+    pc->points.resize(points_added);
   }
 
-  if (make_global)
-  {
+  if (make_global) {
     // Go from OpenGL to (Z-up, X-forward, Y-left)
     Eigen::Matrix4f T;
+    // clang-format off
     T <<  0, 0, -1, 0,
          -1, 0,  0, 0,
           0, 1,  0, 0,
           0, 0,  0, 1;
-    Eigen::Matrix4f m = pose.matrix ().cast<float> ();
+    // clang-format on
+    Eigen::Matrix4f m = pose.matrix().cast<float>();
     m *= T;
-    pcl::transformPointCloud (*pc, *pc, m);
+    pcl::transformPointCloud(*pc, *pc, m);
   }
-  else
-  {
+  else {
     // Go from OpenGL to Camera (Z-forward, X-right, Y-down)
     Eigen::Matrix4f T;
-    T <<  1,  0,  0, 0,
-          0, -1,  0, 0,
-          0,  0, -1, 0,
-          0,  0,  0, 1;
-    pcl::transformPointCloud (*pc, *pc, T);
+    // clang-format off
+    T << 1,  0,  0, 0,
+         0, -1,  0, 0,
+         0,  0, -1, 0,
+         0,  0,  0, 1;
+    // clang-format on
+    pcl::transformPointCloud(*pc, *pc, T);
 
     // Go from Camera to body (Z-up, X-forward, Y-left)
     Eigen::Matrix4f cam_to_body;
+    // clang-format off
     cam_to_body <<  0,  0, 1, 0,
                    -1,  0, 0, 0,
                     0, -1, 0, 0,
                     0,  0, 0, 1;
-    Eigen::Matrix4f camera = pose.matrix ().cast<float> ();
+    // clang-format on
+    Eigen::Matrix4f camera = pose.matrix().cast<float>();
     camera *= cam_to_body;
-    pc->sensor_origin_ = camera.rightCols (1);
-    Eigen::Quaternion<float> quat (camera.block<3,3> (0,0));
+    pc->sensor_origin_ = camera.rightCols(1);
+    Eigen::Quaternion<float> quat(camera.block<3, 3>(0, 0));
     pc->sensor_orientation_ = quat;
   }
 }
 
 void
-pcl::simulation::RangeLikelihood::getRangeImagePlanar(pcl::RangeImagePlanar &rip) const
+pcl::simulation::RangeLikelihood::getRangeImagePlanar(pcl::RangeImagePlanar& rip) const
 {
-  rip.setDepthImage (depth_buffer_,
-    camera_width_,camera_height_, camera_fx_,camera_fy_,
-   camera_fx_, camera_fy_);
+  rip.setDepthImage(depth_buffer_,
+                    camera_width_,
+                    camera_height_,
+                    camera_fx_,
+                    camera_fy_,
+                    camera_fx_,
+                    camera_fy_);
 }
-  
+
 void
-pcl::simulation::RangeLikelihood::addNoise ()
+pcl::simulation::RangeLikelihood::addNoise()
 {
   // Other noises:
   // edge noise: look for edges in the range image and add a few pixels here and there
@@ -782,17 +950,13 @@ pcl::simulation::RangeLikelihood::addNoise ()
   // TODO: make the variance a parameter
   // TODO: might want to add a range-based variance
   float variance = 0.0015f;
-  for (int i = 0; i < camera_width_*camera_height_ ; i++)
-  {
-    if (depth_buffer_[i] < 1)
-    {
-      depth_buffer_[i] += variance * static_cast<float> (sampleNormal ());
-      if (depth_buffer_[i] > 1)
-      {
+  for (int i = 0; i < camera_width_ * camera_height_; i++) {
+    if (depth_buffer_[i] < 1) {
+      depth_buffer_[i] += variance * static_cast<float>(sampleNormal());
+      if (depth_buffer_[i] > 1) {
         depth_buffer_[i] = 1.0;
       }
-      else if (depth_buffer_[i] < 0)
-      {
+      else if (depth_buffer_[i] < 0) {
         depth_buffer_[i] = 0.0;
       }
     }
@@ -806,246 +970,250 @@ pcl::simulation::RangeLikelihood::addNoise ()
   // http://www.ros.org/wiki/kinect_calibration/technical
   // TODO: make a parameter
   float bins = 470;
-  for (int i = 0; i < camera_width_*camera_height_ ; i++)
-  {
-    depth_buffer_[i] =  std::ceil (depth_buffer_[i]*bins)/bins;
+  for (int i = 0; i < camera_width_ * camera_height_; i++) {
+    depth_buffer_[i] = std::ceil(depth_buffer_[i] * bins) / bins;
   }
   std::cout << "in add noise\n";
 }
 
 void
-RangeLikelihood::computeLikelihoods (float* reference,
-                                     std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses,
-                                     std::vector<float> & scores)
+RangeLikelihood::computeLikelihoods(
+    float* reference,
+    std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses,
+    std::vector<float>& scores)
 {
-  #if DO_TIMING_PROFILE
-    std::vector<double> tic_toc;
-    tic_toc.push_back(getTime());
-  #endif  
-  
-  scores.resize (cols_*rows_);
-  std::fill (scores.begin (), scores.end (), 0);
-  
+#if DO_TIMING_PROFILE
+  std::vector<double> tic_toc;
+  tic_toc.push_back(getTime());
+#endif
+
+  scores.resize(cols_ * rows_);
+  std::fill(scores.begin(), scores.end(), 0);
+
   // Generate depth image for each particle
-  render (poses);
-  
-  #if DO_TIMING_PROFILE
-    tic_toc.push_back (getTime ());
-  #endif
-
-  #if DO_TIMING_PROFILE
-    tic_toc.push_back (getTime ());
-  #endif  
+  render(poses);
+
+#if DO_TIMING_PROFILE
+  tic_toc.push_back(getTime());
+#endif
+
+#if DO_TIMING_PROFILE
+  tic_toc.push_back(getTime());
+#endif
   // The depth image is now in depth_texture_
 
   // Compute likelihoods
-  if (compute_likelihood_on_cpu_)
-  {
-    computeScores (reference, scores);
+  if (compute_likelihood_on_cpu_) {
+    computeScores(reference, scores);
   }
-  else
-  {
-    computeScoresShader (reference);
-    
+  else {
+    computeScoresShader(reference);
+
     // Aggregate results (we do not use GPU to sum cpu scores)
-    if (aggregate_on_cpu_)
-    {
+    if (aggregate_on_cpu_) {
       const float* score_buffer = getScoreBuffer();
 
-      for (int n = 0, row = 0; row < height_; ++row)
-      {
-        for (int col = 0; col < width_; ++col, ++n)
-        {
-          scores[row/row_height_ * cols_ + col/col_width_] += score_buffer[n];
+      for (int n = 0, row = 0; row < height_; ++row) {
+        for (int col = 0; col < width_; ++col, ++n) {
+          scores[row / row_height_ * cols_ + col / col_width_] += score_buffer[n];
         }
       }
     }
-    else
-    {
-      int levels = max_level (row_height_, col_width_);
+    else {
+      int levels = max_level(row_height_, col_width_);
       int reduced_width = width_ >> levels;
       int reduced_height = height_ >> levels;
       int reduced_col_width = col_width_ >> levels;
       int reduced_row_height = row_height_ >> levels;
 
       float* score_sum = new float[reduced_width * reduced_height];
-      sum_reduce_.sum (score_texture_, score_sum);
-      for (int n = 0, row = 0; row < reduced_height; ++row)
-      {
-        for (int col = 0; col < reduced_width; ++col, ++n)
-        {
-          scores[row/reduced_row_height * cols_ + col/reduced_col_width] += score_sum[n];
+      sum_reduce_.sum(score_texture_, score_sum);
+      for (int n = 0, row = 0; row < reduced_height; ++row) {
+        for (int col = 0; col < reduced_width; ++col, ++n) {
+          scores[row / reduced_row_height * cols_ + col / reduced_col_width] +=
+              score_sum[n];
         }
       }
-      delete [] score_sum;
+      delete[] score_sum;
     }
   }
-  
-  #if DO_TIMING_PROFILE
-    tic_toc.push_back (getTime ());
-    display_tic_toc (tic_toc, "range_likelihood");
-  #endif    
+
+#if DO_TIMING_PROFILE
+  tic_toc.push_back(getTime());
+  display_tic_toc(tic_toc, "range_likelihood");
+#endif
 }
 
 // Computes the likelihood scores using a shader
 void
-pcl::simulation::RangeLikelihood::computeScoresShader (float* reference)
+pcl::simulation::RangeLikelihood::computeScoresShader(float* reference)
 {
-  if (gllib::getGLError () != GL_NO_ERROR)
-  {
-    std::cout << "GL error: RangeLikelihood::compute_scores_shader - enter" << std::endl;
+  if (gllib::getGLError() != GL_NO_ERROR) {
+    std::cout << "GL error: RangeLikelihood::compute_scores_shader - enter"
+              << std::endl;
   }
 
 #ifdef SIMULATION_DEBUG
-  std::cout << "DepthSampler location: " << likelihood_program_->getUniformLocation ("DepthSampler") << std::endl;
-  std::cout << "ReferenceSampler location: " << likelihood_program_->getUniformLocation ("ReferenceSampler") << std::endl;
-  std::cout << "CostSampler location: " << likelihood_program_->getUniformLocation ("CostSampler") << std::endl;
+  std::cout << "DepthSampler location: "
+            << likelihood_program_->getUniformLocation("DepthSampler") << std::endl;
+  std::cout << "ReferenceSampler location: "
+            << likelihood_program_->getUniformLocation("ReferenceSampler") << std::endl;
+  std::cout << "CostSampler location: "
+            << likelihood_program_->getUniformLocation("CostSampler") << std::endl;
 
   int depth_tex_id;
   int ref_tex_id;
   int cost_tex_id;
 
-  glGetUniformiv(likelihood_program_->programId (), likelihood_program_->getUniformLocation ("DepthSampler"), &depth_tex_id);
-  glGetUniformiv(likelihood_program_->programId (), likelihood_program_->getUniformLocation ("ReferenceSampler"), &ref_tex_id);
-  glGetUniformiv(likelihood_program_->programId (), likelihood_program_->getUniformLocation ("CostSampler"), &cost_tex_id);
+  glGetUniformiv(likelihood_program_->programId(),
+                 likelihood_program_->getUniformLocation("DepthSampler"),
+                 &depth_tex_id);
+  glGetUniformiv(likelihood_program_->programId(),
+                 likelihood_program_->getUniformLocation("ReferenceSampler"),
+                 &ref_tex_id);
+  glGetUniformiv(likelihood_program_->programId(),
+                 likelihood_program_->getUniformLocation("CostSampler"),
+                 &cost_tex_id);
   std::cout << "depth id: " << depth_tex_id << " " << GL_TEXTURE0 << std::endl;
-  std::cout << "ref id: " << ref_tex_id  << " " << GL_TEXTURE1 << std::endl;
+  std::cout << "ref id: " << ref_tex_id << " " << GL_TEXTURE1 << std::endl;
   std::cout << "cost id: " << cost_tex_id << " " << GL_TEXTURE2 << std::endl;
 #endif
 
-  likelihood_program_->use ();
-  likelihood_program_->setUniform ("DepthSampler", 0);
-  likelihood_program_->setUniform ("ReferenceSampler", 1);
-  likelihood_program_->setUniform ("CostSampler", 2);
-  likelihood_program_->setUniform ("cols", cols_);
-  likelihood_program_->setUniform ("rows", rows_);
-  likelihood_program_->setUniform ("near", z_near_);
-  likelihood_program_->setUniform ("far", z_far_);
+  likelihood_program_->use();
+  likelihood_program_->setUniform("DepthSampler", 0);
+  likelihood_program_->setUniform("ReferenceSampler", 1);
+  likelihood_program_->setUniform("CostSampler", 2);
+  likelihood_program_->setUniform("cols", cols_);
+  likelihood_program_->setUniform("rows", rows_);
+  likelihood_program_->setUniform("near", z_near_);
+  likelihood_program_->setUniform("far", z_far_);
 
-  glBindFramebuffer (GL_FRAMEBUFFER, score_fbo_);
-  glDrawBuffer (GL_COLOR_ATTACHMENT0);
+  glBindFramebuffer(GL_FRAMEBUFFER, score_fbo_);
+  glDrawBuffer(GL_COLOR_ATTACHMENT0);
 
-  glReadBuffer (GL_NONE);
+  glReadBuffer(GL_NONE);
   GLboolean enable_depth_test;
-  glGetBooleanv (GL_DEPTH_TEST, &enable_depth_test);
-  glDisable (GL_DEPTH_TEST);
-  glViewport (0, 0, width_, height_);
+  glGetBooleanv(GL_DEPTH_TEST, &enable_depth_test);
+  glDisable(GL_DEPTH_TEST);
+  glViewport(0, 0, width_, height_);
 
   // Setup textures
-  glActiveTexture (GL_TEXTURE0);
-  glBindTexture (GL_TEXTURE_2D, depth_texture_);
+  glActiveTexture(GL_TEXTURE0);
+  glBindTexture(GL_TEXTURE_2D, depth_texture_);
 
-  glActiveTexture (GL_TEXTURE1);
-  glBindTexture (GL_TEXTURE_2D, sensor_texture_);
-  glTexImage2D (GL_TEXTURE_2D, 0, GL_R32F, col_width_, row_height_, 0, GL_RED, GL_FLOAT, reference);
+  glActiveTexture(GL_TEXTURE1);
+  glBindTexture(GL_TEXTURE_2D, sensor_texture_);
+  glTexImage2D(GL_TEXTURE_2D,
+               0,
+               GL_R32F,
+               col_width_,
+               row_height_,
+               0,
+               GL_RED,
+               GL_FLOAT,
+               reference);
 
-  glActiveTexture (GL_TEXTURE2);
-  glBindTexture (GL_TEXTURE_2D, likelihood_texture_);
+  glActiveTexture(GL_TEXTURE2);
+  glBindTexture(GL_TEXTURE_2D, likelihood_texture_);
 
-  quad_.render ();
-  glUseProgram (0);
+  quad_.render();
+  glUseProgram(0);
 
-  glBindFramebuffer (GL_FRAMEBUFFER, 0);
+  glBindFramebuffer(GL_FRAMEBUFFER, 0);
 
   // Unbind all textures that were used
-  glActiveTexture (GL_TEXTURE0);
-  glBindTexture (GL_TEXTURE_2D, 0);
-  glActiveTexture (GL_TEXTURE1);
-  glBindTexture (GL_TEXTURE_2D, 0);
-  glActiveTexture (GL_TEXTURE2);
-  glBindTexture (GL_TEXTURE_2D, 0);
-
-  if (gllib::getGLError () != GL_NO_ERROR)
-  {
+  glActiveTexture(GL_TEXTURE0);
+  glBindTexture(GL_TEXTURE_2D, 0);
+  glActiveTexture(GL_TEXTURE1);
+  glBindTexture(GL_TEXTURE_2D, 0);
+  glActiveTexture(GL_TEXTURE2);
+  glBindTexture(GL_TEXTURE_2D, 0);
+
+  if (gllib::getGLError() != GL_NO_ERROR) {
     std::cout << "GL error: RangeLikelihood::compute_scores_shader - exit" << std::endl;
   }
 
-  if (enable_depth_test == GL_TRUE) glEnable (GL_DEPTH_TEST);
-
+  if (enable_depth_test == GL_TRUE)
+    glEnable(GL_DEPTH_TEST);
 }
 
 void
-RangeLikelihood::render (const std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > & poses)
+RangeLikelihood::render(
+    const std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>&
+        poses)
 {
-  if (gllib::getGLError () != GL_NO_ERROR)
-  {
+  if (gllib::getGLError() != GL_NO_ERROR) {
     std::cerr << "GL Error: RangeLikelihood::render - enter" << std::endl;
   }
 
   GLint old_matrix_mode;
   GLint old_draw_buffer;
   GLint old_read_buffer;
-  glGetIntegerv (GL_DRAW_BUFFER, &old_draw_buffer);
-  glGetIntegerv (GL_READ_BUFFER, &old_read_buffer);
-  glGetIntegerv (GL_MATRIX_MODE, &old_matrix_mode);
+  glGetIntegerv(GL_DRAW_BUFFER, &old_draw_buffer);
+  glGetIntegerv(GL_READ_BUFFER, &old_read_buffer);
+  glGetIntegerv(GL_MATRIX_MODE, &old_matrix_mode);
 
-  glMatrixMode (GL_PROJECTION);
-  glPushMatrix ();
+  glMatrixMode(GL_PROJECTION);
+  glPushMatrix();
 
-  glMatrixMode (GL_MODELVIEW);
-  glPushMatrix ();
+  glMatrixMode(GL_MODELVIEW);
+  glPushMatrix();
 
-  glBindFramebuffer (GL_FRAMEBUFFER, fbo_);
+  glBindFramebuffer(GL_FRAMEBUFFER, fbo_);
 
-  if (use_color_)
-  {
-    glDrawBuffer (GL_COLOR_ATTACHMENT0);
+  if (use_color_) {
+    glDrawBuffer(GL_COLOR_ATTACHMENT0);
   }
-  else
-  {
-    glDrawBuffer (GL_NONE);
+  else {
+    glDrawBuffer(GL_NONE);
   }
-  glReadBuffer (GL_NONE);
+  glReadBuffer(GL_NONE);
 
   GLenum status;
-  status = glCheckFramebufferStatus (GL_FRAMEBUFFER);
-  switch (status)
-  {
-    case GL_FRAMEBUFFER_COMPLETE:
-    {
-      break;
-    }
-    default:
-    {
-      std::cout << "RangeLikelihood::render: Framebuffer failed" << std::endl;
-      exit (-1);
-    }
+  status = glCheckFramebufferStatus(GL_FRAMEBUFFER);
+  switch (status) {
+  case GL_FRAMEBUFFER_COMPLETE: {
+    break;
+  }
+  default: {
+    std::cout << "RangeLikelihood::render: Framebuffer failed" << std::endl;
+    exit(-1);
+  }
   }
 
   // Render
-  glPushAttrib (GL_ALL_ATTRIB_BITS);
-  glEnable (GL_COLOR_MATERIAL);
-  glClearColor (0.0f, 0.0f, 0.0f, 0.0f);
-  glClearDepth (1.0);
-  glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+  glPushAttrib(GL_ALL_ATTRIB_BITS);
+  glEnable(GL_COLOR_MATERIAL);
+  glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
+  glClearDepth(1.0);
+  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 
   // Setup projection matrix
-  setupProjectionMatrix ();
+  setupProjectionMatrix();
+
+  glEnable(GL_DEPTH_TEST);
+  glDepthMask(GL_TRUE);
+  glCullFace(GL_FRONT);
+  drawParticles(poses);
 
-  glEnable (GL_DEPTH_TEST);
-  glDepthMask (GL_TRUE);
-  glCullFace (GL_FRONT);
-  drawParticles (poses);
+  glPopAttrib();
+  glFlush();
 
-  glPopAttrib ();
-  glFlush ();
+  glBindFramebuffer(GL_FRAMEBUFFER, 0);
 
-  glBindFramebuffer (GL_FRAMEBUFFER, 0);
-  
   // Restore OpenGL state
-  glReadBuffer (old_read_buffer);
-  glDrawBuffer (old_draw_buffer);
+  glReadBuffer(old_read_buffer);
+  glDrawBuffer(old_draw_buffer);
 
-  glMatrixMode (GL_MODELVIEW);
-  glPopMatrix ();
+  glMatrixMode(GL_MODELVIEW);
+  glPopMatrix();
 
-  glMatrixMode (GL_PROJECTION);
-  glPopMatrix ();
+  glMatrixMode(GL_PROJECTION);
+  glPopMatrix();
 
-  glMatrixMode (old_matrix_mode);
+  glMatrixMode(old_matrix_mode);
 
-  if (gllib::getGLError () != GL_NO_ERROR)
-  {
+  if (gllib::getGLError() != GL_NO_ERROR) {
     std::cerr << "GL Error: RangeLikelihood::render - exit" << std::endl;
   }
 
@@ -1055,17 +1223,15 @@ RangeLikelihood::render (const std::vector<Eigen::Isometry3d, Eigen::aligned_all
 }
 
 const float*
-RangeLikelihood::getDepthBuffer () const
+RangeLikelihood::getDepthBuffer() const
 {
-  if (depth_buffer_dirty_)
-  {
+  if (depth_buffer_dirty_) {
     // Read depth
-    glBindFramebuffer (GL_FRAMEBUFFER, fbo_);
-    glReadPixels (0, 0, width_, height_, GL_DEPTH_COMPONENT, GL_FLOAT, depth_buffer_);
-    glBindFramebuffer (GL_FRAMEBUFFER, 0);
+    glBindFramebuffer(GL_FRAMEBUFFER, fbo_);
+    glReadPixels(0, 0, width_, height_, GL_DEPTH_COMPONENT, GL_FLOAT, depth_buffer_);
+    glBindFramebuffer(GL_FRAMEBUFFER, 0);
 
-    if (gllib::getGLError () != GL_NO_ERROR)
-    {
+    if (gllib::getGLError() != GL_NO_ERROR) {
       std::cerr << "GL Error: RangeLikelihoodGLSL::getDepthBuffer" << std::endl;
     }
 
@@ -1075,28 +1241,26 @@ RangeLikelihood::getDepthBuffer () const
 }
 
 const uint8_t*
-RangeLikelihood::getColorBuffer () const
+RangeLikelihood::getColorBuffer() const
 {
   // It's only possible to read the color buffer if it
   // was rendered in the first place.
-  assert (use_color_);
+  assert(use_color_);
 
-  if (color_buffer_dirty_)
-  {
+  if (color_buffer_dirty_) {
     std::cout << "Read color buffer" << std::endl;
 
     // Read Color
     GLint old_read_buffer;
-    glGetIntegerv (GL_READ_BUFFER, &old_read_buffer);
+    glGetIntegerv(GL_READ_BUFFER, &old_read_buffer);
 
-    glBindFramebuffer (GL_FRAMEBUFFER, fbo_);
-    glReadBuffer (GL_COLOR_ATTACHMENT0);
-    glReadPixels (0, 0, width_, height_, GL_RGB, GL_UNSIGNED_BYTE, color_buffer_);
-    glBindFramebuffer (GL_FRAMEBUFFER, 0);
-    glReadBuffer (old_read_buffer);
+    glBindFramebuffer(GL_FRAMEBUFFER, fbo_);
+    glReadBuffer(GL_COLOR_ATTACHMENT0);
+    glReadPixels(0, 0, width_, height_, GL_RGB, GL_UNSIGNED_BYTE, color_buffer_);
+    glBindFramebuffer(GL_FRAMEBUFFER, 0);
+    glReadBuffer(old_read_buffer);
 
-    if (gllib::getGLError () != GL_NO_ERROR)
-    {
+    if (gllib::getGLError() != GL_NO_ERROR) {
       std::cerr << "GL Error: RangeLikelihood::getColorBuffer" << std::endl;
     }
 
@@ -1107,16 +1271,14 @@ RangeLikelihood::getColorBuffer () const
 
 // The scores are in score_texture_
 const float*
-RangeLikelihood::getScoreBuffer () const
+RangeLikelihood::getScoreBuffer() const
 {
-  if (score_buffer_dirty_ && !compute_likelihood_on_cpu_)
-  {
-    glActiveTexture (GL_TEXTURE0);
-    glBindTexture (GL_TEXTURE_2D, score_texture_);
-    glGetTexImage (GL_TEXTURE_2D, 0, GL_RED, GL_FLOAT, score_buffer_);
-    glBindTexture (GL_TEXTURE_2D, 0);
-    if (gllib::getGLError () != GL_NO_ERROR)
-    {
+  if (score_buffer_dirty_ && !compute_likelihood_on_cpu_) {
+    glActiveTexture(GL_TEXTURE0);
+    glBindTexture(GL_TEXTURE_2D, score_texture_);
+    glGetTexImage(GL_TEXTURE_2D, 0, GL_RED, GL_FLOAT, score_buffer_);
+    glBindTexture(GL_TEXTURE_2D, 0);
+    if (gllib::getGLError() != GL_NO_ERROR) {
       std::cerr << "GL Error: RangeLikelihood::getScoreBuffer" << std::endl;
     }
     score_buffer_dirty_ = false;
diff --git a/simulation/src/scene.cpp b/simulation/src/scene.cpp
index 56a2501e1ff..59961709128 100644
--- a/simulation/src/scene.cpp
+++ b/simulation/src/scene.cpp
@@ -7,36 +7,33 @@
 
 #include <pcl/simulation/scene.h>
 
-namespace pcl
-{
-
-namespace simulation
-{
+namespace pcl {
+namespace simulation {
 
 void
-Scene::add (Model::Ptr model)
+Scene::add(Model::Ptr model)
 {
   models_.push_back(model);
 }
 
 void
-Scene::addCompleteModel (std::vector<Model::Ptr> model)
+Scene::addCompleteModel(std::vector<Model::Ptr> model)
 {
-  models_.push_back (model[0]);
+  models_.push_back(model[0]);
 }
 
 void
-Scene::draw ()
+Scene::draw()
 {
-  for (auto &model : models_)
-    model->draw ();
+  for (auto& model : models_)
+    model->draw();
 }
 
 void
-Scene::clear ()
+Scene::clear()
 {
   models_.clear();
 }
 
-} // namespace - simulation
-} // namespace - pcl
+} // namespace simulation
+} // namespace pcl
diff --git a/simulation/src/sum_reduce.cpp b/simulation/src/sum_reduce.cpp
index 54da1273de7..20105624cbe 100644
--- a/simulation/src/sum_reduce.cpp
+++ b/simulation/src/sum_reduce.cpp
@@ -2,132 +2,131 @@
 
 using namespace pcl::simulation;
 
-pcl::simulation::SumReduce::SumReduce (int width, int height, int levels) : levels_ (levels),
-                                                                            width_ (width),
-                                                                            height_ (height)
+pcl::simulation::SumReduce::SumReduce(int width, int height, int levels)
+: levels_(levels), width_(width), height_(height)
 {
   std::cout << "SumReduce: levels: " << levels_ << std::endl;
 
   // Load shader
-  sum_program_ = gllib::Program::Ptr (new gllib::Program ());
+  sum_program_ = gllib::Program::Ptr(new gllib::Program());
   // TODO: to remove file dependency include the shader source in the binary
-  if (!sum_program_->addShaderFile ("sum_score.vert", gllib::VERTEX))
-  {
+  if (!sum_program_->addShaderFile("sum_score.vert", gllib::VERTEX)) {
     std::cout << "Failed loading vertex shader" << std::endl;
-    exit (-1);
+    exit(-1);
   }
 
   // TODO: to remove file dependency include the shader source in the binary
-  if (!sum_program_->addShaderFile ("sum_score.frag", gllib::FRAGMENT))
-  {
+  if (!sum_program_->addShaderFile("sum_score.frag", gllib::FRAGMENT)) {
     std::cout << "Failed loading fragment shader" << std::endl;
-    exit (-1);
+    exit(-1);
   }
 
-  sum_program_->link ();
+  sum_program_->link();
 
   // Setup the framebuffer object for rendering
-  glGenFramebuffers (1, &fbo_);
+  glGenFramebuffers(1, &fbo_);
   arrays_ = new GLuint[levels_];
 
-  glGenTextures (levels_, arrays_);
+  glGenTextures(levels_, arrays_);
 
   int level_width = width_;
   int level_height = height_;
 
-  for (int i = 0; i < levels_; ++i)
-  {
+  for (int i = 0; i < levels_; ++i) {
     level_width /= 2;
     level_height /= 2;
 
-    glBindTexture (GL_TEXTURE_2D, arrays_[i]);
-    glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
-    glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
-    glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
-    glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
-    glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
-    glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
-    glTexImage2D (GL_TEXTURE_2D, 0, GL_R32F, level_width, level_height, 0, GL_RED, GL_FLOAT, nullptr);
-    glBindTexture (GL_TEXTURE_2D, 0);
+    glBindTexture(GL_TEXTURE_2D, arrays_[i]);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
+    glTexImage2D(GL_TEXTURE_2D,
+                 0,
+                 GL_R32F,
+                 level_width,
+                 level_height,
+                 0,
+                 GL_RED,
+                 GL_FLOAT,
+                 nullptr);
+    glBindTexture(GL_TEXTURE_2D, 0);
   }
 }
 
-pcl::simulation::SumReduce::~SumReduce ()
+pcl::simulation::SumReduce::~SumReduce()
 {
-  glDeleteTextures (levels_, arrays_);
-  glDeleteFramebuffers (1, &fbo_);
+  glDeleteTextures(levels_, arrays_);
+  glDeleteFramebuffers(1, &fbo_);
 }
 
 void
-pcl::simulation::SumReduce::sum (GLuint input_array, float* output_array)
+pcl::simulation::SumReduce::sum(GLuint input_array, float* output_array)
 {
-  if (gllib::getGLError () != GL_NO_ERROR)
-  {
+  if (gllib::getGLError() != GL_NO_ERROR) {
     std::cout << "SumReduce::sum enter" << std::endl;
   }
 
-  glDisable (GL_DEPTH_TEST);
+  glDisable(GL_DEPTH_TEST);
 
-  glBindFramebuffer (GL_FRAMEBUFFER, fbo_);
+  glBindFramebuffer(GL_FRAMEBUFFER, fbo_);
   int width = width_;
   int height = height_;
 
-  glActiveTexture (GL_TEXTURE0);
-  glBindTexture (GL_TEXTURE_2D, input_array);
+  glActiveTexture(GL_TEXTURE0);
+  glBindTexture(GL_TEXTURE_2D, input_array);
 
   // use program
-  sum_program_->use ();
-  glUniform1i (sum_program_->getUniformLocation ("ArraySampler"), 0);
+  sum_program_->use();
+  glUniform1i(sum_program_->getUniformLocation("ArraySampler"), 0);
 
-  if (gllib::getGLError () != GL_NO_ERROR)
-  {
+  if (gllib::getGLError() != GL_NO_ERROR) {
     std::cout << "SumReduce::sum  set sampler" << std::endl;
   }
 
-  for (int i=0; i < levels_; ++i)
-  {
-    glFramebufferTexture2D (GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, arrays_[i], 0);
-    glDrawBuffer (GL_COLOR_ATTACHMENT0);
+  for (int i = 0; i < levels_; ++i) {
+    glFramebufferTexture2D(
+        GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, arrays_[i], 0);
+    glDrawBuffer(GL_COLOR_ATTACHMENT0);
 
-    glViewport (0, 0, width/2, height/2);
+    glViewport(0, 0, width / 2, height / 2);
 
-    float step_x = 1.0f / float (width);
-    float step_y = 1.0f / float (height);
-    sum_program_->setUniform ("step_x", step_x);
-    sum_program_->setUniform ("step_y", step_y);
-    //float step_x = 1.0f / static_cast<float> (width);
-    //float step_y = 1.0f / static_cast<float> (height);
+    float step_x = 1.0f / float(width);
+    float step_y = 1.0f / float(height);
+    sum_program_->setUniform("step_x", step_x);
+    sum_program_->setUniform("step_y", step_y);
+    // float step_x = 1.0f / static_cast<float> (width);
+    // float step_y = 1.0f / static_cast<float> (height);
 
-    quad_.render ();
+    quad_.render();
 
-    if (gllib::getGLError () != GL_NO_ERROR)
-    {
+    if (gllib::getGLError() != GL_NO_ERROR) {
       std::cout << "SumReduce::sum  render" << std::endl;
     }
 
     width /= 2;
     height /= 2;
 
-    glActiveTexture (GL_TEXTURE0);
-    glBindTexture (GL_TEXTURE_2D, arrays_[i]);
+    glActiveTexture(GL_TEXTURE0);
+    glBindTexture(GL_TEXTURE_2D, arrays_[i]);
   }
 
-  glBindFramebuffer (GL_FRAMEBUFFER, 0);
+  glBindFramebuffer(GL_FRAMEBUFFER, 0);
 
-  glActiveTexture (GL_TEXTURE0);
-  glBindTexture (GL_TEXTURE_2D, 0);
+  glActiveTexture(GL_TEXTURE0);
+  glBindTexture(GL_TEXTURE_2D, 0);
 
-  glUseProgram (0);
+  glUseProgram(0);
 
   // Final results is in arrays_[levels_-1]
-  glActiveTexture (GL_TEXTURE0);
-  glBindTexture (GL_TEXTURE_2D, arrays_[levels_-1]);
-  glGetTexImage (GL_TEXTURE_2D, 0, GL_RED, GL_FLOAT, output_array);
-  glBindTexture (GL_TEXTURE_2D, 0);
+  glActiveTexture(GL_TEXTURE0);
+  glBindTexture(GL_TEXTURE_2D, arrays_[levels_ - 1]);
+  glGetTexImage(GL_TEXTURE_2D, 0, GL_RED, GL_FLOAT, output_array);
+  glBindTexture(GL_TEXTURE_2D, 0);
 
-  if (gllib::getGLError () != GL_NO_ERROR)
-  {
+  if (gllib::getGLError() != GL_NO_ERROR) {
     std::cout << "Error: SumReduce exit" << std::endl;
   }
 }
-
diff --git a/simulation/tools/sim_terminal_demo.cpp b/simulation/tools/sim_terminal_demo.cpp
index 128c25df425..3d63879268d 100644
--- a/simulation/tools/sim_terminal_demo.cpp
+++ b/simulation/tools/sim_terminal_demo.cpp
@@ -2,21 +2,21 @@
  * Demo program for simulation library
  * A virtual camera generates simulated point clouds
  * No visual output, point clouds saved to file
- * 
+ *
  * three different demo modes:
  * 0 - static camera, 100 poses
  * 1 - circular camera flying around the scene, 16 poses
  * 2 - camera translates between 2 poses using slerp, 20 poses
- * pcl_sim_terminal_demo 2 ../../../../kmcl/models/table_models/meta_model.ply  
+ * pcl_sim_terminal_demo 2 ../../../../kmcl/models/table_models/meta_model.ply
  */
 
 #include <Eigen/Dense>
+#include <boost/shared_ptr.hpp>
 #include <cmath>
 #include <iostream>
-#include <boost/shared_ptr.hpp>
 #ifdef _WIN32
-# define WIN32_LEAN_AND_MEAN
-# include <windows.h>
+#define WIN32_LEAN_AND_MEAN
+#include <windows.h>
 #endif
 
 #include "simulation_io.hpp"
@@ -30,59 +30,61 @@ using namespace std;
 
 SimExample::Ptr simexample;
 
-void printHelp (int, char **argv)
+void
+printHelp(int, char** argv)
 {
-  print_error ("Syntax is: %s <mode 1,2 or 3> <filename>\n", argv[0]);
-  print_info ("acceptable filenames include vtk, obj and ply. ply can support colour\n");
+  print_error("Syntax is: %s <mode 1,2 or 3> <filename>\n", argv[0]);
+  print_info("acceptable filenames include vtk, obj and ply. ply can support colour\n");
 }
 
-
 // Output the simulated output to file:
-void write_sim_output(const string &fname_root){ 
-  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out (new pcl::PointCloud<pcl::PointXYZRGB>);
+void
+write_sim_output(const string& fname_root)
+{
+  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out(new pcl::PointCloud<pcl::PointXYZRGB>);
 
   // Read Color Buffer from the GPU before creating PointCloud:
   // By default the buffers are not read back from the GPU
-  simexample->rl_->getColorBuffer ();
-  simexample->rl_->getDepthBuffer ();  
-  // Add noise directly to the CPU depth buffer 
-  simexample->rl_->addNoise ();
+  simexample->rl_->getColorBuffer();
+  simexample->rl_->getDepthBuffer();
+  // Add noise directly to the CPU depth buffer
+  simexample->rl_->addNoise();
 
   // Optional argument to save point cloud in global frame:
   // Save camera relative:
-  //simexample->rl_->getPointCloud(pc_out);
+  // simexample->rl_->getPointCloud(pc_out);
   // Save in global frame - applying the camera frame:
-  //simexample->rl_->getPointCloud(pc_out,true,simexample->camera_->getPose());
+  // simexample->rl_->getPointCloud(pc_out,true,simexample->camera_->getPose());
   // Save in local frame
-  simexample->rl_->getPointCloud (pc_out,false,simexample->camera_->getPose ());
+  simexample->rl_->getPointCloud(pc_out, false, simexample->camera_->getPose());
   // TODO: what to do when there are more than one simulated view?
 
-  if (!pc_out->points.empty ()){
+  if (!pc_out->points.empty()) {
     std::cout << pc_out->points.size() << " points written to file\n";
 
     pcl::PCDWriter writer;
-    //writer.write ( string (fname_root + ".pcd"), *pc_out,	false);  /// ASCII
-    writer.writeBinary (  string (fname_root + ".pcd")  , *pc_out);
-    //std::cout << "finished writing file\n";
-  }else{
+    // writer.write ( string (fname_root + ".pcd"), *pc_out,	false);  /// ASCII
+    writer.writeBinary(string(fname_root + ".pcd"), *pc_out);
+    // std::cout << "finished writing file\n";
+  }
+  else {
     std::cout << pc_out->points.size() << " points in cloud, not written\n";
   }
 
-  //simexample->write_score_image (simexample->rl_->getScoreBuffer (), 
-  //                               string (fname_root + "_score.png") );  
-  simexample->write_rgb_image (simexample->rl_->getColorBuffer (), 
-                               string (fname_root + "_rgb.png") );  
-  simexample->write_depth_image (simexample->rl_->getDepthBuffer (),
-                                 string (fname_root + "_depth.png") );  
-  //simexample->write_depth_image_uint (simexample->rl_->getDepthBuffer (),
+  // simexample->write_score_image (simexample->rl_->getScoreBuffer (),
+  //                               string (fname_root + "_score.png") );
+  simexample->write_rgb_image(simexample->rl_->getColorBuffer(),
+                              string(fname_root + "_rgb.png"));
+  simexample->write_depth_image(simexample->rl_->getDepthBuffer(),
+                                string(fname_root + "_depth.png"));
+  // simexample->write_depth_image_uint (simexample->rl_->getDepthBuffer (),
   //                                    string (fname_root + "_depth_uint.png") );
 
   // Demo interacton with RangeImage:
   pcl::RangeImagePlanar rangeImage;
-  simexample->rl_->getRangeImagePlanar (rangeImage);
+  simexample->rl_->getRangeImagePlanar(rangeImage);
 }
 
-
 // A 'halo' camera - a circular ring of poses all pointing at a center point
 // @param: focus_center: the center points
 // @param: halo_r: radius of the ring
@@ -90,103 +92,111 @@ void write_sim_output(const string &fname_root){
 // @param: n_poses: number of generated poses
 void
 generate_halo(
-  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > &poses,
-  Eigen::Vector3d focus_center,double halo_r,double halo_dz,int n_poses){
-  
-  for (double t=0;t < (2*M_PI);t =t + (2*M_PI)/ ((double) n_poses) ){
-    double x = halo_r*std::cos(t);
-    double y = halo_r*sin(t);
+    std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>& poses,
+    Eigen::Vector3d focus_center,
+    double halo_r,
+    double halo_dz,
+    int n_poses)
+{
+
+  for (double t = 0; t < (2 * M_PI); t = t + (2 * M_PI) / ((double)n_poses)) {
+    double x = halo_r * std::cos(t);
+    double y = halo_r * sin(t);
     double z = halo_dz;
-    double pitch =std::atan2( halo_dz,halo_r); 
-    double yaw = std::atan2(-y,-x);
-   
+    double pitch = std::atan2(halo_dz, halo_r);
+    double yaw = std::atan2(-y, -x);
+
     Eigen::Isometry3d pose;
     pose.setIdentity();
     Eigen::Matrix3d m;
-    m = AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
-    * AngleAxisd(pitch, Eigen::Vector3d::UnitY())
-    * AngleAxisd(0, Eigen::Vector3d::UnitZ());    
+    m = AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *
+        AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
+        AngleAxisd(0, Eigen::Vector3d::UnitZ());
 
-    pose *=m;
-    Vector3d v(x,y,z);
+    pose *= m;
+    Vector3d v(x, y, z);
     v += focus_center;
     pose.translation() = v;
-    poses.push_back(pose);  
+    poses.push_back(pose);
   }
-  return ;
+  return;
 }
 
 int
-main (int argc, char** argv)
+main(int argc, char** argv)
 {
   // 1. Parse arguments:
-  print_info ("Manually generate a simulated RGB-D point cloud using pcl::simulation. For more information, use: %s -h\n", argv[0]);
-  if (argc < 3)
-  {
-    printHelp (argc, argv);
+  print_info("Manually generate a simulated RGB-D point cloud using pcl::simulation. "
+             "For more information, use: %s -h\n",
+             argv[0]);
+  if (argc < 3) {
+    printHelp(argc, argv);
     return (-1);
-  }  
-  int mode=atoi(argv[1]);
-  
+  }
+  int mode = atoi(argv[1]);
+
   // 2 Construct the simulation method:
   int width = 640;
   int height = 480;
-  simexample = SimExample::Ptr (new SimExample (argc, argv, height,width ));
-  
+  simexample = SimExample::Ptr(new SimExample(argc, argv, height, width));
+
   // 3 Generate a series of simulation poses:
-  // -0 100 fixed poses 
-  // -1 a 'halo' camera 
+  // -0 100 fixed poses
+  // -1 a 'halo' camera
   // -2 slerp between two different poses
-  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
-  if (mode==0){
+  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;
+  if (mode == 0) {
     // Create a pose:
     Eigen::Isometry3d pose;
     pose.setIdentity();
     Matrix3d m;
-    //ypr:
-    m = AngleAxisd(-9.14989, Vector3d::UnitZ())     * AngleAxisd(0.20944, Vector3d::UnitY())    * AngleAxisd(0, Vector3d::UnitX());  
+    // ypr:
+    m = AngleAxisd(-9.14989, Vector3d::UnitZ()) *
+        AngleAxisd(0.20944, Vector3d::UnitY()) * AngleAxisd(0, Vector3d::UnitX());
     pose *= m;
     Vector3d v;
     v << 1.31762, 0.382931, 1.89533;
-    pose.translation() = v;  
-    for (int i=0;i< 100;i++){ // duplicate the pose 100 times
-      poses.push_back(pose); 
+    pose.translation() = v;
+    for (int i = 0; i < 100; i++) { // duplicate the pose 100 times
+      poses.push_back(pose);
     }
-  }else if(mode==1){
-    Eigen::Vector3d focus_center(0,0,1.3);
+  }
+  else if (mode == 1) {
+    Eigen::Vector3d focus_center(0, 0, 1.3);
     double halo_r = 4;
     double halo_dz = 2;
-    int n_poses=16;
-    generate_halo(poses,focus_center,halo_r,halo_dz,n_poses);
-  }else if (mode==2){
+    int n_poses = 16;
+    generate_halo(poses, focus_center, halo_r, halo_dz, n_poses);
+  }
+  else if (mode == 2) {
     Eigen::Isometry3d pose1;
     pose1.setIdentity();
-    pose1.translation() << 1,0.75,2;
+    pose1.translation() << 1, 0.75, 2;
     Eigen::Matrix3d rot1;
-    rot1 = AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())
-    * AngleAxisd(M_PI/10, Eigen::Vector3d::UnitY())
-    * AngleAxisd(0.0, Eigen::Vector3d::UnitZ()); // ypr
-    pose1.rotate(rot1);  
-  
+    rot1 = AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()) *
+           AngleAxisd(M_PI / 10, Eigen::Vector3d::UnitY()) *
+           AngleAxisd(0.0, Eigen::Vector3d::UnitZ()); // ypr
+    pose1.rotate(rot1);
+
     Eigen::Isometry3d pose2;
     pose2.setIdentity();
-    pose2.translation() << 1,-1,3;
+    pose2.translation() << 1, -1, 3;
     Eigen::Matrix3d rot2;
-    rot2 = AngleAxisd(3*M_PI/4, Eigen::Vector3d::UnitZ())
-    * AngleAxisd(M_PI/4, Eigen::Vector3d::UnitY())
-    * AngleAxisd(0.0, Eigen::Vector3d::UnitZ()); // ypr    
-    pose2.rotate(rot2);  
-  
+    rot2 = AngleAxisd(3 * M_PI / 4, Eigen::Vector3d::UnitZ()) *
+           AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitY()) *
+           AngleAxisd(0.0, Eigen::Vector3d::UnitZ()); // ypr
+    pose2.rotate(rot2);
+
     int n_poses = 20;
-    for (double i=0; i<=1;i+= 1/((double) n_poses -1) ){
-      Eigen::Quaterniond rot3; 
+    for (double i = 0; i <= 1; i += 1 / ((double)n_poses - 1)) {
+      Eigen::Quaterniond rot3;
       Eigen::Quaterniond r1(pose1.rotation());
       Eigen::Quaterniond r2(pose2.rotation());
-      rot3 = r1.slerp(i,r2);
+      rot3 = r1.slerp(i, r2);
       Eigen::Isometry3d pose;
       pose.setIdentity();
-      Eigen::Vector3d trans3 = (1-i)*pose1.translation() +  i*pose2.translation();
-      pose.translation() <<  trans3[0] ,trans3[1] ,trans3[2];
+      Eigen::Vector3d trans3 = (1 - i) * pose1.translation() + i * pose2.translation();
+      pose.translation() << trans3[0], trans3[1], trans3[2];
       pose.rotate(rot3);
       poses.push_back(pose);
     }
@@ -194,18 +204,19 @@ main (int argc, char** argv)
 
   // 4 Do the simulation and write the output:
   double tic_main = getTime();
-  for (size_t i=0;i < poses.size();i++){
+  for (size_t i = 0; i < poses.size(); i++) {
     std::stringstream ss;
     ss.precision(20);
-    ss << "simcloud_" << i;// << ".pcd";
+    ss << "simcloud_" << i; // << ".pcd";
     double tic = getTime();
     simexample->doSim(poses[i]);
-    
+
     write_sim_output(ss.str());
-    std::cout << (getTime() -tic) << " sec\n";
+    std::cout << (getTime() - tic) << " sec\n";
   }
-  std::cout << poses.size() << " poses simulated in " << (getTime() -tic_main) << "seconds\n";
-  std::cout << (poses.size()/ (getTime() -tic_main) ) << "Hz on average\n";
-  
+  std::cout << poses.size() << " poses simulated in " << (getTime() - tic_main)
+            << "seconds\n";
+  std::cout << (poses.size() / (getTime() - tic_main)) << "Hz on average\n";
+
   return 0;
 }
diff --git a/simulation/tools/sim_test_performance.cpp b/simulation/tools/sim_test_performance.cpp
index 39b26a6d83f..c5093f3a767 100644
--- a/simulation/tools/sim_test_performance.cpp
+++ b/simulation/tools/sim_test_performance.cpp
@@ -6,27 +6,27 @@
  */
 
 #include <Eigen/Dense>
+#include <boost/shared_ptr.hpp>
 #include <cmath>
 #include <iostream>
-#include <boost/shared_ptr.hpp>
 #ifdef _WIN32
-# define WIN32_LEAN_AND_MEAN
-# include <windows.h>
+#define WIN32_LEAN_AND_MEAN
+#include <windows.h>
 #endif
 #include <GL/glew.h>
 
 #include <pcl/pcl_config.h>
 #ifdef OPENGL_IS_A_FRAMEWORK
-# include <OpenGL/gl.h>
-# include <OpenGL/glu.h>
+#include <OpenGL/gl.h>
+#include <OpenGL/glu.h>
 #else
-# include <GL/gl.h>
-# include <GL/glu.h>
+#include <GL/gl.h>
+#include <GL/glu.h>
 #endif
 #ifdef GLUT_IS_A_FRAMEWORK
-# include <GLUT/glut.h>
+#include <GLUT/glut.h>
 #else
-# include <GL/glut.h>
+#include <GL/glut.h>
 #endif
 
 #include <pcl/io/pcd_io.h>
@@ -35,9 +35,9 @@
 #include <pcl/common/common.h>
 #include <pcl/common/transforms.h>
 
-#include <pcl/point_types.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/features/normal_3d.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
 #include <pcl/surface/gp3.h>
 
 // define the following in order to eliminate the deprecated headers warning
@@ -46,11 +46,11 @@
 
 #include <pcl/simulation/camera.h>
 #include <pcl/simulation/model.h>
-#include <pcl/simulation/scene.h>
 #include <pcl/simulation/range_likelihood.h>
+#include <pcl/simulation/scene.h>
 
-#include <pcl/console/print.h>
 #include <pcl/console/parse.h>
+#include <pcl/console/print.h>
 #include <pcl/console/time.h>
 
 using namespace Eigen;
@@ -77,201 +77,208 @@ int window_height_;
 TexturedQuad::Ptr textured_quad_;
 
 void
-printHelp (int, char **argv)
+printHelp(int, char** argv)
 {
-  print_error ("Syntax is: %s <filename>\n", argv[0]);
-  print_info ("acceptable filenames include vtk, obj and ply. ply can support color\n");
+  print_error("Syntax is: %s <filename>\n", argv[0]);
+  print_info("acceptable filenames include vtk, obj and ply. ply can support color\n");
 }
 
 void
-display_score_image (const float* score_buffer)
+display_score_image(const float* score_buffer)
 {
-  int npixels = range_likelihood_->getWidth () * range_likelihood_->getHeight ();
+  int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
   uint8_t* score_img = new uint8_t[npixels * 3];
 
   float min_score = score_buffer[0];
   float max_score = score_buffer[0];
-  for (int i=1; i<npixels; i++)
-  {
-    if (score_buffer[i] < min_score) min_score = score_buffer[i];
-    if (score_buffer[i] > max_score) max_score = score_buffer[i];
+  for (int i = 1; i < npixels; i++) {
+    if (score_buffer[i] < min_score)
+      min_score = score_buffer[i];
+    if (score_buffer[i] > max_score)
+      max_score = score_buffer[i];
   }
-  for (int i=0; i < npixels; i++)
-  {
-    float d = (score_buffer[i]-min_score)/(max_score-min_score);
-    score_img[3*i+0] = 0;
-    score_img[3*i+1] = static_cast<unsigned char> (d*255);
-    score_img[3*i+2] = 0;
+  for (int i = 0; i < npixels; i++) {
+    float d = (score_buffer[i] - min_score) / (max_score - min_score);
+    score_img[3 * i + 0] = 0;
+    score_img[3 * i + 1] = static_cast<unsigned char>(d * 255);
+    score_img[3 * i + 2] = 0;
   }
-  textured_quad_->setTexture (score_img);
-  textured_quad_->render ();
+  textured_quad_->setTexture(score_img);
+  textured_quad_->render();
 
-  delete [] score_img;
+  delete[] score_img;
 }
 
-void display_depth_image (const float* depth_buffer, int width, int height)
+void
+display_depth_image(const float* depth_buffer, int width, int height)
 {
   int npixels = width * height;
   uint8_t* depth_img = new uint8_t[npixels * 3];
 
   float min_depth = depth_buffer[0];
   float max_depth = depth_buffer[0];
-  for (int i = 1; i < npixels; ++i)
-  {
-    if (depth_buffer[i] < min_depth) min_depth = depth_buffer[i];
-    if (depth_buffer[i] > max_depth) max_depth = depth_buffer[i];
+  for (int i = 1; i < npixels; ++i) {
+    if (depth_buffer[i] < min_depth)
+      min_depth = depth_buffer[i];
+    if (depth_buffer[i] > max_depth)
+      max_depth = depth_buffer[i];
   }
 
-  for (int i = 0; i < npixels; ++i)
-  {
+  for (int i = 0; i < npixels; ++i) {
     float zn = 0.7f;
     float zf = 20.0f;
     float d = depth_buffer[i];
-    float z = -zf*zn/((zf-zn)*(d - zf/(zf-zn)));
+    float z = -zf * zn / ((zf - zn) * (d - zf / (zf - zn)));
     float b = 0.075f;
     float f = 580.0f;
-    int kd = static_cast<int>(1090 - b*f/z*8);
-    if (kd < 0) kd = 0;
-    else if (kd > 2047) kd = 2047;
+    int kd = static_cast<int>(1090 - b * f / z * 8);
+    if (kd < 0)
+      kd = 0;
+    else if (kd > 2047)
+      kd = 2047;
 
     int pval = t_gamma[kd];
-    uint8_t lb = static_cast<uint8_t> (pval & 0xff);
-    switch (pval >> 8)
-    {
-      case 0:
-	  depth_img[3*i+0] = 255;
-	  depth_img[3*i+1] = static_cast<uint8_t> (255-lb);
-	  depth_img[3*i+2] = static_cast<uint8_t> (255-lb);
-	  break;
-      case 1:
-	  depth_img[3*i+0] = 255;
-	  depth_img[3*i+1] = lb;
-	  depth_img[3*i+2] = 0;
-	  break;
-      case 2:
-	  depth_img[3*i+0] = static_cast<uint8_t> (255-lb);
-	  depth_img[3*i+1] = 255;
-	  depth_img[3*i+2] = 0;
-	  break;
-      case 3:
-	  depth_img[3*i+0] = 0;
-	  depth_img[3*i+1] = 255;
-	  depth_img[3*i+2] = lb;
-	  break;
-      case 4:
-	  depth_img[3*i+0] = 0;
-	  depth_img[3*i+1] = static_cast<uint8_t> (255-lb);
-	  depth_img[3*i+2] = 255;
-	  break;
-      case 5:
-	  depth_img[3*i+0] = 0;
-	  depth_img[3*i+1] = 0;
-	  depth_img[3*i+2] = static_cast<uint8_t> (255-lb);
-	  break;
-      default:
-	  depth_img[3*i+0] = 0;
-	  depth_img[3*i+1] = 0;
-	  depth_img[3*i+2] = 0;
-	  break;
+    uint8_t lb = static_cast<uint8_t>(pval & 0xff);
+    switch (pval >> 8) {
+    case 0:
+      depth_img[3 * i + 0] = 255;
+      depth_img[3 * i + 1] = static_cast<uint8_t>(255 - lb);
+      depth_img[3 * i + 2] = static_cast<uint8_t>(255 - lb);
+      break;
+    case 1:
+      depth_img[3 * i + 0] = 255;
+      depth_img[3 * i + 1] = lb;
+      depth_img[3 * i + 2] = 0;
+      break;
+    case 2:
+      depth_img[3 * i + 0] = static_cast<uint8_t>(255 - lb);
+      depth_img[3 * i + 1] = 255;
+      depth_img[3 * i + 2] = 0;
+      break;
+    case 3:
+      depth_img[3 * i + 0] = 0;
+      depth_img[3 * i + 1] = 255;
+      depth_img[3 * i + 2] = lb;
+      break;
+    case 4:
+      depth_img[3 * i + 0] = 0;
+      depth_img[3 * i + 1] = static_cast<uint8_t>(255 - lb);
+      depth_img[3 * i + 2] = 255;
+      break;
+    case 5:
+      depth_img[3 * i + 0] = 0;
+      depth_img[3 * i + 1] = 0;
+      depth_img[3 * i + 2] = static_cast<uint8_t>(255 - lb);
+      break;
+    default:
+      depth_img[3 * i + 0] = 0;
+      depth_img[3 * i + 1] = 0;
+      depth_img[3 * i + 2] = 0;
+      break;
     }
   }
 
-  glRasterPos2i (-1,-1);
-  glDrawPixels (width, height,
-                GL_RGB, GL_UNSIGNED_BYTE, depth_img);
+  glRasterPos2i(-1, -1);
+  glDrawPixels(width, height, GL_RGB, GL_UNSIGNED_BYTE, depth_img);
 
-  delete [] depth_img;
+  delete[] depth_img;
 }
 
 void
-display ()
+display()
 {
-  float* reference = new float[range_likelihood_->getRowHeight () * range_likelihood_->getColWidth ()];
-  const float* depth_buffer = range_likelihood_->getDepthBuffer ();
+  float* reference =
+      new float[range_likelihood_->getRowHeight() * range_likelihood_->getColWidth()];
+  const float* depth_buffer = range_likelihood_->getDepthBuffer();
   // Copy one image from our last as a reference.
-  for (int i = 0, n = 0; i < range_likelihood_->getRowHeight (); ++i)
-  {
-    for (int j = 0; j < range_likelihood_->getColWidth (); ++j)
-    {
-      reference[n++] = depth_buffer[ (i + range_likelihood_->getRowHeight () * range_likelihood_->getRows () / 2) * range_likelihood_->getWidth ()
-                                    + j + range_likelihood_->getColWidth () * range_likelihood_->getCols () / 2];
+  for (int i = 0, n = 0; i < range_likelihood_->getRowHeight(); ++i) {
+    for (int j = 0; j < range_likelihood_->getColWidth(); ++j) {
+      reference[n++] = depth_buffer
+          [(i + range_likelihood_->getRowHeight() * range_likelihood_->getRows() / 2) *
+               range_likelihood_->getWidth() +
+           j + range_likelihood_->getColWidth() * range_likelihood_->getCols() / 2];
     }
   }
 
-  float* reference_vis = new float[range_likelihood_visualization_->getRowHeight () * range_likelihood_visualization_->getColWidth ()];
-  const float* depth_buffer_vis = range_likelihood_visualization_->getDepthBuffer ();
+  float* reference_vis = new float[range_likelihood_visualization_->getRowHeight() *
+                                   range_likelihood_visualization_->getColWidth()];
+  const float* depth_buffer_vis = range_likelihood_visualization_->getDepthBuffer();
   // Copy one image from our last as a reference.
-  for (int i = 0, n = 0; i < range_likelihood_visualization_->getRowHeight (); ++i)
-  {
-    for (int j = 0; j < range_likelihood_visualization_->getColWidth (); ++j)
-    {
-      reference_vis[n++] = depth_buffer_vis[i*range_likelihood_visualization_->getWidth () + j];
+  for (int i = 0, n = 0; i < range_likelihood_visualization_->getRowHeight(); ++i) {
+    for (int j = 0; j < range_likelihood_visualization_->getColWidth(); ++j) {
+      reference_vis[n++] =
+          depth_buffer_vis[i * range_likelihood_visualization_->getWidth() + j];
     }
   }
 
-  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
+  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;
   std::vector<float> scores;
 
   // Render a single pose for visualization
-  poses.clear ();
-  poses.push_back (camera_->getPose ());
-  range_likelihood_visualization_->computeLikelihoods (reference_vis, poses, scores);
+  poses.clear();
+  poses.push_back(camera_->getPose());
+  range_likelihood_visualization_->computeLikelihoods(reference_vis, poses, scores);
 
-  glDrawBuffer (GL_BACK);
-  glReadBuffer (GL_BACK);
+  glDrawBuffer(GL_BACK);
+  glReadBuffer(GL_BACK);
 
   // Draw the resulting images from the range_likelihood
-  glViewport (range_likelihood_visualization_->getWidth (), 0,
-              range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
-  glMatrixMode (GL_PROJECTION);
-  glLoadIdentity ();
-  glMatrixMode (GL_MODELVIEW);
-  glLoadIdentity ();
+  glViewport(range_likelihood_visualization_->getWidth(),
+             0,
+             range_likelihood_visualization_->getWidth(),
+             range_likelihood_visualization_->getHeight());
+  glMatrixMode(GL_PROJECTION);
+  glLoadIdentity();
+  glMatrixMode(GL_MODELVIEW);
+  glLoadIdentity();
 
   // Draw the color image
-  glColorMask (true, true, true, true);
-  glClearColor (0, 0, 0, 0);
-  glClearDepth (1);
-  glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
-  glDisable (GL_DEPTH_TEST);
-
-  glRasterPos2i (-1,-1);
-  glDrawPixels (range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight (),
-                GL_RGB, GL_UNSIGNED_BYTE, range_likelihood_visualization_->getColorBuffer ());
+  glColorMask(true, true, true, true);
+  glClearColor(0, 0, 0, 0);
+  glClearDepth(1);
+  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+  glDisable(GL_DEPTH_TEST);
+
+  glRasterPos2i(-1, -1);
+  glDrawPixels(range_likelihood_visualization_->getWidth(),
+               range_likelihood_visualization_->getHeight(),
+               GL_RGB,
+               GL_UNSIGNED_BYTE,
+               range_likelihood_visualization_->getColorBuffer());
 
   // Draw the depth image
-  glViewport (0, 0, range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
-
-  glMatrixMode (GL_PROJECTION);
-  glLoadIdentity ();
-  glMatrixMode (GL_MODELVIEW);
-  glLoadIdentity ();
-  display_depth_image (range_likelihood_visualization_->getDepthBuffer (),
-                       range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
-
-
-  poses.clear ();
-  for (int i = 0; i < range_likelihood_->getRows (); ++i)
-  {
-    for (int j = 0; j < range_likelihood_->getCols (); ++j)
-    {
-      Camera camera (*camera_);
-      camera.move ((j - range_likelihood_->getCols () / 2.0) * 0.1,
-                   (i - range_likelihood_->getRows () / 2.0) * 0.1,
-                   0.0);
-      poses.push_back (camera.getPose ());
+  glViewport(0,
+             0,
+             range_likelihood_visualization_->getWidth(),
+             range_likelihood_visualization_->getHeight());
+
+  glMatrixMode(GL_PROJECTION);
+  glLoadIdentity();
+  glMatrixMode(GL_MODELVIEW);
+  glLoadIdentity();
+  display_depth_image(range_likelihood_visualization_->getDepthBuffer(),
+                      range_likelihood_visualization_->getWidth(),
+                      range_likelihood_visualization_->getHeight());
+
+  poses.clear();
+  for (int i = 0; i < range_likelihood_->getRows(); ++i) {
+    for (int j = 0; j < range_likelihood_->getCols(); ++j) {
+      Camera camera(*camera_);
+      camera.move((j - range_likelihood_->getCols() / 2.0) * 0.1,
+                  (i - range_likelihood_->getRows() / 2.0) * 0.1,
+                  0.0);
+      poses.push_back(camera.getPose());
     }
   }
   std::cout << std::endl;
 
   TicToc tt;
   tt.tic();
-  range_likelihood_->computeLikelihoods (reference, poses, scores);
+  range_likelihood_->computeLikelihoods(reference, poses, scores);
   tt.toc();
   tt.toc_print();
 
-  if (gllib::getGLError () != GL_NO_ERROR)
-  {
+  if (gllib::getGLError() != GL_NO_ERROR) {
     std::cerr << "GL Error: RangeLikelihood::computeLikelihoods: finished" << std::endl;
   }
 
@@ -284,101 +291,96 @@ display ()
   std::cout << std::endl;
 #endif
 
-  std::cout << "camera: " << camera_->getX ()
-       << " " << camera_->getY ()
-       << " " << camera_->getZ ()
-       << " " << camera_->getRoll ()
-       << " " << camera_->getPitch ()
-       << " " << camera_->getYaw ()
-       << std::endl;
+  std::cout << "camera: " << camera_->getX() << " " << camera_->getY() << " "
+            << camera_->getZ() << " " << camera_->getRoll() << " "
+            << camera_->getPitch() << " " << camera_->getYaw() << std::endl;
 
-  delete [] reference_vis;
-  delete [] reference;
+  delete[] reference_vis;
+  delete[] reference;
 
-  if (gllib::getGLError () != GL_NO_ERROR)
-  {
+  if (gllib::getGLError() != GL_NO_ERROR) {
     std::cerr << "GL Error: before buffers" << std::endl;
   }
 
-  glBindFramebuffer (GL_FRAMEBUFFER, 0);
-  glDrawBuffer (GL_BACK);
-  glReadBuffer (GL_BACK);
+  glBindFramebuffer(GL_FRAMEBUFFER, 0);
+  glDrawBuffer(GL_BACK);
+  glReadBuffer(GL_BACK);
 
-  if (gllib::getGLError () != GL_NO_ERROR)
-  {
+  if (gllib::getGLError() != GL_NO_ERROR) {
     std::cerr << "GL Error: after buffers" << std::endl;
   }
 
+  glMatrixMode(GL_PROJECTION);
+  glLoadIdentity();
+  glMatrixMode(GL_MODELVIEW);
+  glLoadIdentity();
 
-  glMatrixMode (GL_PROJECTION);
-  glLoadIdentity ();
-  glMatrixMode (GL_MODELVIEW);
-  glLoadIdentity ();
-
-  if (gllib::getGLError () != GL_NO_ERROR)
-  {
+  if (gllib::getGLError() != GL_NO_ERROR) {
     std::cerr << "GL Error: before viewport" << std::endl;
   }
 
   // Draw the score image for the particles
-  glViewport (0, range_likelihood_visualization_->getHeight (),
-              range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
-
+  glViewport(0,
+             range_likelihood_visualization_->getHeight(),
+             range_likelihood_visualization_->getWidth(),
+             range_likelihood_visualization_->getHeight());
 
-  if (gllib::getGLError () != GL_NO_ERROR)
-  {
+  if (gllib::getGLError() != GL_NO_ERROR) {
     std::cerr << "GL Error: after viewport" << std::endl;
   }
 
-  display_score_image (range_likelihood_->getScoreBuffer ());
+  display_score_image(range_likelihood_->getScoreBuffer());
 
   // Draw the depth image for the particles
-  glViewport (range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight (),
-              range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
+  glViewport(range_likelihood_visualization_->getWidth(),
+             range_likelihood_visualization_->getHeight(),
+             range_likelihood_visualization_->getWidth(),
+             range_likelihood_visualization_->getHeight());
 
-  display_score_image (range_likelihood_->getDepthBuffer ());
+  display_score_image(range_likelihood_->getDepthBuffer());
 
-  glutSwapBuffers ();
+  glutSwapBuffers();
 }
 
 // Handle normal keys
 void
-on_keyboard (unsigned char key, int, int)
+on_keyboard(unsigned char key, int, int)
 {
   if (key == 27)
-    exit (0);
+    exit(0);
 }
 
 // Read in a 3D model
 void
-loadPolygonMeshModel (char* polygon_file)
+loadPolygonMeshModel(char* polygon_file)
 {
   pcl::PolygonMesh mesh;
-  pcl::io::loadPolygonFile (polygon_file, mesh);
-  pcl::PolygonMesh::Ptr cloud (new pcl::PolygonMesh (mesh));
-  
-  TriangleMeshModel::Ptr model = TriangleMeshModel::Ptr (new TriangleMeshModel (cloud));
-  scene_->add (model);
-  
+  pcl::io::loadPolygonFile(polygon_file, mesh);
+  pcl::PolygonMesh::Ptr cloud(new pcl::PolygonMesh(mesh));
+
+  TriangleMeshModel::Ptr model = TriangleMeshModel::Ptr(new TriangleMeshModel(cloud));
+  scene_->add(model);
+
   std::cout << "Just read " << polygon_file << std::endl;
-  std::cout << mesh.polygons.size () << " polygons and "
-	    << mesh.cloud.data.size () << " triangles\n";
+  std::cout << mesh.polygons.size() << " polygons and " << mesh.cloud.data.size()
+            << " triangles\n";
 }
 
 void
-initialize (int argc, char** argv)
+initialize(int argc, char** argv)
 {
-  const GLubyte* version = glGetString (GL_VERSION);
-  print_info ("OpenGL Version: %s\n", version);
+  const GLubyte* version = glGetString(GL_VERSION);
+  print_info("OpenGL Version: %s\n", version);
 
   // works well for MIT CSAIL model 2nd floor:
-  camera_->set (27.4503, 37.383, 4.30908, 0.0, 0.0654498, -2.25802);
+  camera_->set(27.4503, 37.383, 4.30908, 0.0, 0.0654498, -2.25802);
 
-  if (argc > 1) loadPolygonMeshModel (argv[1]);
+  if (argc > 1)
+    loadPolygonMeshModel(argv[1]);
 }
 
 int
-main (int argc, char** argv)
+main(int argc, char** argv)
 {
   int width = 640;
   int height = 480;
@@ -390,73 +392,73 @@ main (int argc, char** argv)
   int rows = 30;
   int col_width = 20;
   int row_height = 15;
-  
-  print_info ("Range likelihood performance tests using pcl::simulation. For more information, use: %s -h\n", argv[0]);
 
-  if (argc < 2)
-  {
-    printHelp (argc, argv);
+  print_info("Range likelihood performance tests using pcl::simulation. For more "
+             "information, use: %s -h\n",
+             argv[0]);
+
+  if (argc < 2) {
+    printHelp(argc, argv);
     return (-1);
-  }  
-  
-  for (int i = 0; i < 2048; ++i)
-  {
-    float v = static_cast<float> (i / 2048.0);
-    v = powf(v, 3)* 6;
-    t_gamma[i] = static_cast<uint16_t> (v*6*256);
-  }  
-
-  glutInit (&argc, argv);
-  glutInitDisplayMode (GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);
-  glutInitWindowPosition (10, 10);
-  glutInitWindowSize (window_width_, window_height_);
-  glutCreateWindow ("OpenGL range likelihood");
-
-  GLenum err = glewInit ();
-  if (GLEW_OK != err)
-  {
-    std::cerr << "Error: " << glewGetErrorString (err) << std::endl;
-    exit (-1);
   }
 
-  std::cout << "Status: Using GLEW " << glewGetString (GLEW_VERSION) << std::endl;
+  for (int i = 0; i < 2048; ++i) {
+    float v = static_cast<float>(i / 2048.0);
+    v = powf(v, 3) * 6;
+    t_gamma[i] = static_cast<uint16_t>(v * 6 * 256);
+  }
+
+  glutInit(&argc, argv);
+  glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);
+  glutInitWindowPosition(10, 10);
+  glutInitWindowSize(window_width_, window_height_);
+  glutCreateWindow("OpenGL range likelihood");
+
+  GLenum err = glewInit();
+  if (GLEW_OK != err) {
+    std::cerr << "Error: " << glewGetErrorString(err) << std::endl;
+    exit(-1);
+  }
 
-  if (glewIsSupported ("GL_VERSION_2_0"))
+  std::cout << "Status: Using GLEW " << glewGetString(GLEW_VERSION) << std::endl;
+
+  if (glewIsSupported("GL_VERSION_2_0"))
     std::cout << "OpenGL 2.0 supported" << std::endl;
-  else
-  {
+  else {
     std::cerr << "Error: OpenGL 2.0 not supported" << std::endl;
-    exit (1);
+    exit(1);
   }
 
   std::cout << "GL_MAX_VIEWPORTS: " << GL_MAX_VIEWPORTS << std::endl;
 
-  camera_ = Camera::Ptr (new Camera ());
-  scene_ = Scene::Ptr (new Scene ());
+  camera_ = Camera::Ptr(new Camera());
+  scene_ = Scene::Ptr(new Scene());
 
-  range_likelihood_visualization_ = RangeLikelihood::Ptr (new RangeLikelihood (1, 1, height, width, scene_));
-  range_likelihood_ = RangeLikelihood::Ptr (new RangeLikelihood (rows, cols, row_height, col_width, scene_));
+  range_likelihood_visualization_ =
+      RangeLikelihood::Ptr(new RangeLikelihood(1, 1, height, width, scene_));
+  range_likelihood_ = RangeLikelihood::Ptr(
+      new RangeLikelihood(rows, cols, row_height, col_width, scene_));
 
   // Actually corresponds to default parameters:
-  range_likelihood_visualization_->setCameraIntrinsicsParameters (
+  range_likelihood_visualization_->setCameraIntrinsicsParameters(
       640, 480, 576.09757860f, 576.09757860f, 321.06398107f, 242.97676897f);
-  range_likelihood_visualization_->setComputeOnCPU (false);
-  range_likelihood_visualization_->setSumOnCPU (false);
-  range_likelihood_visualization_->setUseColor (true);
+  range_likelihood_visualization_->setComputeOnCPU(false);
+  range_likelihood_visualization_->setSumOnCPU(false);
+  range_likelihood_visualization_->setUseColor(true);
 
-  range_likelihood_->setCameraIntrinsicsParameters (
+  range_likelihood_->setCameraIntrinsicsParameters(
       640, 480, 576.09757860f, 576.09757860f, 321.06398107f, 242.97676897f);
-  range_likelihood_->setComputeOnCPU (false);
-  range_likelihood_->setSumOnCPU (false);
-  range_likelihood_->setUseColor (false);
+  range_likelihood_->setComputeOnCPU(false);
+  range_likelihood_->setSumOnCPU(false);
+  range_likelihood_->setUseColor(false);
 
-  textured_quad_ = TexturedQuad::Ptr (new TexturedQuad (range_likelihood_->getWidth (),
-                                                        range_likelihood_->getHeight ()));
+  textured_quad_ = TexturedQuad::Ptr(
+      new TexturedQuad(range_likelihood_->getWidth(), range_likelihood_->getHeight()));
 
-  initialize (argc, argv);
+  initialize(argc, argv);
 
-  glutDisplayFunc (display);
-  glutIdleFunc (display);
-  glutKeyboardFunc (on_keyboard);
-  glutMainLoop ();
+  glutDisplayFunc(display);
+  glutIdleFunc(display);
+  glutKeyboardFunc(on_keyboard);
+  glutMainLoop();
 }
diff --git a/simulation/tools/sim_test_simple.cpp b/simulation/tools/sim_test_simple.cpp
index f35e872643a..4237a1eb49d 100644
--- a/simulation/tools/sim_test_simple.cpp
+++ b/simulation/tools/sim_test_simple.cpp
@@ -9,7 +9,7 @@
  *  q - up
  *  z - down
  *  f - write point cloud file
- * 
+ *
  * The mouse controls the direction of movement
  *
  *  Esc - quit
@@ -17,28 +17,28 @@
  */
 
 #include <Eigen/Dense>
+#include <boost/shared_ptr.hpp>
 #include <cmath>
 #include <iostream>
 #include <thread>
-#include <boost/shared_ptr.hpp>
 #ifdef _WIN32
-# define WIN32_LEAN_AND_MEAN
-# include <windows.h>
+#define WIN32_LEAN_AND_MEAN
+#include <windows.h>
 #endif
 #include <GL/glew.h>
 
 #include <pcl/pcl_config.h>
 #ifdef OPENGL_IS_A_FRAMEWORK
-# include <OpenGL/gl.h>
-# include <OpenGL/glu.h>
+#include <OpenGL/gl.h>
+#include <OpenGL/glu.h>
 #else
-# include <GL/gl.h>
-# include <GL/glu.h>
+#include <GL/gl.h>
+#include <GL/glu.h>
 #endif
 #ifdef GLUT_IS_A_FRAMEWORK
-# include <GLUT/glut.h>
+#include <GLUT/glut.h>
 #else
-# include <GL/glut.h>
+#include <GL/glut.h>
 #endif
 
 #include <pcl/io/pcd_io.h>
@@ -47,9 +47,9 @@
 #include "pcl/common/common.h"
 #include "pcl/common/transforms.h"
 
-#include <pcl/point_types.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/features/normal_3d.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
 #include <pcl/surface/gp3.h>
 
 // define the following in order to eliminate the deprecated headers warning
@@ -58,11 +58,11 @@
 
 #include "pcl/simulation/camera.h"
 #include "pcl/simulation/model.h"
-#include "pcl/simulation/scene.h"
 #include "pcl/simulation/range_likelihood.h"
+#include "pcl/simulation/scene.h"
 
-#include <pcl/console/print.h>
 #include <pcl/console/parse.h>
+#include <pcl/console/print.h>
 
 // RangeImage:
 #include <pcl/range_image/range_image_planar.h>
@@ -90,386 +90,402 @@ int window_height_;
 bool paused_;
 bool write_file_;
 
-void printHelp (int, char **argv)
+void
+printHelp(int, char** argv)
 {
-  print_error ("Syntax is: %s <filename>\n", argv[0]);
-  print_info ("acceptable filenames include vtk, obj and ply. ply can support colour\n");
+  print_error("Syntax is: %s <filename>\n", argv[0]);
+  print_info("acceptable filenames include vtk, obj and ply. ply can support colour\n");
 }
 
-void wait ()
+void
+wait()
 {
-      std::cout << "Press enter to continue";
-      getchar();
-      std::cout << "\n\n";
+  std::cout << "Press enter to continue";
+  getchar();
+  std::cout << "\n\n";
 }
 
-void display_score_image(const float* score_buffer)
+void
+display_score_image(const float* score_buffer)
 {
   int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
   uint8_t* score_img = new uint8_t[npixels * 3];
 
   float min_score = score_buffer[0];
   float max_score = score_buffer[0];
-  for (int i=1; i<npixels; i++)
-  {
-    if (score_buffer[i] < min_score) min_score = score_buffer[i];
-    if (score_buffer[i] > max_score) max_score = score_buffer[i];
+  for (int i = 1; i < npixels; i++) {
+    if (score_buffer[i] < min_score)
+      min_score = score_buffer[i];
+    if (score_buffer[i] > max_score)
+      max_score = score_buffer[i];
   }
 
-  for (int i=0; i<npixels; i++)
-  {
-    float d = (score_buffer[i]-min_score)/(max_score-min_score);
-    score_img[3*i+0] = 0;
-    score_img[3*i+1] = d*255;
-    score_img[3*i+2] = 0;
+  for (int i = 0; i < npixels; i++) {
+    float d = (score_buffer[i] - min_score) / (max_score - min_score);
+    score_img[3 * i + 0] = 0;
+    score_img[3 * i + 1] = d * 255;
+    score_img[3 * i + 2] = 0;
   }
 
-  glRasterPos2i(-1,-1);
-  glDrawPixels(range_likelihood_->getWidth(), range_likelihood_->getHeight(), GL_RGB, GL_UNSIGNED_BYTE, score_img);
+  glRasterPos2i(-1, -1);
+  glDrawPixels(range_likelihood_->getWidth(),
+               range_likelihood_->getHeight(),
+               GL_RGB,
+               GL_UNSIGNED_BYTE,
+               score_img);
 
-  delete [] score_img;
+  delete[] score_img;
 }
 
-
-
-
-void display_depth_image (const float* depth_buffer, int width, int height)
+void
+display_depth_image(const float* depth_buffer, int width, int height)
 {
   int npixels = width * height;
   uint8_t* depth_img = new uint8_t[npixels * 3];
 
   float min_depth = depth_buffer[0];
   float max_depth = depth_buffer[0];
-  for (int i = 1; i < npixels; ++i)
-  {
-    if (depth_buffer[i] < min_depth) min_depth = depth_buffer[i];
-    if (depth_buffer[i] > max_depth) max_depth = depth_buffer[i];
+  for (int i = 1; i < npixels; ++i) {
+    if (depth_buffer[i] < min_depth)
+      min_depth = depth_buffer[i];
+    if (depth_buffer[i] > max_depth)
+      max_depth = depth_buffer[i];
   }
 
-  for (int i = 0; i < npixels; ++i)
-  {
+  for (int i = 0; i < npixels; ++i) {
     float zn = 0.7f;
     float zf = 20.0f;
     float d = depth_buffer[i];
-    float z = -zf*zn/((zf-zn)*(d - zf/(zf-zn)));
+    float z = -zf * zn / ((zf - zn) * (d - zf / (zf - zn)));
     float b = 0.075f;
     float f = 580.0f;
-    uint16_t kd = static_cast<uint16_t>(1090 - b*f/z*8);
-    if (kd > 2047) kd = 2047;
+    uint16_t kd = static_cast<uint16_t>(1090 - b * f / z * 8);
+    if (kd > 2047)
+      kd = 2047;
 
     int pval = t_gamma[kd];
     int lb = pval & 0xff;
-    switch (pval >> 8)
-    {
-      case 0:
-	  depth_img[3*i+0] = 255;
-	  depth_img[3*i+1] = 255-lb;
-	  depth_img[3*i+2] = 255-lb;
-	  break;
-      case 1:
-	  depth_img[3*i+0] = 255;
-	  depth_img[3*i+1] = lb;
-	  depth_img[3*i+2] = 0;
-	  break;
-      case 2:
-	  depth_img[3*i+0] = 255-lb;
-	  depth_img[3*i+1] = 255;
-	  depth_img[3*i+2] = 0;
-	  break;
-      case 3:
-	  depth_img[3*i+0] = 0;
-	  depth_img[3*i+1] = 255;
-	  depth_img[3*i+2] = lb;
-	  break;
-      case 4:
-	  depth_img[3*i+0] = 0;
-	  depth_img[3*i+1] = 255-lb;
-	  depth_img[3*i+2] = 255;
-	  break;
-      case 5:
-	  depth_img[3*i+0] = 0;
-	  depth_img[3*i+1] = 0;
-	  depth_img[3*i+2] = 255-lb;
-	  break;
-      default:
-	  depth_img[3*i+0] = 0;
-	  depth_img[3*i+1] = 0;
-	  depth_img[3*i+2] = 0;
-	  break;
+    switch (pval >> 8) {
+    case 0:
+      depth_img[3 * i + 0] = 255;
+      depth_img[3 * i + 1] = 255 - lb;
+      depth_img[3 * i + 2] = 255 - lb;
+      break;
+    case 1:
+      depth_img[3 * i + 0] = 255;
+      depth_img[3 * i + 1] = lb;
+      depth_img[3 * i + 2] = 0;
+      break;
+    case 2:
+      depth_img[3 * i + 0] = 255 - lb;
+      depth_img[3 * i + 1] = 255;
+      depth_img[3 * i + 2] = 0;
+      break;
+    case 3:
+      depth_img[3 * i + 0] = 0;
+      depth_img[3 * i + 1] = 255;
+      depth_img[3 * i + 2] = lb;
+      break;
+    case 4:
+      depth_img[3 * i + 0] = 0;
+      depth_img[3 * i + 1] = 255 - lb;
+      depth_img[3 * i + 2] = 255;
+      break;
+    case 5:
+      depth_img[3 * i + 0] = 0;
+      depth_img[3 * i + 1] = 0;
+      depth_img[3 * i + 2] = 255 - lb;
+      break;
+    default:
+      depth_img[3 * i + 0] = 0;
+      depth_img[3 * i + 1] = 0;
+      depth_img[3 * i + 2] = 0;
+      break;
     }
   }
 
-  glRasterPos2i (-1,-1);
-  glDrawPixels (width, height,
-                GL_RGB, GL_UNSIGNED_BYTE, depth_img);
+  glRasterPos2i(-1, -1);
+  glDrawPixels(width, height, GL_RGB, GL_UNSIGNED_BYTE, depth_img);
 
-  delete [] depth_img;
+  delete[] depth_img;
 }
 
 /*
-void display_depth_image(const float* depth_buffer)
+
+void
+display_depth_image(const float* depth_buffer)
 {
   int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
   uint8_t* depth_img = new uint8_t[npixels * 3];
 
   float min_depth = depth_buffer[0];
   float max_depth = depth_buffer[0];
-  for (int i=1; i<npixels; i++)
-  {
-    if (depth_buffer[i] < min_depth) min_depth = depth_buffer[i];
-    if (depth_buffer[i] > max_depth) max_depth = depth_buffer[i];
+  for (int i = 1; i < npixels; i++) {
+    if (depth_buffer[i] < min_depth)
+      min_depth = depth_buffer[i];
+    if (depth_buffer[i] > max_depth)
+      max_depth = depth_buffer[i];
   }
 
-  for (int i=0; i<npixels; i++)
-  {
+  for (int i = 0; i < npixels; i++) {
     float zn = 0.7;
     float zf = 20.0;
     float d = depth_buffer[i];
-    float z = -zf*zn/((zf-zn)*(d - zf/(zf-zn)));
+    float z = -zf * zn / ((zf - zn) * (d - zf / (zf - zn)));
     float b = 0.075;
     float f = 580.0;
-    uint16_t kd = static_cast<uint16_t>(1090 - b*f/z*8);
-    if (kd < 0) kd = 0;
-    else if (kd>2047) kd = 2047;
+    uint16_t kd = static_cast<uint16_t>(1090 - b * f / z * 8);
+    if (kd < 0)
+      kd = 0;
+    else if (kd > 2047)
+      kd = 2047;
 
     int pval = t_gamma[kd];
     int lb = pval & 0xff;
-    switch (pval>>8) {
-      case 0:
-	  depth_img[3*i+0] = 255;
-	  depth_img[3*i+1] = 255-lb;
-	  depth_img[3*i+2] = 255-lb;
-	  break;
-      case 1:
-	  depth_img[3*i+0] = 255;
-	  depth_img[3*i+1] = lb;
-	  depth_img[3*i+2] = 0;
-	  break;
-      case 2:
-	  depth_img[3*i+0] = 255-lb;
-	  depth_img[3*i+1] = 255;
-	  depth_img[3*i+2] = 0;
-	  break;
-      case 3:
-	  depth_img[3*i+0] = 0;
-	  depth_img[3*i+1] = 255;
-	  depth_img[3*i+2] = lb;
-	  break;
-      case 4:
-	  depth_img[3*i+0] = 0;
-	  depth_img[3*i+1] = 255-lb;
-	  depth_img[3*i+2] = 255;
-	  break;
-      case 5:
-	  depth_img[3*i+0] = 0;
-	  depth_img[3*i+1] = 0;
-	  depth_img[3*i+2] = 255-lb;
-	  break;
-      default:
-	  depth_img[3*i+0] = 0;
-	  depth_img[3*i+1] = 0;
-	  depth_img[3*i+2] = 0;
-	  break;
+    switch (pval >> 8) {
+    case 0:
+      depth_img[3 * i + 0] = 255;
+      depth_img[3 * i + 1] = 255 - lb;
+      depth_img[3 * i + 2] = 255 - lb;
+      break;
+    case 1:
+      depth_img[3 * i + 0] = 255;
+      depth_img[3 * i + 1] = lb;
+      depth_img[3 * i + 2] = 0;
+      break;
+    case 2:
+      depth_img[3 * i + 0] = 255 - lb;
+      depth_img[3 * i + 1] = 255;
+      depth_img[3 * i + 2] = 0;
+      break;
+    case 3:
+      depth_img[3 * i + 0] = 0;
+      depth_img[3 * i + 1] = 255;
+      depth_img[3 * i + 2] = lb;
+      break;
+    case 4:
+      depth_img[3 * i + 0] = 0;
+      depth_img[3 * i + 1] = 255 - lb;
+      depth_img[3 * i + 2] = 255;
+      break;
+    case 5:
+      depth_img[3 * i + 0] = 0;
+      depth_img[3 * i + 1] = 0;
+      depth_img[3 * i + 2] = 255 - lb;
+      break;
+    default:
+      depth_img[3 * i + 0] = 0;
+      depth_img[3 * i + 1] = 0;
+      depth_img[3 * i + 2] = 0;
+      break;
     }
   }
 
-  glRasterPos2i(-1,-1);
-  glDrawPixels(range_likelihood_->getWidth(), range_likelihood_->getHeight(), GL_RGB, GL_UNSIGNED_BYTE, depth_img);
+  glRasterPos2i(-1, -1);
+  glDrawPixels(range_likelihood_->getWidth(),
+               range_likelihood_->getHeight(),
+               GL_RGB,
+               GL_UNSIGNED_BYTE,
+               depth_img);
 
-  delete [] depth_img;
+  delete[] depth_img;
 }
+
 */
 
-pcl::visualization::PCLVisualizer::Ptr simpleVis (pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
+pcl::visualization::PCLVisualizer::Ptr
+simpleVis(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
 {
   // --------------------------------------------
   // -----Open 3D viewer and add point cloud-----
   // --------------------------------------------
-  pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
-  viewer->setBackgroundColor (0, 0, 0);
+  pcl::visualization::PCLVisualizer::Ptr viewer(
+      new pcl::visualization::PCLVisualizer("3D Viewer"));
+  viewer->setBackgroundColor(0, 0, 0);
   pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
-  viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud");
-  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
-  viewer->addCoordinateSystem (1.0, "reference");
-  viewer->initCameraParameters ();
+  viewer->addPointCloud<pcl::PointXYZRGB>(cloud, rgb, "sample cloud");
+  viewer->setPointCloudRenderingProperties(
+      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
+  viewer->addCoordinateSystem(1.0, "reference");
+  viewer->initCameraParameters();
   return (viewer);
 }
 
-
-void display ()
+void
+display()
 {
-  float* reference = new float[range_likelihood_->getRowHeight() * range_likelihood_->getColWidth()];
+  float* reference =
+      new float[range_likelihood_->getRowHeight() * range_likelihood_->getColWidth()];
   const float* depth_buffer = range_likelihood_->getDepthBuffer();
   // Copy one image from our last as a reference.
-  for (int i=0, n=0; i<range_likelihood_->getRowHeight(); ++i)
-  {
-    for (int j=0; j<range_likelihood_->getColWidth(); ++j)
-    {
-      reference[n++] = depth_buffer[i*range_likelihood_->getWidth() + j];
+  for (int i = 0, n = 0; i < range_likelihood_->getRowHeight(); ++i) {
+    for (int j = 0; j < range_likelihood_->getColWidth(); ++j) {
+      reference[n++] = depth_buffer[i * range_likelihood_->getWidth() + j];
     }
   }
 
-  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
+  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;
   std::vector<float> scores;
-  int n = range_likelihood_->getRows ()*range_likelihood_->getCols ();
-  for (int i = 0; i < n; ++i)
-  {
+  int n = range_likelihood_->getRows() * range_likelihood_->getCols();
+  for (int i = 0; i < n; ++i) {
     Camera camera(*camera_);
-    camera.move(0.0,i*0.02,0.0);
-    //camera.move(0.0,i*0.02,0.0);
-    poses.push_back (camera.getPose ());
+    camera.move(0.0, i * 0.02, 0.0);
+    // camera.move(0.0,i*0.02,0.0);
+    poses.push_back(camera.getPose());
   }
-  
-  range_likelihood_->computeLikelihoods (reference, poses, scores);
-  
-  range_likelihood_->computeLikelihoods (reference, poses, scores);
+
+  range_likelihood_->computeLikelihoods(reference, poses, scores);
+
+  range_likelihood_->computeLikelihoods(reference, poses, scores);
   std::cout << "score: ";
-  for (const float &score : scores)
-  {
+  for (const float& score : scores) {
     std::cout << " " << score;
   }
   std::cout << std::endl;
 
-  std::cout << "camera: " << camera_->getX ()
-       << " " << camera_->getY ()
-       << " " << camera_->getZ ()
-       << " " << camera_->getRoll ()
-       << " " << camera_->getPitch ()
-       << " " << camera_->getYaw ()
-       << std::endl;
+  std::cout << "camera: " << camera_->getX() << " " << camera_->getY() << " "
+            << camera_->getZ() << " " << camera_->getRoll() << " "
+            << camera_->getPitch() << " " << camera_->getYaw() << std::endl;
 
-  delete [] reference;
+  delete[] reference;
 
-  glDrawBuffer (GL_BACK);
-  glReadBuffer (GL_BACK);
+  glDrawBuffer(GL_BACK);
+  glReadBuffer(GL_BACK);
 
   // Draw the resulting images from the range_likelihood
-  glViewport (range_likelihood_->getWidth (), 0, range_likelihood_->getWidth (), range_likelihood_->getHeight ());
-  glMatrixMode (GL_PROJECTION);
-  glLoadIdentity ();
-  glMatrixMode (GL_MODELVIEW);
-  glLoadIdentity ();
+  glViewport(range_likelihood_->getWidth(),
+             0,
+             range_likelihood_->getWidth(),
+             range_likelihood_->getHeight());
+  glMatrixMode(GL_PROJECTION);
+  glLoadIdentity();
+  glMatrixMode(GL_MODELVIEW);
+  glLoadIdentity();
 
   // Draw the color image
-  glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
-  glColorMask (true, true, true, true);
-  glDisable (GL_DEPTH_TEST);
+  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+  glColorMask(true, true, true, true);
+  glDisable(GL_DEPTH_TEST);
 
-  glRasterPos2i (-1,-1);
-  glDrawPixels (range_likelihood_->getWidth (), range_likelihood_->getHeight (),
-                GL_RGB, GL_UNSIGNED_BYTE, range_likelihood_->getColorBuffer ());
+  glRasterPos2i(-1, -1);
+  glDrawPixels(range_likelihood_->getWidth(),
+               range_likelihood_->getHeight(),
+               GL_RGB,
+               GL_UNSIGNED_BYTE,
+               range_likelihood_->getColorBuffer());
 
   // Draw the depth image
-  glViewport (0, 0, range_likelihood_->getWidth (), range_likelihood_->getHeight ());
-
-  glMatrixMode (GL_PROJECTION);
-  glLoadIdentity ();
-  glMatrixMode (GL_MODELVIEW);
-  glLoadIdentity ();
-//  display_depth_image (range_likelihood_->getDepthBuffer ());
-  display_depth_image (range_likelihood_->getDepthBuffer (),
-                       range_likelihood_->getWidth (), range_likelihood_->getHeight ());
-  
-  
+  glViewport(0, 0, range_likelihood_->getWidth(), range_likelihood_->getHeight());
+
+  glMatrixMode(GL_PROJECTION);
+  glLoadIdentity();
+  glMatrixMode(GL_MODELVIEW);
+  glLoadIdentity();
+  //  display_depth_image (range_likelihood_->getDepthBuffer ());
+  display_depth_image(range_likelihood_->getDepthBuffer(),
+                      range_likelihood_->getWidth(),
+                      range_likelihood_->getHeight());
+
   // Draw the score image
-  glViewport (0, range_likelihood_->getHeight (),
-              range_likelihood_->getWidth (), range_likelihood_->getHeight ());
-  glMatrixMode (GL_PROJECTION);
-  glLoadIdentity ();
-  glMatrixMode (GL_MODELVIEW);
-  glLoadIdentity ();
-  display_score_image (range_likelihood_->getScoreBuffer ());
-
-  glutSwapBuffers ();
-  
-  if (write_file_)
-  {
-    range_likelihood_->addNoise ();
+  glViewport(0,
+             range_likelihood_->getHeight(),
+             range_likelihood_->getWidth(),
+             range_likelihood_->getHeight());
+  glMatrixMode(GL_PROJECTION);
+  glLoadIdentity();
+  glMatrixMode(GL_MODELVIEW);
+  glLoadIdentity();
+  display_score_image(range_likelihood_->getScoreBuffer());
+
+  glutSwapBuffers();
+
+  if (write_file_) {
+    range_likelihood_->addNoise();
     pcl::RangeImagePlanar rangeImage;
-    range_likelihood_->getRangeImagePlanar (rangeImage);
+    range_likelihood_->getRangeImagePlanar(rangeImage);
 
-    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out (new pcl::PointCloud<pcl::PointXYZRGB>);
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out(
+        new pcl::PointCloud<pcl::PointXYZRGB>);
 
     // Optional argument to save point cloud in global frame:
     // Save camera relative:
-    //range_likelihood_->getPointCloud(pc_out);
+    // range_likelihood_->getPointCloud(pc_out);
     // Save in global frame - applying the camera frame:
-    //range_likelihood_->getPointCloud(pc_out,true,camera_->pose());
+    // range_likelihood_->getPointCloud(pc_out,true,camera_->pose());
     // Save in local frame
-    range_likelihood_->getPointCloud (pc_out,false,camera_->getPose ());
+    range_likelihood_->getPointCloud(pc_out, false, camera_->getPose());
     // TODO: what to do when there are more than one simulated view?
-    
+
     pcl::PCDWriter writer;
-    writer.write ("simulated_range_image.pcd", *pc_out,	false);  
+    writer.write("simulated_range_image.pcd", *pc_out, false);
     std::cout << "finished writing file\n";
-    
-//     pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
-//     viewer.showCloud (pc_out);
-
-
-  pcl::visualization::PCLVisualizer::Ptr viewer;
-  viewer = simpleVis(pc_out);
-  while (!viewer->wasStopped ())
-  {
-    viewer->spinOnce (100);
-    std::this_thread::sleep_for(100ms);
-  }    
-  
-  // doesn't work:
-//    viewer->~PCLVisualizer();
-//    viewer.reset();
-    
-    
+
+    //     pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
+    //     viewer.showCloud (pc_out);
+
+    pcl::visualization::PCLVisualizer::Ptr viewer;
+    viewer = simpleVis(pc_out);
+    while (!viewer->wasStopped()) {
+      viewer->spinOnce(100);
+      std::this_thread::sleep_for(100ms);
+    }
+
+    // doesn't work:
+    //    viewer->~PCLVisualizer();
+    //    viewer.reset();
+
     std::cout << "done\n";
     // Problem: vtk and opengl don't seem to play very well together
-    // vtk seems to misbehave after a little while and won't keep the window on the screen
+    // vtk seems to misbehave after a little while and won't keep the window on the
+    // screen
 
     // method1: kill with [x] - but eventually it crashes:
-    //while (!viewer.wasStopped ()){
-    //}    
-    
-    // method2: eventually starts ignoring cin and pops up on screen and closes almost 
+    // while (!viewer.wasStopped ()){
+    //}
+
+    // method2: eventually starts ignoring cin and pops up on screen and closes almost
     // immediately
     //  std::cout << "enter 1 to cont\n";
     //  cin >> pause;
     //  viewer.wasStopped ();
-    
-    // method 3: if you interact with the window with keys, the window is not closed properly
+
+    // method 3: if you interact with the window with keys, the window is not closed
+    // properly
     // TODO: use pcl methods as this time stuff is probably not cross playform
-//     struct timespec t;
-//     t.tv_sec = 100;
-//     //t.tv_nsec = (time_t)(20000000); // short sleep
-//     t.tv_nsec = (time_t)(0);  // long sleep - normal speed
-//     nanosleep (&t, NULL);
+    //     struct timespec t;
+    //     t.tv_sec = 100;
+    //     //t.tv_nsec = (time_t)(20000000); // short sleep
+    //     t.tv_nsec = (time_t)(0);  // long sleep - normal speed
+    //     nanosleep (&t, NULL);
     write_file_ = false;
   }
 }
 
 // Handle normal keys
 void
-on_keyboard (unsigned char key, int, int)
+on_keyboard(unsigned char key, int, int)
 {
   double speed = 0.1;
 
   if (key == 27)
     exit(0);
   else if (key == 'w' || key == 'W')
-    camera_->move(speed,0,0);
+    camera_->move(speed, 0, 0);
   else if (key == 's' || key == 'S')
-    camera_->move(-speed,0,0);
+    camera_->move(-speed, 0, 0);
   else if (key == 'a' || key == 'A')
-    camera_->move(0,speed,0);
+    camera_->move(0, speed, 0);
   else if (key == 'd' || key == 'D')
-    camera_->move(0,-speed,0);
+    camera_->move(0, -speed, 0);
   else if (key == 'q' || key == 'Q')
-    camera_->move(0,0,speed);
+    camera_->move(0, 0, speed);
   else if (key == 'z' || key == 'Z')
-    camera_->move(0,0,-speed);
+    camera_->move(0, 0, -speed);
   else if (key == 'p' || key == 'P')
     paused_ = !paused_;
   else if (key == 'v' || key == 'V')
     write_file_ = true;
-  
+
   // Use glutGetModifiers for modifiers
   // GLUT_ACTIVE_SHIFT, GLUT_ACTIVE_CTRL, GLUT_ACTIVE_ALT
 }
@@ -509,74 +525,76 @@ on_mouse(int, int, int, int)
 
 void
 on_motion(int, int)
-{
-}
+{}
 
 void
 on_passive_motion(int x, int y)
 {
-  if (paused_) return;
+  if (paused_)
+    return;
 
-  double pitch = -(0.5-(double)y/window_height_)*M_PI * 4; // in window coordinates positive y-axis is down
-  double yaw =    (0.5-(double)x/window_width_)*M_PI*2 * 4;
+  // in window coordinates positive y-axis is down
+  double pitch = -(0.5 - (double)y / window_height_) * M_PI * 4;
+  double yaw = (0.5 - (double)x / window_width_) * M_PI * 2 * 4;
 
-  camera_->setPitch (pitch);
-  camera_->setYaw (yaw);
+  camera_->setPitch(pitch);
+  camera_->setYaw(yaw);
 }
 
-void on_entry (int)
+void
+on_entry(int)
 {
   // state:
   // GLUT_LEFT
   // GLUT_ENTERED
 }
 
-
 // Read in a 3D model
-void load_PolygonMesh_model (char* polygon_file)
+void
+load_PolygonMesh_model(char* polygon_file)
 {
-  pcl::PolygonMesh mesh;	// (new pcl::PolygonMesh);
-  //pcl::io::loadPolygonFile("/home/mfallon/data/models/dalet/Darlek_modified_works.obj",mesh);
-  pcl::io::loadPolygonFile (polygon_file, mesh);
-  pcl::PolygonMesh::Ptr cloud (new pcl::PolygonMesh (mesh));
-  
+  pcl::PolygonMesh mesh; // (new pcl::PolygonMesh);
+  // pcl::io::loadPolygonFile("/home/mfallon/data/models/dalet/Darlek_modified_works.obj",mesh);
+  pcl::io::loadPolygonFile(polygon_file, mesh);
+  pcl::PolygonMesh::Ptr cloud(new pcl::PolygonMesh(mesh));
+
   // Not sure if PolygonMesh assumes triangles if to
   // TODO: Ask a developer
-  PolygonMeshModel::Ptr model = PolygonMeshModel::Ptr (new PolygonMeshModel (GL_POLYGON, cloud));
-  scene_->add (model);
-  
+  PolygonMeshModel::Ptr model =
+      PolygonMeshModel::Ptr(new PolygonMeshModel(GL_POLYGON, cloud));
+  scene_->add(model);
+
   std::cout << "Just read " << polygon_file << std::endl;
-  std::cout << mesh.polygons.size () << " polygons and "
-	    << mesh.cloud.data.size () << " triangles\n";
-  
+  std::cout << mesh.polygons.size() << " polygons and " << mesh.cloud.data.size()
+            << " triangles\n";
 }
 
 void
-initialize (int, char** argv)
+initialize(int, char** argv)
 {
-  const GLubyte* version = glGetString (GL_VERSION);
+  const GLubyte* version = glGetString(GL_VERSION);
   std::cout << "OpenGL Version: " << version << std::endl;
 
   // works well for MIT CSAIL model 3rd floor:
-  //camera_->set(4.04454, 44.9377, 1.1, 0.0, 0.0, -2.00352);
+  // camera_->set(4.04454, 44.9377, 1.1, 0.0, 0.0, -2.00352);
 
   // works well for MIT CSAIL model 2nd floor:
-//  camera_->set (27.4503, 37.383, 4.30908, 0.0, 0.0654498, -2.25802);
+  //  camera_->set (27.4503, 37.383, 4.30908, 0.0, 0.0654498, -2.25802);
 
   // works for small files:
-  //camera_->set(-5.0, 0.0, 1.0, 0.0, 0.0, 0.0);
-  camera_->set( 1.31762, 0.382931, 1.89533, 0, 0.20944, -9.14989);
-  camera_->setPitch(0.20944); // not sure why this is here: 
-  //camera_->set(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
-  
+  // camera_->set(-5.0, 0.0, 1.0, 0.0, 0.0, 0.0);
+  camera_->set(1.31762, 0.382931, 1.89533, 0, 0.20944, -9.14989);
+  camera_->setPitch(0.20944); // not sure why this is here:
+  // camera_->set(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+
   std::cout << "About to read: " << argv[1] << std::endl;
-  load_PolygonMesh_model (argv[1]);
-    
+  load_PolygonMesh_model(argv[1]);
+
   paused_ = false;
 }
 
 int
-main (int argc, char** argv)
+main(int argc, char** argv)
 {
   int width = 640;
   int height = 480;
@@ -584,72 +602,73 @@ main (int argc, char** argv)
   window_width_ = width * 2;
   window_height_ = height * 2;
 
-  print_info ("Manually generate a simulated RGB-D point cloud using pcl::simulation. For more information, use: %s -h\n", argv[0]);
+  print_info("Manually generate a simulated RGB-D point cloud using pcl::simulation. "
+             "For more information, use: %s -h\n",
+             argv[0]);
 
-  if (argc < 2)
-  {
-    printHelp (argc, argv);
+  if (argc < 2) {
+    printHelp(argc, argv);
     return (-1);
-  }  
-  
-  for (int i=0; i < 2048; i++)
-  {
-    float v = i/2048.0;
-    v = powf(v, 3)* 6;
-    t_gamma[i] = v*6*256;
-  }  
-
-  glutInit (&argc, argv);
-  glutInitDisplayMode (GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);// was GLUT_RGBA
-  glutInitWindowPosition (10, 10);
-  glutInitWindowSize (window_width_, window_height_);
-  glutCreateWindow ("OpenGL range likelihood");
-
-  GLenum err = glewInit ();
-  if (GLEW_OK != err)
-  {
-    std::cerr << "Error: " << glewGetErrorString (err) << std::endl;
-    exit (-1);
   }
 
-  std::cout << "Status: Using GLEW " << glewGetString (GLEW_VERSION) << std::endl;
+  for (int i = 0; i < 2048; i++) {
+    float v = i / 2048.0;
+    v = powf(v, 3) * 6;
+    t_gamma[i] = v * 6 * 256;
+  }
+
+  glutInit(&argc, argv);
+  glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB); // was GLUT_RGBA
+  glutInitWindowPosition(10, 10);
+  glutInitWindowSize(window_width_, window_height_);
+  glutCreateWindow("OpenGL range likelihood");
+
+  GLenum err = glewInit();
+  if (GLEW_OK != err) {
+    std::cerr << "Error: " << glewGetErrorString(err) << std::endl;
+    exit(-1);
+  }
+
+  std::cout << "Status: Using GLEW " << glewGetString(GLEW_VERSION) << std::endl;
 
-  if (glewIsSupported ("GL_VERSION_2_0"))
+  if (glewIsSupported("GL_VERSION_2_0"))
     std::cout << "OpenGL 2.0 supported" << std::endl;
-  else
-  {
+  else {
     std::cerr << "Error: OpenGL 2.0 not supported" << std::endl;
     exit(1);
   }
   std::cout << "GL_MAX_VIEWPORTS: " << GL_MAX_VIEWPORTS << std::endl;
 
-  camera_ = Camera::Ptr (new Camera ());
-  scene_ = Scene::Ptr (new Scene ());
+  camera_ = Camera::Ptr(new Camera());
+  scene_ = Scene::Ptr(new Scene());
 
-  //range_likelihood_ = RangeLikelihoodGLSL::Ptr(new RangeLikelihoodGLSL(1, 1, height, width, scene_, 0));
+  // range_likelihood_ = RangeLikelihoodGLSL::Ptr(new RangeLikelihoodGLSL(1, 1, height,
+  // width, scene_, 0));
 
-  range_likelihood_ = RangeLikelihood::Ptr (new RangeLikelihood (2, 2, height/2, width/2, scene_));
-  // range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(10, 10, 96, 96, scene_));
-  // range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, 480, 640, scene_));
+  range_likelihood_ =
+      RangeLikelihood::Ptr(new RangeLikelihood(2, 2, height / 2, width / 2, scene_));
+  // range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(10, 10, 96, 96,
+  // scene_)); range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, 480,
+  // 640, scene_));
 
   // Actually corresponds to default parameters:
-  range_likelihood_->setCameraIntrinsicsParameters (640,480, 576.09757860,
-            576.09757860, 321.06398107, 242.97676897);
-  range_likelihood_->setComputeOnCPU (false);
-  range_likelihood_->setSumOnCPU (true);
-  range_likelihood_->setUseColor (true);
-  
-  initialize (argc, argv);
-
-  glutReshapeFunc (on_reshape);
-  glutDisplayFunc (display);
-  glutIdleFunc (display);
-  glutKeyboardFunc (on_keyboard);
-  glutMouseFunc (on_mouse);
-  glutMotionFunc (on_motion);
-  glutPassiveMotionFunc (on_passive_motion);
-  glutEntryFunc (on_entry);
-  glutMainLoop ();
+  range_likelihood_->setCameraIntrinsicsParameters(
+      640, 480, 576.09757860, 576.09757860, 321.06398107, 242.97676897);
+  range_likelihood_->setComputeOnCPU(false);
+  range_likelihood_->setSumOnCPU(true);
+  range_likelihood_->setUseColor(true);
+
+  initialize(argc, argv);
+
+  glutReshapeFunc(on_reshape);
+  glutDisplayFunc(display);
+  glutIdleFunc(display);
+  glutKeyboardFunc(on_keyboard);
+  glutMouseFunc(on_mouse);
+  glutMotionFunc(on_motion);
+  glutPassiveMotionFunc(on_passive_motion);
+  glutEntryFunc(on_entry);
+  glutMainLoop();
 
   return 0;
 }
diff --git a/simulation/tools/sim_viewer.cpp b/simulation/tools/sim_viewer.cpp
index b245694cae5..6bdff9a6247 100644
--- a/simulation/tools/sim_viewer.cpp
+++ b/simulation/tools/sim_viewer.cpp
@@ -36,23 +36,23 @@
  * $Id: pcd_viewer.cpp 5094 2012-03-15 01:03:51Z rusu $
  *
  */
-#include <Eigen/Geometry>
 #include <Eigen/Dense>
+#include <Eigen/Geometry>
+#include <boost/shared_ptr.hpp>
 #include <cmath>
 #include <iostream>
-#include <boost/shared_ptr.hpp>
 #ifdef _WIN32
-# define WIN32_LEAN_AND_MEAN
-# include <windows.h>
+#define WIN32_LEAN_AND_MEAN
+#include <windows.h>
 #endif
 
 #include <GL/glew.h>
 
 #include <pcl/pcl_config.h>
 #ifdef OPENGL_IS_A_FRAMEWORK
-# include <OpenGL/gl.h>
+#include <OpenGL/gl.h>
 #else
-# include <GL/gl.h>
+#include <GL/gl.h>
 #endif
 
 #include <pcl/io/pcd_io.h>
@@ -61,9 +61,9 @@
 #include "pcl/common/common.h"
 #include "pcl/common/transforms.h"
 
-#include <pcl/point_types.h>
-#include <pcl/io/pcd_io.h>
 #include <pcl/features/normal_3d.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
 #include <pcl/surface/gp3.h>
 
 // define the following in order to eliminate the deprecated headers warning
@@ -72,11 +72,11 @@
 
 #include "pcl/simulation/camera.h"
 #include "pcl/simulation/model.h"
-#include "pcl/simulation/scene.h"
 #include "pcl/simulation/range_likelihood.h"
+#include "pcl/simulation/scene.h"
 
-#include <pcl/console/print.h>
 #include <pcl/console/parse.h>
+#include <pcl/console/print.h>
 #include <pcl/console/time.h>
 
 // RangeImage:
@@ -85,18 +85,18 @@
 // Pop-up viewer
 #include <pcl/visualization/cloud_viewer.h>
 
+#include <cfloat>
 #include <pcl/common/common.h>
+#include <pcl/console/parse.h>
+#include <pcl/console/print.h>
+#include <pcl/console/time.h>
 #include <pcl/io/pcd_io.h>
-#include <cfloat>
-#include <pcl/visualization/point_cloud_handlers.h>
-#include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/visualization/histogram_visualizer.h>
+#include <pcl/visualization/keyboard_event.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/visualization/point_cloud_handlers.h>
 #include <pcl/visualization/point_picking_event.h>
-#include <pcl/console/print.h>
-#include <pcl/console/parse.h>
-#include <pcl/console/time.h>
 #include <vtkPolyDataReader.h>
-#include <pcl/visualization/keyboard_event.h>
 
 using namespace Eigen;
 using namespace pcl;
@@ -110,7 +110,8 @@ using ColorHandler = pcl::visualization::PointCloudColorHandler<pcl::PCLPointClo
 using ColorHandlerPtr = ColorHandler::Ptr;
 using ColorHandlerConstPtr = ColorHandler::ConstPtr;
 
-using GeometryHandler = pcl::visualization::PointCloudGeometryHandler<pcl::PCLPointCloud2>;
+using GeometryHandler =
+    pcl::visualization::PointCloudGeometryHandler<pcl::PCLPointCloud2>;
 using GeometryHandlerPtr = GeometryHandler::Ptr;
 using GeometryHandlerConstPtr = GeometryHandler::ConstPtr;
 
@@ -127,18 +128,19 @@ bool paused_;
 bool write_file_;
 
 bool
-isValidFieldName (const std::string &field)
+isValidFieldName(const std::string& field)
 {
   if (field == "_")
     return (false);
 
-  if ((field == "vp_x") || (field == "vx") || (field == "vp_y") || (field == "vy") || (field == "vp_z") || (field == "vz"))
+  if ((field == "vp_x") || (field == "vx") || (field == "vp_y") || (field == "vy") ||
+      (field == "vp_z") || (field == "vz"))
     return (false);
   return (true);
 }
 
 bool
-isMultiDimensionalFeatureField (const pcl::PCLPointField &field)
+isMultiDimensionalFeatureField(const pcl::PCLPointField& field)
 {
   if (field.count > 1)
     return (true);
@@ -146,38 +148,71 @@ isMultiDimensionalFeatureField (const pcl::PCLPointField &field)
 }
 
 void
-printHelp (int, char **argv)
+printHelp(int, char** argv)
 {
-  print_error ("Syntax is: %s <file_name 1..N>.<pcd or vtk> <options>\n", argv[0]);
-  print_info ("pcl::simulation viewer\n");  
-  print_info ("  where options are:\n");
-  print_info ("                     -bc r,g,b                = background color\n");
-  print_info ("                     -fc r,g,b                = foreground color\n");
-  print_info ("                     -ps X                    = point size ("); print_value ("1..64"); print_info (") \n");
-  print_info ("                     -opaque X                = rendered point cloud opacity ("); print_value ("0..1"); print_info (")\n");
-
-  print_info ("                     -ax "); print_value ("n"); print_info ("                    = enable on-screen display of ");
-  print_color (stdout, TT_BRIGHT, TT_RED, "X"); print_color (stdout, TT_BRIGHT, TT_GREEN, "Y"); print_color (stdout, TT_BRIGHT, TT_BLUE, "Z");
-  print_info (" axes and scale them to "); print_value ("n\n");
-  print_info ("                     -ax_pos X,Y,Z            = if axes are enabled, set their X,Y,Z position in space (default "); print_value ("0,0,0"); print_info (")\n");
-
-  print_info ("\n");
-  print_info ("                     -cam (*)                 = use given camera settings as initial view\n");
-  print_info (stderr, " (*) [Clipping Range / Focal Point / Position / ViewUp / Distance / Field of View Y / Window Size / Window Pos] or use a <filename.cam> that contains the same information.\n");
-
-  print_info ("\n");
-  print_info ("                     -multiview 0/1           = enable/disable auto-multi viewport rendering (default "); print_value ("disabled"); print_info (")\n");
-  print_info ("\n");
-
-  print_info ("\n");
-  print_info ("                     -normals 0/X             = disable/enable the display of every Xth point's surface normal as lines (default "); print_value ("disabled"); print_info (")\n");
-  print_info ("                     -normals_scale X         = resize the normal unit vector size to X (default "); print_value ("0.02"); print_info (")\n");
-  print_info ("\n");
-  print_info ("                     -pc 0/X                  = disable/enable the display of every Xth point's principal curvatures as lines (default "); print_value ("disabled"); print_info (")\n");
-  print_info ("                     -pc_scale X              = resize the principal curvatures vectors size to X (default "); print_value ("0.02"); print_info (")\n");
-  print_info ("\n");
-
-  print_info ("\n(Note: for multiple .pcd files, provide multiple -{fc,ps,opaque} parameters; they will be automatically assigned to the right file)\n");
+  print_error("Syntax is: %s <file_name 1..N>.<pcd or vtk> <options>\n", argv[0]);
+  print_info("pcl::simulation viewer\n");
+  print_info("  where options are:\n");
+  print_info("                     -bc r,g,b                = background color\n");
+  print_info("                     -fc r,g,b                = foreground color\n");
+  print_info("                     -ps X                    = point size (");
+  print_value("1..64");
+  print_info(") \n");
+  print_info(
+      "                     -opaque X                = rendered point cloud opacity (");
+  print_value("0..1");
+  print_info(")\n");
+
+  print_info("                     -ax ");
+  print_value("n");
+  print_info("                    = enable on-screen display of ");
+  print_color(stdout, TT_BRIGHT, TT_RED, "X");
+  print_color(stdout, TT_BRIGHT, TT_GREEN, "Y");
+  print_color(stdout, TT_BRIGHT, TT_BLUE, "Z");
+  print_info(" axes and scale them to ");
+  print_value("n\n");
+  print_info("                     -ax_pos X,Y,Z            = if axes are enabled, set "
+             "their X,Y,Z position in space (default ");
+  print_value("0,0,0");
+  print_info(")\n");
+
+  print_info("\n");
+  print_info("                     -cam (*)                 = use given camera "
+             "settings as initial view\n");
+  print_info(stderr,
+             " (*) [Clipping Range / Focal Point / Position / ViewUp / Distance / "
+             "Field of View Y / Window Size / Window Pos] or use a <filename.cam> that "
+             "contains the same information.\n");
+
+  print_info("\n");
+  print_info("                     -multiview 0/1           = enable/disable "
+             "auto-multi viewport rendering (default ");
+  print_value("disabled");
+  print_info(")\n");
+  print_info("\n");
+
+  print_info("\n");
+  print_info("                     -normals 0/X             = disable/enable the "
+             "display of every Xth point's surface normal as lines (default ");
+  print_value("disabled");
+  print_info(")\n");
+  print_info("                     -normals_scale X         = resize the normal unit "
+             "vector size to X (default ");
+  print_value("0.02");
+  print_info(")\n");
+  print_info("\n");
+  print_info("                     -pc 0/X                  = disable/enable the "
+             "display of every Xth point's principal curvatures as lines (default ");
+  print_value("disabled");
+  print_info(")\n");
+  print_info("                     -pc_scale X              = resize the principal "
+             "curvatures vectors size to X (default ");
+  print_value("0.02");
+  print_info(")\n");
+  print_info("\n");
+
+  print_info("\n(Note: for multiple .pcd files, provide multiple -{fc,ps,opaque} "
+             "parameters; they will be automatically assigned to the right file)\n");
 }
 
 // Global visualizer object
@@ -185,9 +220,9 @@ pcl::visualization::PCLHistogramVisualizer ph_global;
 pcl::visualization::PCLVisualizer::Ptr p;
 
 void
-pp_callback (const pcl::visualization::PointPickingEvent& event, void* cookie)
+pp_callback(const pcl::visualization::PointPickingEvent& event, void* cookie)
 {
-  if (event.getPointIndex () == -1)
+  if (event.getPointIndex() == -1)
     return;
   pcl::PCLPointCloud2::Ptr cloud = *static_cast<pcl::PCLPointCloud2::Ptr*>(cookie);
   if (!cloud)
@@ -195,74 +230,66 @@ pp_callback (const pcl::visualization::PointPickingEvent& event, void* cookie)
 
   // If two points were selected, draw an arrow between them
   pcl::PointXYZ p1, p2;
-  if (event.getPoints (p1.x, p1.y, p1.z, p2.x, p2.y, p2.z) && p)
-  {
+  if (event.getPoints(p1.x, p1.y, p1.z, p2.x, p2.y, p2.z) && p) {
     std::stringstream ss;
     ss << p1 << p2;
-    p->addArrow<pcl::PointXYZ, pcl::PointXYZ> (p1, p2, 1.0, 1.0, 1.0, ss.str ());
+    p->addArrow<pcl::PointXYZ, pcl::PointXYZ>(p1, p2, 1.0, 1.0, 1.0, ss.str());
     return;
   }
 
   // Else, if a single point has been selected
   std::stringstream ss;
-  ss << event.getPointIndex ();
+  ss << event.getPointIndex();
   // Get the cloud's fields
-  for (size_t i = 0; i < cloud->fields.size (); ++i)
-  {
-    if (!isMultiDimensionalFeatureField (cloud->fields[i]))
+  for (size_t i = 0; i < cloud->fields.size(); ++i) {
+    if (!isMultiDimensionalFeatureField(cloud->fields[i]))
       continue;
-    ph_global.addFeatureHistogram (*cloud, cloud->fields[i].name, event.getPointIndex (), ss.str ());
+    ph_global.addFeatureHistogram(
+        *cloud, cloud->fields[i].name, event.getPointIndex(), ss.str());
   }
-  if (p)
-  {
+  if (p) {
     pcl::PointXYZ pos;
-    event.getPoint (pos.x, pos.y, pos.z);
-    p->addText3D<pcl::PointXYZ> (ss.str (), pos, 0.0005, 1.0, 1.0, 1.0, ss.str ());
+    event.getPoint(pos.x, pos.y, pos.z);
+    p->addText3D<pcl::PointXYZ>(ss.str(), pos, 0.0005, 1.0, 1.0, 1.0, ss.str());
   }
-  ph_global.spinOnce ();
+  ph_global.spinOnce();
 }
 
-void capture (Eigen::Isometry3d pose_in)
+void
+capture(Eigen::Isometry3d pose_in)
 {
   // No reference image - but this is kept for compatibility with range_test_v2:
-  float* reference = new float[range_likelihood_->getRowHeight() * range_likelihood_->getColWidth()];
+  float* reference =
+      new float[range_likelihood_->getRowHeight() * range_likelihood_->getColWidth()];
   const float* depth_buffer = range_likelihood_->getDepthBuffer();
   // Copy one image from our last as a reference.
-  for (int i=0, n=0; i<range_likelihood_->getRowHeight(); ++i)
-  {
-    for (int j=0; j<range_likelihood_->getColWidth(); ++j)
-    {
-      reference[n++] = depth_buffer[i*range_likelihood_->getWidth() + j];
+  for (int i = 0, n = 0; i < range_likelihood_->getRowHeight(); ++i) {
+    for (int j = 0; j < range_likelihood_->getColWidth(); ++j) {
+      reference[n++] = depth_buffer[i * range_likelihood_->getWidth() + j];
     }
   }
 
-  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
+  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;
   std::vector<float> scores;
-  poses.push_back (pose_in);
+  poses.push_back(pose_in);
 
-  range_likelihood_->computeLikelihoods (reference, poses, scores);
+  range_likelihood_->computeLikelihoods(reference, poses, scores);
   std::cout << "score: ";
-  for (const float &score : scores)
-  {
+  for (const float& score : scores) {
     std::cout << " " << score;
   }
   std::cout << std::endl;
 
-  std::cout << "camera: " << camera_->getX ()
-       << " " << camera_->getY ()
-       << " " << camera_->getZ ()
-       << " " << camera_->getRoll ()
-       << " " << camera_->getPitch ()
-       << " " << camera_->getYaw ()
-       << std::endl;
-       
-  delete [] reference;
+  std::cout << "camera: " << camera_->getX() << " " << camera_->getY() << " "
+            << camera_->getZ() << " " << camera_->getRoll() << " "
+            << camera_->getPitch() << " " << camera_->getYaw() << std::endl;
 
+  delete[] reference;
 
-  // Benchmark Values for 
+  // Benchmark Values for
   // 27840 triangle faces
   // 13670 vertices
-  
+
   // 45.00Hz: simuation only
   //  1.28Hz: simuation, addNoise?    , getPointCloud, writeASCII
   // 33.33Hz: simuation, getPointCloud
@@ -273,254 +300,265 @@ void capture (Eigen::Isometry3d pose_in)
   // addNoise      0.03	     41%
   // getPointCloud 0.008     11%
   // writeBinary   0.012     16%
-  // total	   0.07222	
+  // total	   0.07222
 
-  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out (new pcl::PointCloud<pcl::PointXYZRGB>);
+  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out(new pcl::PointCloud<pcl::PointXYZRGB>);
 }
 
-
-
-
-
-
-void print_Quaterniond(Eigen::Quaterniond r, std::stringstream &ss){
-  ss <<r.w()<<", "<<r.x()<<", "<<r.y()<<", "<<r.z() ;
+void
+print_Quaterniond(Eigen::Quaterniond r, std::stringstream& ss)
+{
+  ss << r.w() << ", " << r.x() << ", " << r.y() << ", " << r.z();
   //  std::cout << r.str() << "q\n";
 }
 
-
 // Normalize angle to be within the interval [-pi,pi].
-double standardRad(double t) {
+double
+standardRad(double t)
+{
   if (t >= 0.) {
-    t = std::fmod(t+M_PI, 2*M_PI) - M_PI;
-  } else {
-    t = std::fmod(t-M_PI, -2*M_PI) + M_PI;
+    t = std::fmod(t + M_PI, 2 * M_PI) - M_PI;
+  }
+  else {
+    t = std::fmod(t - M_PI, -2 * M_PI) + M_PI;
   }
   return t;
 }
 
-void wRo_to_euler(const Eigen::Matrix3f& wRo, double& yaw, double& pitch, double& roll) {
-  yaw = standardRad(std::atan2(wRo(1,0), wRo(0,0)));
+void
+wRo_to_euler(const Eigen::Matrix3f& wRo, double& yaw, double& pitch, double& roll)
+{
+  yaw = standardRad(std::atan2(wRo(1, 0), wRo(0, 0)));
   double c = std::cos(yaw);
   double s = sin(yaw);
-  pitch = standardRad(std::atan2(static_cast<double> (-wRo(2,0)), wRo(0,0)*c + wRo(1,0)*s));
-  roll  = standardRad(std::atan2(wRo(0,2)*s - wRo(1,2)*c, -wRo(0,1)*s + wRo(1,1)*c));
+  pitch = standardRad(
+      std::atan2(static_cast<double>(-wRo(2, 0)), wRo(0, 0) * c + wRo(1, 0) * s));
+  roll = standardRad(
+      std::atan2(wRo(0, 2) * s - wRo(1, 2) * c, -wRo(0, 1) * s + wRo(1, 1) * c));
 }
 
-void print_Isometry3d(Eigen::Isometry3d pose, std::stringstream &ss){
+void
+print_Isometry3d(Eigen::Isometry3d pose, std::stringstream& ss)
+{
   Eigen::Vector3d t(pose.translation());
   Eigen::Quaterniond r(pose.rotation());
-  ss <<t[0]<<", "<<t[1]<<", "<<t[2]<<" | " 
-       <<r.w()<<", "<<r.x()<<", "<<r.y()<<", "<<r.z() ;
+  ss << t[0] << ", " << t[1] << ", " << t[2] << " | " << r.w() << ", " << r.x() << ", "
+     << r.y() << ", " << r.z();
   //  std::cout << ss.str() << "q\n";
 }
 
-
-
-void simulate_callback (const pcl::visualization::KeyboardEvent &event,
-                        void* viewer_void)
+void
+simulate_callback(const pcl::visualization::KeyboardEvent& event, void* viewer_void)
 {
-  pcl::visualization::PCLVisualizer::Ptr viewer = *static_cast<pcl::visualization::PCLVisualizer::Ptr *> (viewer_void);
+  pcl::visualization::PCLVisualizer::Ptr viewer =
+      *static_cast<pcl::visualization::PCLVisualizer::Ptr*>(viewer_void);
   // I choose v for virtual as s for simulate is takwen
-  if (event.getKeySym () == "v" && event.keyDown ())
-  {
+  if (event.getKeySym() == "v" && event.keyDown()) {
     std::cout << "v was pressed => simulate cloud" << std::endl;
 
     std::vector<pcl::visualization::Camera> cams;
     viewer->getCameras(cams);
-    
-    if (cams.size() !=1){
+
+    if (cams.size() != 1) {
       std::cout << "n cams not 1 exiting\n"; // for now in case ...
-     return; 
+      return;
     }
-   // std::cout << "n cams: " << cams.size() << "\n";
+    // std::cout << "n cams: " << cams.size() << "\n";
     pcl::visualization::Camera cam = cams[0];
-    
-
-
-	      
-	      Eigen::Affine3f pose;
-	      pose = viewer->getViewerPose() ;
-	      
-	      
-     std::cout << cam.pos[0] << " " 
-               << cam.pos[1] << " " 
-               << cam.pos[2] << " p\n";	      
-	      
-	     Eigen::Matrix3f m;
-	     m =pose.rotation();
-	     //All axies use right hand rule. x=red axis, y=green axis, z=blue axis z direction is point into the screen. z \ \ \ -----------> x | | | | | | y 
-	      
- std::cout << pose(0,0)  << " " << pose(0,1) << " " << pose(0,2)  << " " << pose(0,3) << " x0\n";
- std::cout << pose(1,0) << " " << pose(1,1) << " " << pose(1,2) << " " << pose(1,3) << " x1\n";
-  std::cout << pose(2,0) << " " << pose(2,1)  << " " << pose(2,2) << " " << pose(2,3)<< "x2\n";
-
-  double yaw,pitch, roll;
-  wRo_to_euler(m,yaw,pitch,roll);
-  
- printf("RPY: %f %f %f\n", roll*180/M_PI,pitch*180/M_PI,yaw*180/M_PI);    
-
-// matrix->GetElement(1,0);
-  
- std::cout << m(0,0)  << " " << m(0,1) << " " << m(0,2)  << " "  << " x0\n";
- std::cout << m(1,0) << " " << m(1,1) << " " << m(1,2) << " "  << " x1\n";
-  std::cout << m(2,0) << " " << m(2,1)  << " " << m(2,2) << " "<< "x2\n\n";
-  
-  Eigen::Quaternionf rf;
-  rf = Eigen::Quaternionf(m);
-  
-  
-   Eigen::Quaterniond r(rf.w(),rf.x(),rf.y(),rf.z());
-  
-   Eigen::Isometry3d init_pose;
-   init_pose.setIdentity();
-   init_pose.translation() << cam.pos[0], cam.pos[1], cam.pos[2];  
-   //Eigen::Quaterniond m = euler_to_quat(-1.54, 0, 0);
-   init_pose.rotate(r);  
-//   
-  
+
+    Eigen::Affine3f pose;
+    pose = viewer->getViewerPose();
+
+    std::cout << cam.pos[0] << " " << cam.pos[1] << " " << cam.pos[2] << " p\n";
+
+    Eigen::Matrix3f m;
+    m = pose.rotation();
+    // All axies use right hand rule. x=red axis, y=green axis, z=blue axis z direction
+    // is point into the screen. z \ \ \ -----------> x | | | | | | y
+
+    std::cout << pose(0, 0) << " " << pose(0, 1) << " " << pose(0, 2) << " "
+              << pose(0, 3) << " x0\n";
+    std::cout << pose(1, 0) << " " << pose(1, 1) << " " << pose(1, 2) << " "
+              << pose(1, 3) << " x1\n";
+    std::cout << pose(2, 0) << " " << pose(2, 1) << " " << pose(2, 2) << " "
+              << pose(2, 3) << "x2\n";
+
+    double yaw, pitch, roll;
+    wRo_to_euler(m, yaw, pitch, roll);
+
+    printf("RPY: %f %f %f\n", roll * 180 / M_PI, pitch * 180 / M_PI, yaw * 180 / M_PI);
+
+    // matrix->GetElement(1,0);
+
+    std::cout << m(0, 0) << " " << m(0, 1) << " " << m(0, 2) << " "
+              << " x0\n";
+    std::cout << m(1, 0) << " " << m(1, 1) << " " << m(1, 2) << " "
+              << " x1\n";
+    std::cout << m(2, 0) << " " << m(2, 1) << " " << m(2, 2) << " "
+              << "x2\n\n";
+
+    Eigen::Quaternionf rf;
+    rf = Eigen::Quaternionf(m);
+
+    Eigen::Quaterniond r(rf.w(), rf.x(), rf.y(), rf.z());
+
+    Eigen::Isometry3d init_pose;
+    init_pose.setIdentity();
+    init_pose.translation() << cam.pos[0], cam.pos[1], cam.pos[2];
+    // Eigen::Quaterniond m = euler_to_quat(-1.54, 0, 0);
+    init_pose.rotate(r);
+    //
+
     std::stringstream ss;
-  print_Isometry3d(init_pose,ss);
-  std::cout << "init_pose: " << ss.str() << "\n";
-  
-  
-  
-  
-  viewer->addCoordinateSystem (1.0,pose,"reference");
-  
-  
-  
+    print_Isometry3d(init_pose, ss);
+    std::cout << "init_pose: " << ss.str() << "\n";
+
+    viewer->addCoordinateSystem(1.0, pose, "reference");
+
     double tic = getTime();
     std::stringstream ss2;
     ss2.precision(20);
     ss2 << "simulated_pcl_" << tic << ".pcd";
     capture(init_pose);
-    std::cout << (getTime() -tic) << " sec\n";  
+    std::cout << (getTime() - tic) << " sec\n";
   }
 }
 
 // Read in a 3D model
 void
-loadPolygonMeshModel (char* polygon_file)
+loadPolygonMeshModel(char* polygon_file)
 {
-  pcl::PolygonMesh mesh;	// (new pcl::PolygonMesh);
-  //pcl::io::loadPolygonFile("/home/mfallon/data/models/dalet/Darlek_modified_works.obj",mesh);
-  pcl::io::loadPolygonFile (polygon_file, mesh);
-  pcl::PolygonMesh::Ptr cloud (new pcl::PolygonMesh (mesh));
-  
+  pcl::PolygonMesh mesh; // (new pcl::PolygonMesh);
+  // pcl::io::loadPolygonFile("/home/mfallon/data/models/dalet/Darlek_modified_works.obj",mesh);
+  pcl::io::loadPolygonFile(polygon_file, mesh);
+  pcl::PolygonMesh::Ptr cloud(new pcl::PolygonMesh(mesh));
+
   // Not sure if PolygonMesh assumes triangles if to
   // TODO: Ask a developer
-  //PolygonMeshModel::Ptr model = PolygonMeshModel::Ptr (new PolygonMeshModel (GL_POLYGON, cloud));
-  TriangleMeshModel::Ptr model = TriangleMeshModel::Ptr (new TriangleMeshModel (cloud));
-  scene_->add (model);
-  
+  // PolygonMeshModel::Ptr model = PolygonMeshModel::Ptr (new PolygonMeshModel
+  // (GL_POLYGON, cloud));
+  TriangleMeshModel::Ptr model = TriangleMeshModel::Ptr(new TriangleMeshModel(cloud));
+  scene_->add(model);
+
   std::cout << "Just read " << polygon_file << std::endl;
-  std::cout << mesh.polygons.size () << " polygons and "
-	    << mesh.cloud.data.size () << " triangles\n";
+  std::cout << mesh.polygons.size() << " polygons and " << mesh.cloud.data.size()
+            << " triangles\n";
 }
 
 void
-initialize (int, char** argv)
+initialize(int, char** argv)
 {
-  const GLubyte* version = glGetString (GL_VERSION);
+  const GLubyte* version = glGetString(GL_VERSION);
   std::cout << "OpenGL Version: " << version << std::endl;
 
   // works for small files:
   camera_->set(-5.0, 0.0, 1.0, 0.0, 0.0, 0.0);
   pcl::console::print_info("About to read: %s", argv[2]);
-  loadPolygonMeshModel (argv[2]);
+  loadPolygonMeshModel(argv[2]);
 }
 
 int
-main (int argc, char** argv)
+main(int argc, char** argv)
 {
-  srand (time (nullptr));
+  srand(time(nullptr));
 
-  print_info ("The viewer window provides interactive commands; for help, press 'h' or 'H' from within the window.\n");
+  print_info("The viewer window provides interactive commands; for help, press 'h' or "
+             "'H' from within the window.\n");
 
-  if (argc < 2)
-  {
-    printHelp (argc, argv);
+  if (argc < 2) {
+    printHelp(argc, argv);
     return (-1);
   }
 
   // Command line parsing
   double bcolor[3] = {0, 0, 0};
-  pcl::console::parse_3x_arguments (argc, argv, "-bc", bcolor[0], bcolor[1], bcolor[2]);
+  pcl::console::parse_3x_arguments(argc, argv, "-bc", bcolor[0], bcolor[1], bcolor[2]);
 
   std::vector<double> fcolor_r, fcolor_b, fcolor_g;
-  bool fcolorparam = pcl::console::parse_multiple_3x_arguments (argc, argv, "-fc", fcolor_r, fcolor_g, fcolor_b);
+  bool fcolorparam = pcl::console::parse_multiple_3x_arguments(
+      argc, argv, "-fc", fcolor_r, fcolor_g, fcolor_b);
 
   std::vector<int> psize;
-  pcl::console::parse_multiple_arguments (argc, argv, "-ps", psize);
+  pcl::console::parse_multiple_arguments(argc, argv, "-ps", psize);
 
   std::vector<double> opaque;
-  pcl::console::parse_multiple_arguments (argc, argv, "-opaque", opaque);
+  pcl::console::parse_multiple_arguments(argc, argv, "-opaque", opaque);
 
   int mview = 0;
-  pcl::console::parse_argument (argc, argv, "-multiview", mview);
+  pcl::console::parse_argument(argc, argv, "-multiview", mview);
 
   int normals = 0;
-  pcl::console::parse_argument (argc, argv, "-normals", normals);
+  pcl::console::parse_argument(argc, argv, "-normals", normals);
   double normals_scale = NORMALS_SCALE;
-  pcl::console::parse_argument (argc, argv, "-normals_scale", normals_scale);
+  pcl::console::parse_argument(argc, argv, "-normals_scale", normals_scale);
 
   int pc = 0;
-  pcl::console::parse_argument (argc, argv, "-pc", pc);
+  pcl::console::parse_argument(argc, argv, "-pc", pc);
   double pc_scale = PC_SCALE;
-  pcl::console::parse_argument (argc, argv, "-pc_scale", pc_scale);
+  pcl::console::parse_argument(argc, argv, "-pc_scale", pc_scale);
 
   // Parse the command line arguments for .pcd files
-  std::vector<int> p_file_indices   = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
-  std::vector<int> vtk_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".vtk");
+  std::vector<int> p_file_indices =
+      pcl::console::parse_file_extension_argument(argc, argv, ".pcd");
+  std::vector<int> vtk_file_indices =
+      pcl::console::parse_file_extension_argument(argc, argv, ".vtk");
 
-  if (p_file_indices.empty () && vtk_file_indices.empty ())
-  {
-    print_error ("No .PCD or .VTK file given. Nothing to visualize.\n");
+  if (p_file_indices.empty() && vtk_file_indices.empty()) {
+    print_error("No .PCD or .VTK file given. Nothing to visualize.\n");
     return (-1);
   }
 
   // Multiview enabled?
   int x_s = 0;
   double x_step = 0, y_step = 0;
-  if (mview)
-  {
-    print_highlight ("Multi-viewport rendering enabled.\n");
+  if (mview) {
+    print_highlight("Multi-viewport rendering enabled.\n");
 
     int y_s = 0;
-    if (!p_file_indices.empty ())
-    {
-      y_s = static_cast<int>(std::floor (sqrt (static_cast<float>(p_file_indices.size ()))));
-      x_s = y_s + static_cast<int>(std::ceil ((p_file_indices.size () / static_cast<double>(y_s)) - y_s));
-      print_highlight ("Preparing to load "); print_value ("%d", p_file_indices.size ());
+    if (!p_file_indices.empty()) {
+      y_s =
+          static_cast<int>(std::floor(sqrt(static_cast<float>(p_file_indices.size()))));
+      x_s = y_s + static_cast<int>(std::ceil(
+                      (p_file_indices.size() / static_cast<double>(y_s)) - y_s));
+      print_highlight("Preparing to load ");
+      print_value("%d", p_file_indices.size());
     }
-    else if (!vtk_file_indices.empty ())
-    {
-      y_s = static_cast<int>(std::floor (sqrt (static_cast<float>(vtk_file_indices.size ()))));
-      x_s = y_s + static_cast<int>(std::ceil ((vtk_file_indices.size () / static_cast<double>(y_s)) - y_s));
-      print_highlight ("Preparing to load "); print_value ("%d", vtk_file_indices.size ());
+    else if (!vtk_file_indices.empty()) {
+      y_s = static_cast<int>(
+          std::floor(sqrt(static_cast<float>(vtk_file_indices.size()))));
+      x_s = y_s + static_cast<int>(std::ceil(
+                      (vtk_file_indices.size() / static_cast<double>(y_s)) - y_s));
+      print_highlight("Preparing to load ");
+      print_value("%d", vtk_file_indices.size());
     }
 
     x_step = static_cast<double>(1.0 / static_cast<double>(x_s));
     y_step = static_cast<double>(1.0 / static_cast<double>(y_s));
-    print_info (" files ("); print_value ("%d", x_s);    print_info ("x"); print_value ("%d", y_s);
-    print_info (" / ");      print_value ("%f", x_step); print_info ("x"); print_value ("%f", y_step);
-    print_info (")\n");
+    print_info(" files (");
+    print_value("%d", x_s);
+    print_info("x");
+    print_value("%d", y_s);
+    print_info(" / ");
+    print_value("%f", x_step);
+    print_info("x");
+    print_value("%f", y_step);
+    print_info(")\n");
   }
 
   // Fix invalid multiple arguments
-  if (psize.size () != p_file_indices.size () && !psize.empty ())
-    for (size_t i = psize.size (); i < p_file_indices.size (); ++i)
-      psize.push_back (1);
-  if (opaque.size () != p_file_indices.size () && !opaque.empty ())
-    for (size_t i = opaque.size (); i < p_file_indices.size (); ++i)
-      opaque.push_back (1.0);
+  if (psize.size() != p_file_indices.size() && !psize.empty())
+    for (size_t i = psize.size(); i < p_file_indices.size(); ++i)
+      psize.push_back(1);
+  if (opaque.size() != p_file_indices.size() && !opaque.empty())
+    for (size_t i = opaque.size(); i < p_file_indices.size(); ++i)
+      opaque.push_back(1.0);
 
   // Create the PCLHistogramVisualizer object
   pcl::visualization::PCLHistogramVisualizer::Ptr ph;
 
   // Using min_p, max_p to set the global Y min/max range for the histogram
-  float min_p = FLT_MAX; float max_p = -FLT_MAX;
+  float min_p = FLT_MAX;
+  float max_p = -FLT_MAX;
 
   int k = 0, l = 0, viewport = 0;
   // Load the data files
@@ -530,324 +568,385 @@ main (int argc, char** argv)
   GeometryHandlerPtr geometry_handler;
 
   // Go through VTK files
-  for (size_t i = 0; i < vtk_file_indices.size (); ++i)
-  {
+  for (size_t i = 0; i < vtk_file_indices.size(); ++i) {
     // Load file
-    tt.tic ();
-    print_highlight (stderr, "Loading "); print_value (stderr, "%s ", argv[vtk_file_indices.at (i)]);
-    vtkPolyDataReader* reader = vtkPolyDataReader::New ();
-    reader->SetFileName (argv[vtk_file_indices.at (i)]);
-    reader->Update ();
-    vtkSmartPointer<vtkPolyData> polydata = reader->GetOutput ();
+    tt.tic();
+    print_highlight(stderr, "Loading ");
+    print_value(stderr, "%s ", argv[vtk_file_indices.at(i)]);
+    vtkPolyDataReader* reader = vtkPolyDataReader::New();
+    reader->SetFileName(argv[vtk_file_indices.at(i)]);
+    reader->Update();
+    vtkSmartPointer<vtkPolyData> polydata = reader->GetOutput();
     if (!polydata)
       return (-1);
-    print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", polydata->GetNumberOfPoints ()); print_info (" points]\n");
+    print_info("[done, ");
+    print_value("%g", tt.toc());
+    print_info(" ms : ");
+    print_value("%d", polydata->GetNumberOfPoints());
+    print_info(" points]\n");
 
     // Create the PCLVisualizer object here on the first encountered XYZ file
     if (!p)
-      p.reset (new pcl::visualization::PCLVisualizer (argc, argv, "PCD viewer"));
+      p.reset(new pcl::visualization::PCLVisualizer(argc, argv, "PCD viewer"));
 
     // Multiview enabled?
-    if (mview)
-    {
-      p->createViewPort (k * x_step, l * y_step, (k + 1) * x_step, (l + 1) * y_step, viewport);
+    if (mview) {
+      p->createViewPort(
+          k * x_step, l * y_step, (k + 1) * x_step, (l + 1) * y_step, viewport);
       k++;
-      if (k >= x_s)
-      {
+      if (k >= x_s) {
         k = 0;
         l++;
       }
     }
 
     // Add as actor
-    std::stringstream cloud_name ("vtk-");
-    cloud_name << argv[vtk_file_indices.at (i)] << "-" << i;
-    p->addModelFromPolyData (polydata, cloud_name.str (), viewport);
+    std::stringstream cloud_name("vtk-");
+    cloud_name << argv[vtk_file_indices.at(i)] << "-" << i;
+    p->addModelFromPolyData(polydata, cloud_name.str(), viewport);
 
     // Change the shape rendered color
-    if (fcolorparam && fcolor_r.size () > i && fcolor_g.size () > i && fcolor_b.size () > i)
-      p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, fcolor_r[i], fcolor_g[i], fcolor_b[i], cloud_name.str ());
+    if (fcolorparam && fcolor_r.size() > i && fcolor_g.size() > i &&
+        fcolor_b.size() > i)
+      p->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,
+                                     fcolor_r[i],
+                                     fcolor_g[i],
+                                     fcolor_b[i],
+                                     cloud_name.str());
 
     // Change the shape rendered point size
-    if (!psize.empty ())
-      p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at (i), cloud_name.str ());
+    if (!psize.empty())
+      p->setShapeRenderingProperties(
+          pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at(i), cloud_name.str());
 
     // Change the shape rendered opacity
-    if (!opaque.empty ())
-      p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at (i), cloud_name.str ());
+    if (!opaque.empty())
+      p->setShapeRenderingProperties(
+          pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at(i), cloud_name.str());
   }
 
   pcl::PCLPointCloud2::Ptr cloud;
   // Go through PCD files
-  for (size_t i = 0; i < p_file_indices.size (); ++i)
-  {
-    cloud.reset (new pcl::PCLPointCloud2);
+  for (size_t i = 0; i < p_file_indices.size(); ++i) {
+    cloud.reset(new pcl::PCLPointCloud2);
     Eigen::Vector4f origin;
     Eigen::Quaternionf orientation;
     int version;
 
-    print_highlight (stderr, "Loading "); print_value (stderr, "%s ", argv[p_file_indices.at (i)]);
+    print_highlight(stderr, "Loading ");
+    print_value(stderr, "%s ", argv[p_file_indices.at(i)]);
 
-    tt.tic ();
-    if (pcd.read (argv[p_file_indices.at (i)], *cloud, origin, orientation, version) < 0)
+    tt.tic();
+    if (pcd.read(argv[p_file_indices.at(i)], *cloud, origin, orientation, version) < 0)
       return (-1);
 
     std::stringstream cloud_name;
 
     // ---[ Special check for 1-point multi-dimension histograms
-    if (cloud->fields.size () == 1 && isMultiDimensionalFeatureField (cloud->fields[0]))
-    {
-      cloud_name << argv[p_file_indices.at (i)];
+    if (cloud->fields.size() == 1 && isMultiDimensionalFeatureField(cloud->fields[0])) {
+      cloud_name << argv[p_file_indices.at(i)];
 
       if (!ph)
-        ph.reset (new pcl::visualization::PCLHistogramVisualizer);
-      print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud->fields[0].count); print_info (" points]\n");
-
-      pcl::getMinMax (*cloud, 0, cloud->fields[0].name, min_p, max_p);
-      ph->addFeatureHistogram (*cloud, cloud->fields[0].name, cloud_name.str ());
+        ph.reset(new pcl::visualization::PCLHistogramVisualizer);
+      print_info("[done, ");
+      print_value("%g", tt.toc());
+      print_info(" ms : ");
+      print_value("%d", cloud->fields[0].count);
+      print_info(" points]\n");
+
+      pcl::getMinMax(*cloud, 0, cloud->fields[0].name, min_p, max_p);
+      ph->addFeatureHistogram(*cloud, cloud->fields[0].name, cloud_name.str());
       continue;
     }
 
-    cloud_name << argv[p_file_indices.at (i)] << "-" << i;
+    cloud_name << argv[p_file_indices.at(i)] << "-" << i;
 
     // Create the PCLVisualizer object here on the first encountered XYZ file
-    if (!p)
-    {
-      p.reset (new pcl::visualization::PCLVisualizer (argc, argv, "PCD viewer"));
-      p->registerPointPickingCallback (&pp_callback, (void*)&cloud);
+    if (!p) {
+      p.reset(new pcl::visualization::PCLVisualizer(argc, argv, "PCD viewer"));
+      p->registerPointPickingCallback(&pp_callback, (void*)&cloud);
       Eigen::Matrix3f rotation;
       rotation = orientation;
-      p->setCameraPosition (origin [0]                  , origin [1]                  , origin [2],
-                        origin [0] + rotation (0, 2), origin [1] + rotation (1, 2), origin [2] + rotation (2, 2),
-                                     rotation (0, 1),              rotation (1, 1),              rotation (2, 1));
+      p->setCameraPosition(origin[0],
+                           origin[1],
+                           origin[2],
+                           origin[0] + rotation(0, 2),
+                           origin[1] + rotation(1, 2),
+                           origin[2] + rotation(2, 2),
+                           rotation(0, 1),
+                           rotation(1, 1),
+                           rotation(2, 1));
     }
 
     // Multiview enabled?
-    if (mview)
-    {
-      p->createViewPort (k * x_step, l * y_step, (k + 1) * x_step, (l + 1) * y_step, viewport);
+    if (mview) {
+      p->createViewPort(
+          k * x_step, l * y_step, (k + 1) * x_step, (l + 1) * y_step, viewport);
       k++;
-      if (k >= x_s)
-      {
+      if (k >= x_s) {
         k = 0;
         l++;
       }
     }
 
-    if (cloud->width * cloud->height == 0)
-    {
-      print_error ("[error: no points found!]\n");
+    if (cloud->width * cloud->height == 0) {
+      print_error("[error: no points found!]\n");
       return (-1);
     }
-    print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", (int)cloud->width * cloud->height); print_info (" points]\n");
-    print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (*cloud).c_str ());
+    print_info("[done, ");
+    print_value("%g", tt.toc());
+    print_info(" ms : ");
+    print_value("%d", (int)cloud->width * cloud->height);
+    print_info(" points]\n");
+    print_info("Available dimensions: ");
+    print_value("%s\n", pcl::getFieldsList(*cloud).c_str());
 
     // If no color was given, get random colors
-    if (fcolorparam)
-    {
-      if (fcolor_r.size () > i && fcolor_g.size () > i && fcolor_b.size () > i)
-        color_handler.reset (new pcl::visualization::PointCloudColorHandlerCustom<pcl::PCLPointCloud2> (cloud, fcolor_r[i], fcolor_g[i], fcolor_b[i]));
+    if (fcolorparam) {
+      if (fcolor_r.size() > i && fcolor_g.size() > i && fcolor_b.size() > i)
+        color_handler.reset(
+            new pcl::visualization::PointCloudColorHandlerCustom<pcl::PCLPointCloud2>(
+                cloud, fcolor_r[i], fcolor_g[i], fcolor_b[i]));
       else
-        color_handler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (cloud));
+        color_handler.reset(
+            new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2>(
+                cloud));
     }
     else
-      color_handler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (cloud));
+      color_handler.reset(
+          new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2>(
+              cloud));
 
     // Add the dataset with a XYZ and a random handler
-    geometry_handler.reset (new pcl::visualization::PointCloudGeometryHandlerXYZ<pcl::PCLPointCloud2> (cloud));
+    geometry_handler.reset(
+        new pcl::visualization::PointCloudGeometryHandlerXYZ<pcl::PCLPointCloud2>(
+            cloud));
     // Add the cloud to the renderer
-    //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, color_handler, cloud_name.str (), viewport);
-    p->addPointCloud (cloud, geometry_handler, color_handler, origin, orientation, cloud_name.str (), viewport);
+    // p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, color_handler,
+    // cloud_name.str (), viewport);
+    p->addPointCloud(cloud,
+                     geometry_handler,
+                     color_handler,
+                     origin,
+                     orientation,
+                     cloud_name.str(),
+                     viewport);
 
     // If normal lines are enabled
-    if (normals != 0)
-    {
-      int normal_idx = pcl::getFieldIndex (*cloud, "normal_x");
-      if (normal_idx == -1)
-      {
-        print_error ("Normal information requested but not available.\n");
+    if (normals != 0) {
+      int normal_idx = pcl::getFieldIndex(*cloud, "normal_x");
+      if (normal_idx == -1) {
+        print_error("Normal information requested but not available.\n");
         continue;
-        //return (-1);
+        // return (-1);
       }
       //
       // Convert from blob to pcl::PointCloud
-      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
-      pcl::fromPCLPointCloud2 (*cloud, *cloud_xyz);
+      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);
+      pcl::fromPCLPointCloud2(*cloud, *cloud_xyz);
       cloud_xyz->sensor_origin_ = origin;
       cloud_xyz->sensor_orientation_ = orientation;
 
-      pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
-      pcl::fromPCLPointCloud2 (*cloud, *cloud_normals);
+      pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
+      pcl::fromPCLPointCloud2(*cloud, *cloud_normals);
       std::stringstream cloud_name_normals;
-      cloud_name_normals << argv[p_file_indices.at (i)] << "-" << i << "-normals";
-      p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, normals, normals_scale, cloud_name_normals.str (), viewport);
+      cloud_name_normals << argv[p_file_indices.at(i)] << "-" << i << "-normals";
+      p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud_xyz,
+                                                          cloud_normals,
+                                                          normals,
+                                                          normals_scale,
+                                                          cloud_name_normals.str(),
+                                                          viewport);
     }
 
     // If principal curvature lines are enabled
-    if (pc != 0)
-    {
+    if (pc != 0) {
       if (normals == 0)
         normals = pc;
 
-      int normal_idx = pcl::getFieldIndex (*cloud, "normal_x");
-      if (normal_idx == -1)
-      {
-        print_error ("Normal information requested but not available.\n");
+      int normal_idx = pcl::getFieldIndex(*cloud, "normal_x");
+      if (normal_idx == -1) {
+        print_error("Normal information requested but not available.\n");
         continue;
-        //return (-1);
+        // return (-1);
       }
-      int pc_idx = pcl::getFieldIndex (*cloud, "principal_curvature_x");
-      if (pc_idx == -1)
-      {
-        print_error ("Principal Curvature information requested but not available.\n");
+      int pc_idx = pcl::getFieldIndex(*cloud, "principal_curvature_x");
+      if (pc_idx == -1) {
+        print_error("Principal Curvature information requested but not available.\n");
         continue;
-        //return (-1);
+        // return (-1);
       }
       //
       // Convert from blob to pcl::PointCloud
-      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
-      pcl::fromPCLPointCloud2 (*cloud, *cloud_xyz);
+      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);
+      pcl::fromPCLPointCloud2(*cloud, *cloud_xyz);
       cloud_xyz->sensor_origin_ = origin;
       cloud_xyz->sensor_orientation_ = orientation;
-      pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
-      pcl::fromPCLPointCloud2 (*cloud, *cloud_normals);
-      pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr cloud_pc (new pcl::PointCloud<pcl::PrincipalCurvatures>);
-      pcl::fromPCLPointCloud2 (*cloud, *cloud_pc);
+      pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
+      pcl::fromPCLPointCloud2(*cloud, *cloud_normals);
+      pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr cloud_pc(
+          new pcl::PointCloud<pcl::PrincipalCurvatures>);
+      pcl::fromPCLPointCloud2(*cloud, *cloud_pc);
       std::stringstream cloud_name_normals_pc;
-      cloud_name_normals_pc << argv[p_file_indices.at (i)] << "-" << i << "-normals";
+      cloud_name_normals_pc << argv[p_file_indices.at(i)] << "-" << i << "-normals";
       int factor = (std::min)(normals, pc);
-      p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, factor, normals_scale, cloud_name_normals_pc.str (), viewport);
-      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, cloud_name_normals_pc.str ());
-      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc.str ());
+      p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud_xyz,
+                                                          cloud_normals,
+                                                          factor,
+                                                          normals_scale,
+                                                          cloud_name_normals_pc.str(),
+                                                          viewport);
+      p->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,
+                                          1.0,
+                                          0.0,
+                                          0.0,
+                                          cloud_name_normals_pc.str());
+      p->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH,
+                                          3,
+                                          cloud_name_normals_pc.str());
       cloud_name_normals_pc << "-pc";
-      p->addPointCloudPrincipalCurvatures<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, cloud_pc, factor, pc_scale, cloud_name_normals_pc.str (), viewport);
-      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc.str ());
+      p->addPointCloudPrincipalCurvatures<pcl::PointXYZ, pcl::Normal>(
+          cloud_xyz,
+          cloud_normals,
+          cloud_pc,
+          factor,
+          pc_scale,
+          cloud_name_normals_pc.str(),
+          viewport);
+      p->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH,
+                                          3,
+                                          cloud_name_normals_pc.str());
     }
 
     // Add every dimension as a possible color
-    if (!fcolorparam)
-    {
-      for (size_t f = 0; f < cloud->fields.size (); ++f)
-      {
+    if (!fcolorparam) {
+      for (size_t f = 0; f < cloud->fields.size(); ++f) {
         if (cloud->fields[f].name == "rgb" || cloud->fields[f].name == "rgba")
-          color_handler.reset (new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PCLPointCloud2> (cloud));
-        else
-        {
-          if (!isValidFieldName (cloud->fields[f].name))
+          color_handler.reset(new pcl::visualization::PointCloudColorHandlerRGBField<
+                              pcl::PCLPointCloud2>(cloud));
+        else {
+          if (!isValidFieldName(cloud->fields[f].name))
             continue;
-          color_handler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (cloud, cloud->fields[f].name));
+          color_handler.reset(
+              new pcl::visualization::PointCloudColorHandlerGenericField<
+                  pcl::PCLPointCloud2>(cloud, cloud->fields[f].name));
         }
         // Add the cloud to the renderer
-        //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, color_handler, cloud_name.str (), viewport);
-        p->addPointCloud (cloud, color_handler, origin, orientation, cloud_name.str (), viewport);
+        // p->addPointCloud<pcl::PointXYZ> (cloud_xyz, color_handler, cloud_name.str (),
+        // viewport);
+        p->addPointCloud(
+            cloud, color_handler, origin, orientation, cloud_name.str(), viewport);
       }
     }
     // Additionally, add normals as a handler
-    geometry_handler.reset (new pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<pcl::PCLPointCloud2> (cloud));
-    if (geometry_handler->isCapable ())
-      //p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, cloud_name.str (), viewport);
-      p->addPointCloud (cloud, geometry_handler, origin, orientation, cloud_name.str (), viewport);
+    geometry_handler.reset(
+        new pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<
+            pcl::PCLPointCloud2>(cloud));
+    if (geometry_handler->isCapable())
+      // p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, cloud_name.str
+      // (), viewport);
+      p->addPointCloud(
+          cloud, geometry_handler, origin, orientation, cloud_name.str(), viewport);
 
     // Set immediate mode rendering ON
-    p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_IMMEDIATE_RENDERING, 1.0, cloud_name.str ());
+    p->setPointCloudRenderingProperties(
+        pcl::visualization::PCL_VISUALIZER_IMMEDIATE_RENDERING, 1.0, cloud_name.str());
 
     // Change the cloud rendered point size
-    if (!psize.empty ())
-      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at (i), cloud_name.str ());
+    if (!psize.empty())
+      p->setPointCloudRenderingProperties(
+          pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at(i), cloud_name.str());
 
     // Change the cloud rendered opacity
-    if (!opaque.empty ())
-      p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at (i), cloud_name.str ());
-
-    // Reset camera viewpoint to center of cloud if camera parameters were not passed manually and this is the first loaded cloud
-    //if (i == 0 && !p->cameraParamsSet ())
-     // p->resetCameraViewpoint (cloud_name.str ());
+    if (!opaque.empty())
+      p->setPointCloudRenderingProperties(
+          pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at(i), cloud_name.str());
+
+    // Reset camera viewpoint to center of cloud if camera parameters were not passed
+    // manually and this is the first loaded cloud
+    // if (i == 0 && !p->cameraParamsSet ())
+    // p->resetCameraViewpoint (cloud_name.str ());
   }
-  
-  
+
   ////////////////////////////////////////////////////////////////
   // Key binding for saving simulated point cloud:
   if (p)
     p->registerKeyboardCallback(simulate_callback, (void*)&p);
-  
-  
-  
-  
+
   int width = 640;
   int height = 480;
   window_width_ = width * 2;
   window_height_ = height * 2;
 
-  print_info ("Manually generate a simulated RGB-D point cloud using pcl::simulation. For more information, use: %s -h\n", argv[0]);
+  print_info("Manually generate a simulated RGB-D point cloud using pcl::simulation. "
+             "For more information, use: %s -h\n",
+             argv[0]);
 
-  for (int i=0; i<2048; i++)
-  {
-    float v = i/2048.0;
-    v = powf(v, 3)* 6;
-    t_gamma[i] = v*6*256;
-  }    
+  for (int i = 0; i < 2048; i++) {
+    float v = i / 2048.0;
+    v = powf(v, 3) * 6;
+    t_gamma[i] = v * 6 * 256;
+  }
 
-  GLenum err = glewInit ();
-  if (GLEW_OK != err)
-  {
-    std::cerr << "Error: " << glewGetErrorString (err) << std::endl;
-    exit (-1);
+  GLenum err = glewInit();
+  if (GLEW_OK != err) {
+    std::cerr << "Error: " << glewGetErrorString(err) << std::endl;
+    exit(-1);
   }
 
-  std::cout << "Status: Using GLEW " << glewGetString (GLEW_VERSION) << std::endl;
+  std::cout << "Status: Using GLEW " << glewGetString(GLEW_VERSION) << std::endl;
 
-  if (glewIsSupported ("GL_VERSION_2_0"))
+  if (glewIsSupported("GL_VERSION_2_0"))
     std::cout << "OpenGL 2.0 supported" << std::endl;
-  else
-  {
+  else {
     std::cerr << "Error: OpenGL 2.0 not supported" << std::endl;
     exit(1);
   }
 
+  camera_ = Camera::Ptr(new Camera());
+  scene_ = Scene::Ptr(new Scene());
 
-  camera_ = Camera::Ptr (new Camera ());
-  scene_ = Scene::Ptr (new Scene ());
-
-  range_likelihood_ = RangeLikelihood::Ptr (new RangeLikelihood(1, 1, height, width, scene_));
+  range_likelihood_ =
+      RangeLikelihood::Ptr(new RangeLikelihood(1, 1, height, width, scene_));
 
-  // range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(10, 10, 96, 96, scene_));
-  // range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, 480, 640, scene_));
+  // range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(10, 10, 96, 96,
+  // scene_)); range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, 480,
+  // 640, scene_));
 
   // Actually corresponds to default parameters:
-  range_likelihood_->setCameraIntrinsicsParameters (640,480, 576.09757860,
-            576.09757860, 321.06398107, 242.97676897);
-  range_likelihood_->setComputeOnCPU (false);
-  range_likelihood_->setSumOnCPU (true);
-  range_likelihood_->setUseColor (true);
-  initialize (argc, argv); 
+  range_likelihood_->setCameraIntrinsicsParameters(
+      640, 480, 576.09757860, 576.09757860, 321.06398107, 242.97676897);
+  range_likelihood_->setComputeOnCPU(false);
+  range_likelihood_->setSumOnCPU(true);
+  range_likelihood_->setUseColor(true);
+  initialize(argc, argv);
 
   if (p)
-    p->setBackgroundColor (bcolor[0], bcolor[1], bcolor[2]);
+    p->setBackgroundColor(bcolor[0], bcolor[1], bcolor[2]);
   // Read axes settings
-  double axes  = 0.0;
-  pcl::console::parse_argument (argc, argv, "-ax", axes);
-  if (axes != 0.0 && p)
-  {
+  double axes = 0.0;
+  pcl::console::parse_argument(argc, argv, "-ax", axes);
+  if (axes != 0.0 && p) {
     double ax_x = 0.0, ax_y = 0.0, ax_z = 0.0;
-    pcl::console::parse_3x_arguments (argc, argv, "-ax_pos", ax_x, ax_y, ax_z, false);
+    pcl::console::parse_3x_arguments(argc, argv, "-ax_pos", ax_x, ax_y, ax_z, false);
     // Draw XYZ axes if command-line enabled
-    p->addCoordinateSystem (axes, ax_x, ax_y, ax_z, "reference");
+    p->addCoordinateSystem(axes, ax_x, ax_y, ax_z, "reference");
   }
 
   // Clean up the memory used by the binary blob
   // Note: avoid resetting the cloud, otherwise the PointPicking callback will fail
-  //cloud.reset ();
+  // cloud.reset ();
 
-  if (ph)
-  {
-    print_highlight ("Setting the global Y range for all histograms to: "); print_value ("%f -> %f\n", min_p, max_p);
-    ph->setGlobalYRange (min_p, max_p);
-    ph->updateWindowPositions ();
+  if (ph) {
+    print_highlight("Setting the global Y range for all histograms to: ");
+    print_value("%f -> %f\n", min_p, max_p);
+    ph->setGlobalYRange(min_p, max_p);
+    ph->updateWindowPositions();
     if (p)
-      p->spin ();
+      p->spin();
     else
-      ph->spin ();
+      ph->spin();
   }
   else if (p)
-    p->spin ();
+    p->spin();
 }
diff --git a/simulation/tools/simulation_io.cpp b/simulation/tools/simulation_io.cpp
index 7d4f39cb623..f0b8e747892 100644
--- a/simulation/tools/simulation_io.cpp
+++ b/simulation/tools/simulation_io.cpp
@@ -1,274 +1,260 @@
 #include "simulation_io.hpp"
 #include <pcl/io/png_io.h>
 
+pcl::simulation::SimExample::SimExample(int argc, char** argv, int height, int width)
+: height_(height), width_(width)
+{
 
+  initializeGL(argc, argv);
 
-pcl::simulation::SimExample::SimExample(int argc, char** argv,
-	int height,int width):
-        height_(height), width_(width){
-
-  initializeGL (argc, argv);
-  
   // 1. construct member elements:
-  camera_ = Camera::Ptr (new Camera ());
-  scene_ = Scene::Ptr (new Scene ());
+  camera_ = Camera::Ptr(new Camera());
+  scene_ = Scene::Ptr(new Scene());
 
-  //rl_ = RangeLikelihoodGLSL::Ptr(new RangeLikelihoodGLSL(1, 1, height, width, scene_, 0));
-  rl_ = RangeLikelihood::Ptr (new RangeLikelihood (1, 1, height, width, scene_));
+  // rl_ = RangeLikelihoodGLSL::Ptr(new RangeLikelihoodGLSL(1, 1, height, width, scene_,
+  // 0));
+  rl_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, height, width, scene_));
   // rl_ = RangeLikelihood::Ptr(new RangeLikelihood(10, 10, 96, 96, scene_));
   // rl_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, height_, width_, scene_));
 
   // Actually corresponds to default parameters:
-  rl_->setCameraIntrinsicsParameters (width_,height_, 576.09757860,
-            576.09757860, 321.06398107, 242.97676897);
-  rl_->setComputeOnCPU (false);
-  rl_->setSumOnCPU (true);
-  rl_->setUseColor (true);  
-  
+  rl_->setCameraIntrinsicsParameters(
+      width_, height_, 576.09757860, 576.09757860, 321.06398107, 242.97676897);
+  rl_->setComputeOnCPU(false);
+  rl_->setSumOnCPU(true);
+  rl_->setUseColor(true);
+
   // 2. read mesh and setup model:
   std::cout << "About to read: " << argv[2] << std::endl;
-  pcl::PolygonMesh mesh;	// (new pcl::PolygonMesh);
-  pcl::io::loadPolygonFile (argv[2], mesh);
-  pcl::PolygonMesh::Ptr cloud (new pcl::PolygonMesh (mesh));
-  
+  pcl::PolygonMesh mesh; // (new pcl::PolygonMesh);
+  pcl::io::loadPolygonFile(argv[2], mesh);
+  pcl::PolygonMesh::Ptr cloud(new pcl::PolygonMesh(mesh));
+
   // Not sure if PolygonMesh assumes triangles if to, TODO: Ask a developer
-  PolygonMeshModel::Ptr model = PolygonMeshModel::Ptr (new PolygonMeshModel (GL_POLYGON, cloud));
-  scene_->add (model);
-  
+  PolygonMeshModel::Ptr model =
+      PolygonMeshModel::Ptr(new PolygonMeshModel(GL_POLYGON, cloud));
+  scene_->add(model);
+
   std::cout << "Just read " << argv[2] << std::endl;
-  std::cout << mesh.polygons.size () << " polygons and "
-	    << mesh.cloud.data.size () << " triangles\n";  
-	    
+  std::cout << mesh.polygons.size() << " polygons and " << mesh.cloud.data.size()
+            << " triangles\n";
+
   // works well for MIT CSAIL model 3rd floor:
-  //camera_->set(4.04454, 44.9377, 1.1, 0.0, 0.0, -2.00352);
+  // camera_->set(4.04454, 44.9377, 1.1, 0.0, 0.0, -2.00352);
 
   // works well for MIT CSAIL model 2nd floor:
-//  camera_->set (27.4503, 37.383, 4.30908, 0.0, 0.0654498, -2.25802);
+  //  camera_->set (27.4503, 37.383, 4.30908, 0.0, 0.0654498, -2.25802);
 
   // works for small files:
-  //camera_->set(-5.0, 0.0, 1.0, 0.0, 0.0, 0.0);
+  // camera_->set(-5.0, 0.0, 1.0, 0.0, 0.0, 0.0);
   camera_->set(0.471703, 1.59862, 3.10937, 0, 0.418879, -12.2129);
   camera_->setPitch(0.418879); // not sure why this is here:
-  
-  for (int i=0; i<2048; i++)
-  {
-    float v = i/2048.0;
-    v = powf(v, 3)* 6;
-    t_gamma[i] = v*6*256;
-  }  
-}
-
-
 
+  for (int i = 0; i < 2048; i++) {
+    float v = i / 2048.0;
+    v = powf(v, 3) * 6;
+    t_gamma[i] = v * 6 * 256;
+  }
+}
 
-void 
-pcl::simulation::SimExample::initializeGL (int argc, char** argv)
+void
+pcl::simulation::SimExample::initializeGL(int argc, char** argv)
 {
-  glutInit (&argc, argv);
-  glutInitDisplayMode (GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);// was GLUT_RGBA
-  glutInitWindowPosition (10, 10);
-  glutInitWindowSize (10, 10);
-  //glutInitWindowSize (window_width_, window_height_);
-  glutCreateWindow ("OpenGL range likelihood");
-
-  GLenum err = glewInit ();
-  if (GLEW_OK != err)
-  {
-    std::cerr << "Error: " << glewGetErrorString (err) << std::endl;
-    exit (-1);
+  glutInit(&argc, argv);
+  glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB); // was GLUT_RGBA
+  glutInitWindowPosition(10, 10);
+  glutInitWindowSize(10, 10);
+  // glutInitWindowSize (window_width_, window_height_);
+  glutCreateWindow("OpenGL range likelihood");
+
+  GLenum err = glewInit();
+  if (GLEW_OK != err) {
+    std::cerr << "Error: " << glewGetErrorString(err) << std::endl;
+    exit(-1);
   }
 
-  std::cout << "Status: Using GLEW " << glewGetString (GLEW_VERSION) << std::endl;
-  if (glewIsSupported ("GL_VERSION_2_0"))
+  std::cout << "Status: Using GLEW " << glewGetString(GLEW_VERSION) << std::endl;
+  if (glewIsSupported("GL_VERSION_2_0"))
     std::cout << "OpenGL 2.0 supported" << std::endl;
-  else
-  {
+  else {
     std::cerr << "Error: OpenGL 2.0 not supported" << std::endl;
     exit(1);
   }
-  
+
   std::cout << "GL_MAX_VIEWPORTS: " << GL_MAX_VIEWPORTS << std::endl;
-  const GLubyte* version = glGetString (GL_VERSION);
-  std::cout << "OpenGL Version: " << version << std::endl;  
+  const GLubyte* version = glGetString(GL_VERSION);
+  std::cout << "OpenGL Version: " << version << std::endl;
 }
 
-
-
 void
-pcl::simulation::SimExample::doSim (Eigen::Isometry3d pose_in)
+pcl::simulation::SimExample::doSim(Eigen::Isometry3d pose_in)
 {
   // No reference image - but this is kept for compatibility with range_test_v2:
   float* reference = new float[rl_->getRowHeight() * rl_->getColWidth()];
   const float* depth_buffer = rl_->getDepthBuffer();
   // Copy one image from our last as a reference.
-  for (int i=0, n=0; i<rl_->getRowHeight(); ++i)
-  {
-    for (int j=0; j<rl_->getColWidth(); ++j)
-    {
-      reference[n++] = depth_buffer[i*rl_->getWidth() + j];
+  for (int i = 0, n = 0; i < rl_->getRowHeight(); ++i) {
+    for (int j = 0; j < rl_->getColWidth(); ++j) {
+      reference[n++] = depth_buffer[i * rl_->getWidth() + j];
     }
   }
 
-  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
+  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;
   std::vector<float> scores;
-  poses.push_back (pose_in);
-  rl_->computeLikelihoods (reference, poses, scores);
-  std::cout << "camera: " << camera_->getX ()
-       << " " << camera_->getY ()
-       << " " << camera_->getZ ()
-       << " " << camera_->getRoll ()
-       << " " << camera_->getPitch ()
-       << " " << camera_->getYaw ()
-       << std::endl;
-       
-  delete [] reference;
-}
-
+  poses.push_back(pose_in);
+  rl_->computeLikelihoods(reference, poses, scores);
+  std::cout << "camera: " << camera_->getX() << " " << camera_->getY() << " "
+            << camera_->getZ() << " " << camera_->getRoll() << " "
+            << camera_->getPitch() << " " << camera_->getYaw() << std::endl;
 
+  delete[] reference;
+}
 
 void
-pcl::simulation::SimExample::write_score_image(const float* score_buffer, std::string fname)
+pcl::simulation::SimExample::write_score_image(const float* score_buffer,
+                                               std::string fname)
 {
   int npixels = rl_->getWidth() * rl_->getHeight();
   uint8_t* score_img = new uint8_t[npixels * 3];
 
   float min_score = score_buffer[0];
   float max_score = score_buffer[0];
-  for (int i=1; i<npixels; i++)
-  {
-    if (score_buffer[i] < min_score) min_score = score_buffer[i];
-    if (score_buffer[i] > max_score) max_score = score_buffer[i];
+  for (int i = 1; i < npixels; i++) {
+    if (score_buffer[i] < min_score)
+      min_score = score_buffer[i];
+    if (score_buffer[i] > max_score)
+      max_score = score_buffer[i];
   }
 
-  for (int y = 0; y <  height_; ++y)
-  {
-    for (int x = 0; x < width_; ++x)
-    {
-      int i = y*width_ + x ;
-      int i_in= (height_-1 -y) *width_ + x ; // flip up
-
-      float d = (score_buffer[i_in]-min_score)/(max_score-min_score);
-      score_img[3*i+0] = 0;
-      score_img[3*i+1] = d*255;
-      score_img[3*i+2] = 0;
+  for (int y = 0; y < height_; ++y) {
+    for (int x = 0; x < width_; ++x) {
+      int i = y * width_ + x;
+      int i_in = (height_ - 1 - y) * width_ + x; // flip up
+
+      float d = (score_buffer[i_in] - min_score) / (max_score - min_score);
+      score_img[3 * i + 0] = 0;
+      score_img[3 * i + 1] = d * 255;
+      score_img[3 * i + 2] = 0;
     }
   }
 
   // Write to file:
-  pcl::io::saveRgbPNGFile (fname, score_img, width_, height_);
-  
-  delete [] score_img;
+  pcl::io::saveRgbPNGFile(fname, score_img, width_, height_);
+
+  delete[] score_img;
 }
 
 void
-pcl::simulation::SimExample::write_depth_image(const float* depth_buffer, std::string fname)
+pcl::simulation::SimExample::write_depth_image(const float* depth_buffer,
+                                               std::string fname)
 {
   int npixels = rl_->getWidth() * rl_->getHeight();
   uint8_t* depth_img = new uint8_t[npixels * 3];
 
   float min_depth = depth_buffer[0];
   float max_depth = depth_buffer[0];
-  for (int i=1; i<npixels; i++)
-  {
-    if (depth_buffer[i] < min_depth) min_depth = depth_buffer[i];
-    if (depth_buffer[i] > max_depth) max_depth = depth_buffer[i];
+  for (int i = 1; i < npixels; i++) {
+    if (depth_buffer[i] < min_depth)
+      min_depth = depth_buffer[i];
+    if (depth_buffer[i] > max_depth)
+      max_depth = depth_buffer[i];
   }
 
-  for (int y = 0; y <  height_; ++y)
-  {
-    for (int x = 0; x < width_; ++x)
-    {
-      int i= y*width_ + x ;
-      int i_in= (height_-1 -y) *width_ + x ; // flip up down
-    
+  for (int y = 0; y < height_; ++y) {
+    for (int x = 0; x < width_; ++x) {
+      int i = y * width_ + x;
+      int i_in = (height_ - 1 - y) * width_ + x; // flip up down
+
       float zn = 0.7;
       float zf = 20.0;
       float d = depth_buffer[i_in];
-      float z = -zf*zn/((zf-zn)*(d - zf/(zf-zn)));
+      float z = -zf * zn / ((zf - zn) * (d - zf / (zf - zn)));
       float b = 0.075;
       float f = 580.0;
-      uint16_t kd = static_cast<uint16_t>(1090 - b*f/z*8);
-      if (kd>2047) kd = 2047;
+      uint16_t kd = static_cast<uint16_t>(1090 - b * f / z * 8);
+      if (kd > 2047)
+        kd = 2047;
 
       int pval = t_gamma[kd];
       int lb = pval & 0xff;
-      switch (pval>>8) {
-	case 0:
-	    depth_img[3*i+0] = 255;
-	    depth_img[3*i+1] = 255-lb;
-	    depth_img[3*i+2] = 255-lb;
-	    break;
-	case 1:
-	    depth_img[3*i+0] = 255;
-	    depth_img[3*i+1] = lb;
-	    depth_img[3*i+2] = 0;
-	    break;
-	case 2:
-	    depth_img[3*i+0] = 255-lb;
-	    depth_img[3*i+1] = 255;
-	    depth_img[3*i+2] = 0;
-	    break;
-	case 3:
-	    depth_img[3*i+0] = 0;
-	    depth_img[3*i+1] = 255;
-	    depth_img[3*i+2] = lb;
-	    break;
-	case 4:
-	    depth_img[3*i+0] = 0;
-	    depth_img[3*i+1] = 255-lb;
-	    depth_img[3*i+2] = 255;
-	    break;
-	case 5:
-	    depth_img[3*i+0] = 0;
-	    depth_img[3*i+1] = 0;
-	    depth_img[3*i+2] = 255-lb;
-	    break;
-	default:
-	    depth_img[3*i+0] = 0;
-	    depth_img[3*i+1] = 0;
-	    depth_img[3*i+2] = 0;
-	    break;
+      switch (pval >> 8) {
+      case 0:
+        depth_img[3 * i + 0] = 255;
+        depth_img[3 * i + 1] = 255 - lb;
+        depth_img[3 * i + 2] = 255 - lb;
+        break;
+      case 1:
+        depth_img[3 * i + 0] = 255;
+        depth_img[3 * i + 1] = lb;
+        depth_img[3 * i + 2] = 0;
+        break;
+      case 2:
+        depth_img[3 * i + 0] = 255 - lb;
+        depth_img[3 * i + 1] = 255;
+        depth_img[3 * i + 2] = 0;
+        break;
+      case 3:
+        depth_img[3 * i + 0] = 0;
+        depth_img[3 * i + 1] = 255;
+        depth_img[3 * i + 2] = lb;
+        break;
+      case 4:
+        depth_img[3 * i + 0] = 0;
+        depth_img[3 * i + 1] = 255 - lb;
+        depth_img[3 * i + 2] = 255;
+        break;
+      case 5:
+        depth_img[3 * i + 0] = 0;
+        depth_img[3 * i + 1] = 0;
+        depth_img[3 * i + 2] = 255 - lb;
+        break;
+      default:
+        depth_img[3 * i + 0] = 0;
+        depth_img[3 * i + 1] = 0;
+        depth_img[3 * i + 2] = 0;
+        break;
       }
     }
   }
 
   // Write to file:
-  pcl::io::saveRgbPNGFile (fname, depth_img, width_, height_);
-  
-  delete [] depth_img;
-}
+  pcl::io::saveRgbPNGFile(fname, depth_img, width_, height_);
 
+  delete[] depth_img;
+}
 
 void
-pcl::simulation::SimExample::write_depth_image_uint(const float* depth_buffer, std::string fname)
+pcl::simulation::SimExample::write_depth_image_uint(const float* depth_buffer,
+                                                    std::string fname)
 {
   int npixels = rl_->getWidth() * rl_->getHeight();
-  unsigned short * depth_img = new unsigned short[npixels ];
+  unsigned short* depth_img = new unsigned short[npixels];
 
   float min_depth = depth_buffer[0];
   float max_depth = depth_buffer[0];
-  for (int i=1; i<npixels; i++)
-  {
-    if (depth_buffer[i] < min_depth) min_depth = depth_buffer[i];
-    if (depth_buffer[i] > max_depth) max_depth = depth_buffer[i];
+  for (int i = 1; i < npixels; i++) {
+    if (depth_buffer[i] < min_depth)
+      min_depth = depth_buffer[i];
+    if (depth_buffer[i] > max_depth)
+      max_depth = depth_buffer[i];
   }
 
-  for (int y = 0; y <  height_; ++y)
-  {
-    for (int x = 0; x < width_; ++x)
-    {
-      int i= y*width_ + x ;
-      int i_in= (height_-1 -y) *width_ + x ; // flip up down
-    
+  for (int y = 0; y < height_; ++y) {
+    for (int x = 0; x < width_; ++x) {
+      int i = y * width_ + x;
+      int i_in = (height_ - 1 - y) * width_ + x; // flip up down
+
       float zn = 0.7;
       float zf = 20.0;
       float d = depth_buffer[i_in];
-      
-      unsigned short z_new = (unsigned short)  std::floor( 1000*( -zf*zn/((zf-zn)*(d - zf/(zf-zn)))));
-      if (z_new>65535) z_new = 65535;
-      
-      if ( z_new < 18000){
-	  std::cout << z_new << " " << d << " " << x << "\n";  
+
+      unsigned short z_new = (unsigned short)std::floor(
+          1000 * (-zf * zn / ((zf - zn) * (d - zf / (zf - zn)))));
+      if (z_new > 65535)
+        z_new = 65535;
+
+      if (z_new < 18000) {
+        std::cout << z_new << " " << d << " " << x << "\n";
       }
 
       depth_img[i] = z_new;
@@ -276,34 +262,30 @@ pcl::simulation::SimExample::write_depth_image_uint(const float* depth_buffer, s
   }
 
   // Write to file:
-  pcl::io::saveShortPNGFile (fname, depth_img, width_, height_, 1);
-  
-  delete [] depth_img;
-}
+  pcl::io::saveShortPNGFile(fname, depth_img, width_, height_, 1);
 
+  delete[] depth_img;
+}
 
 void
-pcl::simulation::SimExample::write_rgb_image(const uint8_t* rgb_buffer, std::string fname)
+pcl::simulation::SimExample::write_rgb_image(const uint8_t* rgb_buffer,
+                                             std::string fname)
 {
   int npixels = rl_->getWidth() * rl_->getHeight();
   uint8_t* rgb_img = new uint8_t[npixels * 3];
 
-  for (int y = 0; y <  height_; ++y)
-  {
-    for (int x = 0; x < width_; ++x)
-    {
-      int px= y*width_ + x ;
-      int px_in= (height_-1 -y) *width_ + x ; // flip up down
-      rgb_img [3* (px) +0] = rgb_buffer[3*px_in+0];
-      rgb_img [3* (px) +1] = rgb_buffer[3*px_in+1];
-      rgb_img [3* (px) +2] = rgb_buffer[3*px_in+2];      
+  for (int y = 0; y < height_; ++y) {
+    for (int x = 0; x < width_; ++x) {
+      int px = y * width_ + x;
+      int px_in = (height_ - 1 - y) * width_ + x; // flip up down
+      rgb_img[3 * (px) + 0] = rgb_buffer[3 * px_in + 0];
+      rgb_img[3 * (px) + 1] = rgb_buffer[3 * px_in + 1];
+      rgb_img[3 * (px) + 2] = rgb_buffer[3 * px_in + 2];
     }
-  }  
-  
-  // Write to file:
-  pcl::io::saveRgbPNGFile (fname, rgb_img, width_, height_);
-  
-  delete [] rgb_img;
-}
+  }
 
+  // Write to file:
+  pcl::io::saveRgbPNGFile(fname, rgb_img, width_, height_);
 
+  delete[] rgb_img;
+}
diff --git a/simulation/tools/simulation_io.hpp b/simulation/tools/simulation_io.hpp
index 7be86de761a..10d819c425f 100644
--- a/simulation/tools/simulation_io.hpp
+++ b/simulation/tools/simulation_io.hpp
@@ -1,5 +1,4 @@
-#ifndef PCL_SIMULATION_IO_
-#define PCL_SIMULATION_IO_
+#pragma once
 
 #include <boost/shared_ptr.hpp>
 
@@ -7,65 +6,66 @@
 
 #include <pcl/pcl_config.h>
 #ifdef OPENGL_IS_A_FRAMEWORK
-# include <OpenGL/gl.h>
-# include <OpenGL/glu.h>
+#include <OpenGL/gl.h>
+#include <OpenGL/glu.h>
 #else
-# include <GL/gl.h>
-# include <GL/glu.h>
+#include <GL/gl.h>
+#include <GL/glu.h>
 #endif
 #ifdef GLUT_IS_A_FRAMEWORK
-# include <GLUT/glut.h>
+#include <GLUT/glut.h>
 #else
-# include <GL/glut.h>
+#include <GL/glut.h>
 #endif
 
 // define the following in order to eliminate the deprecated headers warning
 #define VTK_EXCLUDE_STRSTREAM_HEADERS
 #include <pcl/io/pcd_io.h>
-#include <pcl/point_types.h>
 #include <pcl/io/vtk_lib_io.h>
-
+#include <pcl/point_types.h>
 
 #include <pcl/simulation/camera.h>
-#include <pcl/simulation/scene.h>
 #include <pcl/simulation/range_likelihood.h>
+#include <pcl/simulation/scene.h>
 
-namespace pcl
-{
-  namespace simulation
-  {
-    class PCL_EXPORTS SimExample
-    {
-      public:
-        using Ptr = boost::shared_ptr<SimExample>;
-        using ConstPtr = boost::shared_ptr<const SimExample>;
-    	
-        SimExample (int argc, char** argv,
-    		int height,int width);
-        void initializeGL (int argc, char** argv);
-        
-        Scene::Ptr scene_;
-        Camera::Ptr camera_;
-        RangeLikelihood::Ptr rl_;  
-    
-        void doSim (Eigen::Isometry3d pose_in);
-    
-        void write_score_image(const float* score_buffer,std::string fname);
-        void write_depth_image(const float* depth_buffer,std::string fname);
-        void write_depth_image_uint(const float* depth_buffer,std::string fname);
-        void write_rgb_image(const uint8_t* rgb_buffer,std::string fname);
-    
-      private:
-        uint16_t t_gamma[2048];  
-    
-        // of platter, usually 640x480
-        int height_;
-        int width_;
-    };
-  }
-}
+namespace pcl {
+namespace simulation {
 
+class PCL_EXPORTS SimExample {
+public:
+  using Ptr = boost::shared_ptr<SimExample>;
+  using ConstPtr = boost::shared_ptr<const SimExample>;
 
+  SimExample(int argc, char** argv, int height, int width);
+  void
+  initializeGL(int argc, char** argv);
 
+  Scene::Ptr scene_;
+  Camera::Ptr camera_;
+  RangeLikelihood::Ptr rl_;
 
-#endif
+  void
+  doSim(Eigen::Isometry3d pose_in);
+
+  void
+  write_score_image(const float* score_buffer, std::string fname);
+
+  void
+  write_depth_image(const float* depth_buffer, std::string fname);
+
+  void
+  write_depth_image_uint(const float* depth_buffer, std::string fname);
+
+  void
+  write_rgb_image(const uint8_t* rgb_buffer, std::string fname);
+
+private:
+  uint16_t t_gamma[2048];
+
+  // of platter, usually 640x480
+  int height_;
+  int width_;
+};
+
+} // namespace simulation
+} // namespace pcl