Point cloud to points (viamrobotics#5248)

Author: penguinland

pointcloud/pointcloud.go

@@ -170,6 +170,16 @@ func CloudCentroid(pc PointCloud) r3.Vector {
 	}
 }
 
+// CloudToPoints converts a point cloud to a list of points (a list of vectors).
+func CloudToPoints(pc PointCloud) []r3.Vector {
+	points := make([]r3.Vector, 0, pc.Size())
+	pc.Iterate(0, 0, func(point r3.Vector, _ Data) bool {
+		points = append(points, point)
+		return true // Keep going through all points
+	})
+	return points
+}
+
 // CloudMatrixCol is a type that represents the columns of a CloudMatrix.
 type CloudMatrixCol int
 

pointcloud/pointcloud_test.go

@@ -113,6 +113,35 @@ func TestPointCloudCentroid(t *testing.T) {
 	test.That(t, CloudCentroid(pc), test.ShouldResemble, r3.Vector{20, 200, 2000})
 }
 
+func TestPointCloudPoints(t *testing.T) {
+	var data Data
+	pc := NewBasicPointCloud(3)
+	test.That(t, pc.Set(r3.Vector{1, 2, 3}, data), test.ShouldBeNil)
+	test.That(t, pc.Set(r3.Vector{4, 5, 6}, data), test.ShouldBeNil)
+	test.That(t, pc.Set(r3.Vector{7, 8, 9}, data), test.ShouldBeNil)
+	points := CloudToPoints(pc)
+	test.That(t, len(points), test.ShouldEqual, 3)
+	// The points can come in an unexpected order. So, just check that the 3 points we expect are
+	// in there somewhere.
+	has1 := false
+	has2 := false
+	has3 := false
+	for _, point := range points {
+		if point.ApproxEqual(r3.Vector{1, 2, 3}) {
+			has1 = true
+		}
+		if point.ApproxEqual(r3.Vector{4, 5, 6}) {
+			has2 = true
+		}
+		if point.ApproxEqual(r3.Vector{7, 8, 9}) {
+			has3 = true
+		}
+	}
+	test.That(t, has1, test.ShouldBeTrue)
+	test.That(t, has2, test.ShouldBeTrue)
+	test.That(t, has3, test.ShouldBeTrue)
+}
+
 func TestPointCloudMatrix(t *testing.T) {
 	pc := NewBasicPointCloud(0)