[RSDK-1258] World State to Point Cloud Conversion Method

pointcloud/basic.go

@@ -1,6 +1,8 @@
 package pointcloud
 
 import (
+	"image/color"
+
 	"github.com/golang/geo/r3"
 )
 
@@ -51,3 +53,21 @@ func (cloud *basicPointCloud) Set(p r3.Vector, d Data) error {
 func (cloud *basicPointCloud) Iterate(numBatches, myBatch int, fn func(p r3.Vector, d Data) bool) {
 	cloud.points.Iterate(numBatches, myBatch, fn)
 }
+
+// VectorsToPointCloud converts a list of r3.Vectors into a pointcloud with the specified color.
+func VectorsToPointCloud(vectors []r3.Vector, c color.NRGBA) (PointCloud, error) {
+	// initialize empty pointcloud
+	cloud := basicPointCloud{
+		points: &matrixStorage{points: make([]PointAndData, 0, len(vectors)), indexMap: make(map[r3.Vector]uint, len(vectors))},
+		meta:   NewMetaData(),
+	}
+	// TODO: the for loop below can be made concurrent
+	// iterate thought the vector list and add to the pointcloud
+	for _, v := range vectors {
+		data := &basicData{hasColor: true, c: c}
+		if err := cloud.Set(v, data); err != nil {
+			return &cloud, err
+		}
+	}
+	return &cloud, nil
+}

pointcloud/pointcloud_test.go

@@ -171,3 +171,16 @@ func TestPointCloudMatrix(t *testing.T) {
 	})
 	test.That(t, mcv, test.ShouldResemble, mat.NewDense(1, 7, []float64{1, 2, 3, 123, 45, 67, 5}))
 }
+
+func TestVectorsToPointCloud(t *testing.T) {
+	vecLst := []r3.Vector{{0, 0.75, 0}, {0, -0.75, 0}, {1, 0.75, 1}, {-1, 0.75, -1}}
+	color := &color.NRGBA{0, 0, 255, 255}
+	pc, err := VectorsToPointCloud(vecLst, *color)
+	test.That(t, err, test.ShouldBeNil)
+	// make sure all points were added with the specified color
+	for _, v := range vecLst {
+		data, boolVal := pc.At(v.X, v.Y, v.Z)
+		test.That(t, boolVal, test.ShouldBeTrue)
+		test.That(t, data.Color(), test.ShouldResemble, color)
+	}
+}

spatialmath/box.go

@@ -11,6 +11,9 @@ import (
 	"go.viam.com/rdk/utils"
 )
 
+// box density corresponding to how many points per square mm.
+const defaultBoxPointDensity = .5
+
 // BoxCreator implements the GeometryCreator interface for box structs.
 type boxCreator struct {
 	halfSize        r3.Vector
@@ -167,7 +170,7 @@ func (b *box) CollidesWith(g Geometry) (bool, error) {
 		return sphereVsBoxCollision(other, b), nil
 	}
 	if other, ok := g.(*point); ok {
-		return pointVsBoxCollision(b, other.pose.Point()), nil
+		return pointVsBoxCollision(b, other.position), nil
 	}
 	return true, newCollisionTypeUnsupportedError(b, g)
 }
@@ -180,7 +183,7 @@ func (b *box) DistanceFrom(g Geometry) (float64, error) {
 		return sphereVsBoxDistance(other, b), nil
 	}
 	if other, ok := g.(*point); ok {
-		return pointVsBoxDistance(b, other.pose.Point()), nil
+		return pointVsBoxDistance(b, other.position), nil
 	}
 	return math.Inf(-1), newCollisionTypeUnsupportedError(b, g)
 }
@@ -352,3 +355,91 @@ func separatingAxisTest(positionDelta, plane r3.Vector, halfSizeA, halfSizeB [3]
 	}
 	return sum
 }
