From 3fc968aa3893b017660eb03915fb473769cc6df7 Mon Sep 17 00:00:00 2001
From: Greg Klein <gregklein@google.com>
Date: Tue, 14 Apr 2015 13:39:40 -0700
Subject: [PATCH] Move matrix identity checks out of inner loops in cropbox to
 increase performance.

---
 filters/include/pcl/filters/impl/crop_box.hpp | 11 +++++++---
 filters/src/crop_box.cpp                      | 21 +++++++++++++------
 2 files changed, 23 insertions(+), 9 deletions(-)

diff --git a/filters/include/pcl/filters/impl/crop_box.hpp b/filters/include/pcl/filters/impl/crop_box.hpp
index 6f55f9eabac..a6dd9848aef 100644
--- a/filters/include/pcl/filters/impl/crop_box.hpp
+++ b/filters/include/pcl/filters/impl/crop_box.hpp
@@ -3,6 +3,7 @@
  *
  *  Point Cloud Library (PCL) - www.pointclouds.org
  *  Copyright (c) 2009-2011, Willow Garage, Inc.
+ *  Copyright (c) 2015, Google, Inc.
  *
  *  All rights reserved.
  *
@@ -89,6 +90,10 @@ pcl::CropBox<PointT>::applyFilter (std::vector<int> &indices)
     inverse_transform = transform.inverse ();
   }
 
+  bool transform_matrix_is_identity = transform_.matrix ().isIdentity ();
+  bool translation_is_zero = (translation_ != Eigen::Vector3f::Zero ());
+  bool inverse_transform_matrix_is_identity = inverse_transform.matrix ().isIdentity ();
+
   for (size_t index = 0; index < indices_->size (); ++index)
   {
     if (!input_->is_dense)
@@ -100,10 +105,10 @@ pcl::CropBox<PointT>::applyFilter (std::vector<int> &indices)
     PointT local_pt = input_->points[(*indices_)[index]];
 
     // Transform point to world space
-    if (!(transform_.matrix ().isIdentity ()))
+    if (!transform_matrix_is_identity)
       local_pt = pcl::transformPoint<PointT> (local_pt, transform_);
 
-    if (translation_ != Eigen::Vector3f::Zero ())
+    if (translation_is_zero)
     {
       local_pt.x -= translation_ (0);
       local_pt.y -= translation_ (1);
@@ -111,7 +116,7 @@ pcl::CropBox<PointT>::applyFilter (std::vector<int> &indices)
     }
 
     // Transform point to local space of crop box
-    if (!(inverse_transform.matrix ().isIdentity ()))
+    if (!inverse_transform_matrix_is_identity)
       local_pt = pcl::transformPoint<PointT> (local_pt, inverse_transform);
 
     // If outside the cropbox
diff --git a/filters/src/crop_box.cpp b/filters/src/crop_box.cpp
index 91ce97ec9ca..996a7d0619e 100644
--- a/filters/src/crop_box.cpp
+++ b/filters/src/crop_box.cpp
@@ -4,6 +4,7 @@
  *  Point Cloud Library (PCL) - www.pointclouds.org
  *  Copyright (c) 2009, Willow Garage, Inc.
  *  Copyright (c) 2012-, Open Perception, Inc.
+ *  Copyright (c) 2015, Google, Inc.
  *
  *  All rights reserved.
  *
@@ -70,6 +71,10 @@ pcl::CropBox<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
   //PointXYZ local_pt;
   Eigen::Vector3f local_pt (Eigen::Vector3f::Zero ());
 
+  bool transform_matrix_is_identity = transform_.matrix ().isIdentity ();
+  bool translation_is_zero = (translation_ != Eigen::Vector3f::Zero ());
+  bool inverse_transform_matrix_is_identity = inverse_transform.matrix ().isIdentity ();
+
   for (size_t index = 0; index < indices_->size (); ++index)
   {
     // Get local point
@@ -84,10 +89,10 @@ pcl::CropBox<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
       continue;
 
     // Transform point to world space
-    if (!(transform_.matrix().isIdentity()))
+    if (!transform_matrix_is_identity)
       local_pt = transform_ * local_pt;
 
-    if (translation_ != Eigen::Vector3f::Zero ())
+    if (translation_is_zero)
     {
       local_pt.x () = local_pt.x () - translation_ (0);
       local_pt.y () = local_pt.y () - translation_ (1);
@@ -95,7 +100,7 @@ pcl::CropBox<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
     }
 
     // Transform point to local space of crop box
-    if (!(inverse_transform.matrix ().isIdentity ()))
+    if (!inverse_transform_matrix_is_identity)
       local_pt = inverse_transform * local_pt;
 
     // If outside the cropbox
@@ -156,6 +161,10 @@ pcl::CropBox<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &indices)
   //PointXYZ local_pt;
   Eigen::Vector3f local_pt (Eigen::Vector3f::Zero ());
 
+  bool transform_matrix_is_identity = transform_.matrix ().isIdentity ();
+  bool translation_is_zero = (translation_ != Eigen::Vector3f::Zero ());
+  bool inverse_transform_matrix_is_identity = inverse_transform.matrix ().isIdentity ();
+
   for (size_t index = 0; index < indices_->size (); index++)
   {
     // Get local point
@@ -164,10 +173,10 @@ pcl::CropBox<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &indices)
     memcpy (&local_pt, &input_->data[offset], sizeof (float)*3);
 
     // Transform point to world space
-    if (!(transform_.matrix().isIdentity()))
+    if (!transform_matrix_is_identity)
       local_pt = transform_ * local_pt;
 
-    if (translation_ != Eigen::Vector3f::Zero ())
+    if (translation_is_zero)
     {
       local_pt.x () -= translation_ (0);
       local_pt.y () -= translation_ (1);
@@ -175,7 +184,7 @@ pcl::CropBox<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &indices)
     }
 
     // Transform point to local space of crop box
-    if (!(inverse_transform.matrix().isIdentity()))
+    if (!(inverse_transform_matrix_is_identity))
       local_pt = inverse_transform * local_pt;
 
     // If outside the cropbox