diff --git a/common/include/pcl/PolygonMesh.h b/common/include/pcl/PolygonMesh.h
index ae610dcc34f..55d69995ade 100644
--- a/common/include/pcl/PolygonMesh.h
+++ b/common/include/pcl/PolygonMesh.h
@@ -30,6 +30,8 @@ namespace pcl
     static bool
     concatenate (pcl::PolygonMesh &mesh1, const pcl::PolygonMesh &mesh2)
     {
+      const auto point_offset = mesh1.cloud.width * mesh1.cloud.height;
+      
       bool success = pcl::PCLPointCloud2::concatenate(mesh1.cloud, mesh2.cloud);
       if (success == false) {
         return false;
@@ -37,7 +39,6 @@ namespace pcl
       // Make the resultant polygon mesh take the newest stamp
       mesh1.header.stamp = std::max(mesh1.header.stamp, mesh2.header.stamp);
 
-      const auto point_offset = mesh1.cloud.width * mesh1.cloud.height;
       std::transform(mesh2.polygons.begin (),
                      mesh2.polygons.end (),
                      std::back_inserter (mesh1.polygons),
diff --git a/test/common/test_polygon_mesh.cpp b/test/common/test_polygon_mesh.cpp
index 4f2d41a218d..d03038ab192 100644
--- a/test/common/test_polygon_mesh.cpp
+++ b/test/common/test_polygon_mesh.cpp
@@ -91,11 +91,13 @@ TEST(PolygonMesh, concatenate_cloud)
 
 TEST(PolygonMesh, concatenate_vertices)
 {
+    const std::size_t size = 15;
+
     PolygonMesh test, dummy;
-    test.cloud.width = 10;
-    test.cloud.height = 5;
+    // The algorithm works regardless of the organization.
+    test.cloud.width = dummy.cloud.width = size;
+    test.cloud.height = dummy.cloud.height = 1;
 
-    const std::size_t size = 15;
     for (std::size_t i = 0; i < size; ++i)
     {
         dummy.polygons.emplace_back();
@@ -111,18 +113,16 @@ TEST(PolygonMesh, concatenate_vertices)
     EXPECT_EQ(2 * dummy.polygons.size(), test.polygons.size());
 
     const auto cloud_size = test.cloud.width * test.cloud.height;
-    for (std::size_t i = 0; i < dummy.polygons.size(); ++i)
-    {
-        EXPECT_EQ(dummy.polygons[i].vertices.size(), test.polygons[i].vertices.size());
-        EXPECT_EQ(dummy.polygons[i].vertices.size(),
-                  test.polygons[i + dummy.polygons.size()].vertices.size());
-        for (std::size_t j = 0; j < size; ++j)
-        {
-            EXPECT_EQ(dummy.polygons[i].vertices[j],
-                      test.polygons[i].vertices[j]);
-            EXPECT_EQ(dummy.polygons[i].vertices[j] + cloud_size,
-                      test.polygons[i + dummy.polygons.size()].vertices[j]);
-        }
+    for (const auto& polygon : test.polygons)
+      for (const auto& vertex : polygon.vertices)
+        EXPECT_LT(vertex, cloud_size);
+
+    pcl::Indices vertices(size);
+    for (std::size_t i = 0; i < size; ++i) {
+        vertices = dummy.polygons[i].vertices;
+        EXPECT_EQ_VECTORS(vertices, test.polygons[i].vertices);
+        for (auto& vertex : vertices) { vertex += size; }
+        EXPECT_EQ_VECTORS(vertices, test.polygons[i + size].vertices);
     }
 }