+
+// ToPointCloud converts a box geometry into a []r3.Vector. This method takes one argument which
+// determines how many points to place per square mm. If the argument is set to 0. we automatically
+// substitute the value with defaultBoxPointDensity.
+func (b *box) ToPointCloud(resolution float64) []r3.Vector {
+	// check for user defined spacing
+	var iter float64
+	if resolution != 0. {
+		iter = resolution
+	} else {
+		iter = defaultBoxPointDensity
+	}
+
+	// the boolean values which are passed into the fillFaces method allow for the edges of the
+	// box to only be iterated over once. This removes duplicate points.
+	// TODO: the fillFaces method calls can be made concurrent if the ToPointCloud method is too slow
+	var facePoints []r3.Vector
+	facePoints = append(facePoints, fillFaces(b.halfSize, iter, 0, true, false)...)
+	facePoints = append(facePoints, fillFaces(b.halfSize, iter, 1, true, true)...)
+	facePoints = append(facePoints, fillFaces(b.halfSize, iter, 2, false, false)...)
+
+	transformedVecs := transformPointsToPose(facePoints, b.Pose())
+	return transformedVecs
+}
+
+// fillFaces returns a list of vectors which lie on the surface of the box.
+func fillFaces(halfSize [3]float64, iter float64, fixedDimension int, orEquals1, orEquals2 bool) []r3.Vector {
+	var facePoints []r3.Vector
+	// create points on box faces with box centered at (0, 0, 0)
+	starts := [3]float64{0.0, 0.0, 0.0}
+	// depending on which face we want to fill, one of i,j,k is kept constant
+	starts[fixedDimension] = halfSize[fixedDimension]
+	for i := starts[0]; lessThan(orEquals1, i, halfSize[0]); i += iter {
+		for j := starts[1]; lessThan(orEquals2, j, halfSize[1]); j += iter {
+			for k := starts[2]; k <= halfSize[2]; k += iter {
+				p1 := r3.Vector{i, j, k}
+				p2 := r3.Vector{i, j, -k}
+				p3 := r3.Vector{i, -j, k}
+				p4 := r3.Vector{i, -j, -k}
+				p5 := r3.Vector{-i, j, k}
+				p6 := r3.Vector{-i, j, -k}
+				p7 := r3.Vector{-i, -j, -k}
+				p8 := r3.Vector{-i, -j, k}
+
+				switch {
+				case i == 0.0 && j == 0.0:
+					facePoints = append(facePoints, p1, p2)
+				case j == 0.0 && k == 0.0:
+					facePoints = append(facePoints, p1, p5)
+				case i == 0.0 && k == 0.0:
+					facePoints = append(facePoints, p1, p7)
+				case i == 0.0:
+					facePoints = append(facePoints, p1, p2, p3, p4)
+				case j == 0.0:
+					facePoints = append(facePoints, p1, p2, p5, p6)
+				case k == 0.0:
+					facePoints = append(facePoints, p1, p3, p5, p8)
+				default:
+					facePoints = append(facePoints, p1, p2, p3, p4, p5, p6, p7, p8)
+				}
+			}
+		}
+	}
+	return facePoints
+}
+
+// lessThan checks if v1 <= v1 only if orEquals is true, otherwise we check if v1 < v2.
+func lessThan(orEquals bool, v1, v2 float64) bool {
+	if orEquals {
+		return v1 <= v2
+	}
+	return v1 < v2
+}
+
+// transformPointsToPose gives vectors the proper orientation then translates them to the desired position.
+func transformPointsToPose(facePoints []r3.Vector, pose Pose) []r3.Vector {
+	var transformedVectors []r3.Vector
+	for i := range facePoints {
+		// create pose for a vector at origin from the desired orientation
+		originWithPose := NewPoseFromOrientation(r3.Vector{0, 0, 0}, pose.Orientation())
+		// create the desired pose for points[i]
+		pointPose := Compose(originWithPose, NewPoseFromPoint(facePoints[i]))
+		// translate the vector to the desired position
+		transformedVec := Compose(NewPoseFromPoint(pose.Point()), pointPose).Point()
+		transformedVectors = append(transformedVectors, transformedVec)
+	}
+	return transformedVectors
+}

spatialmath/box_test.go

@@ -51,3 +51,81 @@ func TestBoxVertices(t *testing.T) {
 	test.That(t, R3VectorAlmostEqual(vertices[6], r3.Vector{-1, -1, 1}.Add(offset), 1e-8), test.ShouldBeTrue)
 	test.That(t, R3VectorAlmostEqual(vertices[7], r3.Vector{-1, -1, -1}.Add(offset), 1e-8), test.ShouldBeTrue)
 }
