Address review comments

simulation/src/range_likelihood.cpp

@@ -527,49 +527,30 @@ pcl::simulation::RangeLikelihood::setupProjectionMatrix()
   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;
+  // 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)
 {
-  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);
+  // 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);
 }
 
@@ -587,23 +568,14 @@ pcl::simulation::RangeLikelihood::drawParticles(
 
       // 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;
+      // 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
@@ -638,8 +610,8 @@ costFunction1(float ref_val, float depth_val)
   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);
@@ -884,10 +856,9 @@ pcl::simulation::RangeLikelihood::getPointCloud(
 
         // 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
+        // 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_);
@@ -919,20 +890,35 @@ pcl::simulation::RangeLikelihood::getPointCloud(
   if (make_global) {
     // Go from OpenGL to (Z-up, X-forward, Y-left)
     Eigen::Matrix4f T;
-    T << 0, 0, -1, 0, -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1;
+    // clang-format off
+    T <<  0, 0, -1, 0,
+         -1, 0,  0, 0,
+          0, 1,  0, 0,
+          0, 0,  0, 1;
+    // clang-format on
     Eigen::Matrix4f m = pose.matrix().cast<float>();
     m *= T;
     pcl::transformPointCloud(*pc, *pc, m);
   }
   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;
+    // 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;
-    cam_to_body << 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1;
+    // clang-format off
+    cam_to_body <<  0,  0, 1, 0,
+                   -1,  0, 0, 0,
+                    0, -1, 0, 0,
+                    0,  0, 0, 1;
+    // clang-format on
     Eigen::Matrix4f camera = pose.matrix().cast<float>();
     camera *= cam_to_body;
     pc->sensor_origin_ = camera.rightCols(1);