+
+func TestBoxPC(t *testing.T) {
+	offset1 := r3.Vector{2, 2, 0}
+	dims1 := r3.Vector{2, 2, 2}
+	eulerAngle1 := &EulerAngles{45, 45, 0}
+	pose1 := NewPoseFromOrientation(offset1, eulerAngle1)
+	box1 := &box{pose1, [3]float64{0.5 * dims1.X, 0.5 * dims1.Y, 0.5 * dims1.Z}, 10, ""} // with abitrary radius bounding sphere
+	customDensity := 1.
+	output1 := box1.ToPointCloud(customDensity)
+
+	checkAgainst1 := []r3.Vector{
+		{2.525321988817730733956068, 2.000000000000001332267630, -0.850903524534118327338206},
+		{1.474678011182271264445376, 2.000000000000000888178420, 0.850903524534118549382811},
+		{2.972320320618009770186063, 1.149096475465882560840214, -0.574940332598703474076274},
+		{2.078323657017452141815284, 2.850903524534119881650440, -1.126866716469533624689348},
+		{1.921676342982550300675371, 1.149096475465882782884819, 1.126866716469533624689348},
+		{1.027679679381992006170776, 2.850903524534119881650440, 0.574940332598703696120879},
+		{2.724036808064585812871883, 2.525321988817730733956068, 0.446998331800279036229995},
+		{1.275963191935416185529562, 1.474678011182271486489981, -0.446998331800278925207692},
+		{3.171035139864865737280297, 1.674418464283612628662468, 0.722961523735694333581137},
+		{2.277038476264307220731098, 3.376225513351849727428089, 0.171035139864864182968063},
+		{1.722961523735694999714951, 0.623774486648152826084868, -0.171035139864864044190185},
+		{0.828964860135136816232659, 2.325581535716389591783582, -0.722961523735694111536532},
+		{3.249358796882316102738741, 2.525321988817730733956068, -0.403905192733839402130513},
+		{1.801285180753145809262605, 1.474678011182271264445376, -1.297901856334397363568200},
+		{2.198714819246856411183444, 2.525321988817731178045278, 1.297901856334397585612805},
+		{0.750641203117686117707308, 1.474678011182271486489981, 0.403905192733839513152816},
+		{3.696357128682595138968736, 1.674418464283612628662468, -0.127942000798424243557250},
+		{2.802360465082037066508747, 3.376225513351848839249669, -0.679868384669254366414748},
+		{2.248283512553424845492600, 0.623774486648152715062565, -1.021938664398982510306269},
+		{1.354286848952866773032611, 2.325581535716390035872791, -1.573865048269812660919342},
+		{2.645713151047135447413439, 1.674418464283612184573258, 1.573865048269812660919342},
+		{1.751716487446577152908844, 3.376225513351849283338879, 1.021938664398982510306269},
+		{0.303642871317407081477313, 2.325581535716390035872791, 0.127942000798424410090703},
+		{1.197639534917965153937303, 0.623774486648152715062565, 0.679868384669254477437050},
+		{2.446998331800279924408414, 1.149096475465882782884819, 0.275963191935414964284234},
+		{1.553001668199722073993030, 2.850903524534119881650440, -0.275963191935414853261932},
+	}
+	for i, v := range output1 {
+		test.That(t, R3VectorAlmostEqual(v, checkAgainst1[i], 1e-2), test.ShouldBeTrue)
+	}
+
+	// second check
+	offset2 := r3.Vector{2, 2, 2}
+	dims2 := r3.Vector{1, 1.5, 4}
+	eulerAngle2 := &EulerAngles{0, 45, 0}
+	pose2 := NewPoseFromOrientation(offset2, eulerAngle2)
+	box2 := &box{pose2, [3]float64{0.5 * dims2.X, 0.5 * dims2.Y, 0.5 * dims2.Z}, 10, ""} // with abitrary radius bounding sphere
+	output2 := box2.ToPointCloud(customDensity)
+
+	checkAgainst2 := []r3.Vector{
+		{2.262660994408865811067244, 2.000000000000000888178420, 1.574548237732941391442409},
+		{1.737339005591135743244990, 2.000000000000000888178420, 2.425451762267059496736010},
+		{3.113564518942984360450055, 2.000000000000000888178420, 2.099870226550671237220058},
+		{1.411757469874747261684433, 2.000000000000000888178420, 1.049226248915211323620156},
+		{2.588242530125254514672406, 2.000000000000000888178420, 2.950773751084789342513659},
+		{0.886435481057017415906785, 2.000000000000000888178420, 1.900129773449329650958362},
+		{3.964468043477102909832865, 2.000000000000000888178420, 2.625192215368401527086917},
+		{0.560853945340628823323925, 2.000000000000000888178420, 0.523904260097481366820205},
+		{3.439146054659373064055217, 2.000000000000000888178420, 3.476095739902519188291308},
+		{0.035531956522898887340656, 2.000000000000000888178420, 1.374807784631600027225318},
+		{2.000000000000000888178420, 2.750000000000000888178420, 2.000000000000000444089210},
+		{2.000000000000000888178420, 1.250000000000000666133815, 2.000000000000000444089210},
+		{2.850903524534118993472021, 2.750000000000000888178420, 2.525321988817730289866859},
+		{1.149096475465882338795609, 2.750000000000000888178420, 1.474678011182270598311561},
+		{2.850903524534118993472021, 1.250000000000000666133815, 2.525321988817730289866859},
+		{1.149096475465882338795609, 1.250000000000000666133815, 1.474678011182270598311561},
+		{3.701807049068237986944041, 2.750000000000000888178420, 3.050643977635460579733717},
+		{0.298192950931763844923950, 2.750000000000000888178420, 0.949356022364540641511610},
+		{3.701807049068237986944041, 1.250000000000000666133815, 3.050643977635460579733717},
+		{0.298192950931763844923950, 1.250000000000000666133815, 0.949356022364540641511610},
+		{3.701807049068237986944041, 2.000000000000000888178420, 3.050643977635460579733717},
+		{0.298192950931763844923950, 2.000000000000000888178420, 0.949356022364540641511610},
+	}
+	for i, v := range output2 {
+		test.That(t, R3VectorAlmostEqual(v, checkAgainst2[i], 1e-2), test.ShouldBeTrue)
+	}
+}

spatialmath/geometry.go

@@ -28,6 +28,7 @@ type Geometry interface {
 	DistanceFrom(Geometry) (float64, error)
 	EncompassedBy(Geometry) (bool, error)
 	Label() string
+	ToPointCloud(float64) []r3.Vector
 }
 
 // GeometryType defines what geometry creator representations are known.

spatialmath/point.go

@@ -17,8 +17,8 @@ type pointCreator struct {
 
 // point is a collision geometry that represents a single point in 3D space that occupies no geometry.
 type point struct {
-	pose  Pose
-	label string
+	position r3.Vector
+	label    string
 }
 
 // NewPointCreator instantiates a PointCreator class, which allows instantiating point geometries given only a pose which is applied
@@ -29,7 +29,7 @@ func NewPointCreator(offset Pose, label string) GeometryCreator {
 
 // NewGeometry instantiates a new point from a PointCreator class.
 func (pc *pointCreator) NewGeometry(pose Pose) Geometry {
-	return &point{Compose(pc.offset, pose), pc.label}
+	return &point{Compose(pc.offset, pose).Point(), pc.label}
 }
 
 func (pc *pointCreator) Offset() Pose {
@@ -65,7 +65,7 @@ func (pc *pointCreator) ToProtobuf() *commonpb.Geometry {
 
 // NewPoint instantiates a new point Geometry.
 func NewPoint(pt r3.Vector, label string) Geometry {
-	return &point{NewPoseFromPoint(pt), label}
+	return &point{pt, label}
 }
 
 // Label returns the labels of this point.
@@ -78,12 +78,12 @@ func (pt *point) Label() string {
 
 // Pose returns the pose of the point.
 func (pt *point) Pose() Pose {
-	return pt.pose
+	return NewPoseFromPoint(pt.position)
 }
 
-// Vertices returns the vertices defining the point.
+// Vertices returns the vertex defining the point.
 func (pt *point) Vertices() []r3.Vector {
-	return []r3.Vector{pt.pose.Point()}
+	return []r3.Vector{pt.position}
 }
 
 // AlmostEqual compares the point with another geometry and checks if they are equivalent.
@@ -92,18 +92,18 @@ func (pt *point) AlmostEqual(g Geometry) bool {
 	if !ok {
 		return false
 	}
-	return PoseAlmostEqual(pt.pose, other.pose)
+	return PoseAlmostEqual(NewPoseFromPoint(pt.position), NewPoseFromPoint(other.position))
 }
 
 // Transform premultiplies the point pose with a transform, allowing the point to be moved in space.
 func (pt *point) Transform(toPremultiply Pose) Geometry {
-	return &point{Compose(toPremultiply, pt.pose), pt.label}
+	return &point{Compose(toPremultiply, NewPoseFromPoint(pt.position)).Point(), pt.label}
 }
 
 // ToProto converts the point to a Geometry proto message.
 func (pt *point) ToProtobuf() *commonpb.Geometry {
 	return &commonpb.Geometry{
-		Center: PoseToProtobuf(pt.pose),
+		Center: PoseToProtobuf(NewPoseFromPoint(pt.position)),
 		GeometryType: &commonpb.Geometry_Sphere{
 			Sphere: &commonpb.Sphere{
 				RadiusMm: 0,
@@ -116,10 +116,10 @@ func (pt *point) ToProtobuf() *commonpb.Geometry {
 // CollidesWith checks if the given point collides with the given geometry and returns true if it does.
 func (pt *point) CollidesWith(g Geometry) (bool, error) {
 	if other, ok := g.(*box); ok {
-		return pointVsBoxCollision(other, pt.pose.Point()), nil
+		return pointVsBoxCollision(other, pt.position), nil
 	}
 	if other, ok := g.(*sphere); ok {
-		return sphereVsPointDistance(other, pt.pose.Point()) <= 0, nil
+		return sphereVsPointDistance(other, pt.position) <= 0, nil
 	}
 	if other, ok := g.(*point); ok {
 		return pt.AlmostEqual(other), nil
@@ -130,13 +130,13 @@ func (pt *point) CollidesWith(g Geometry) (bool, error) {
 // CollidesWith checks if the given point collides with the given geometry and returns true if it does.
 func (pt *point) DistanceFrom(g Geometry) (float64, error) {
 	if other, ok := g.(*box); ok {
-		return pointVsBoxDistance(other, pt.pose.Point()), nil
+		return pointVsBoxDistance(other, pt.position), nil
 	}
 	if other, ok := g.(*sphere); ok {
-		return sphereVsPointDistance(other, pt.pose.Point()), nil
+		return sphereVsPointDistance(other, pt.position), nil
 	}
 	if other, ok := g.(*point); ok {
-		return pt.pose.Point().Sub(other.pose.Point()).Norm(), nil
+		return pt.position.Sub(other.position).Norm(), nil
 	}
 	return math.Inf(-1), newCollisionTypeUnsupportedError(pt, g)
 }
@@ -162,3 +162,8 @@ func pointVsBoxDistance(b *box, pt r3.Vector) float64 {
 	}
 	return -b.penetrationDepth(pt)
 }
+
+// ToPointCloud converts a point geometry into a []r3.Vector.
+func (pt *point) ToPointCloud(resolution float64) []r3.Vector {
+	return []r3.Vector{pt.position}
+}

spatialmath/point_test.go

@@ -11,11 +11,11 @@ import (
 func TestNewPoint(t *testing.T) {
 	offset := NewPoseFromOrientation(r3.Vector{X: 1, Y: 0, Z: 0}, &EulerAngles{0, 0, math.Pi})
 
-	// test sphere created from NewBox method
+	// test point created from NewBox method
 	geometry := NewPoint(offset.Point(), "")
-	test.That(t, geometry, test.ShouldResemble, &point{NewPoseFromPoint(offset.Point()), ""})
+	test.That(t, geometry, test.ShouldResemble, &point{offset.Point(), ""})
 
-	// test sphere created from GeometryCreator with offset
+	// test point created from GeometryCreator with offset
 	geometry = NewPointCreator(offset, "").NewGeometry(PoseInverse(offset))
 	test.That(t, PoseAlmostCoincident(geometry.Pose(), NewZeroPose()), test.ShouldBeTrue)
 }