RSDK-10392 remove magic numbers from point cloud collision check (#4896)

Author: raybjork

motionplan/constraint.go

@@ -117,7 +117,7 @@ func createAllCollisionConstraints(
 		for _, geom := range worldGeometries {
 			if octree, ok := geom.(*pointcloud.BasicOctree); ok {
 				if zeroCG == nil {
-					zeroCG, err = setupZeroCG(movingRobotGeometries, worldGeometries, allowedCollisions, collisionBufferMM)
+					zeroCG, err = setupZeroCG(movingRobotGeometries, worldGeometries, allowedCollisions, false, collisionBufferMM)
 					if err != nil {
 						return nil, nil, err
 					}
@@ -193,12 +193,14 @@ func createAllCollisionConstraints(
 	return constraintFSMap, constraintMap, nil
 }
 
-func setupZeroCG(moving, static []spatial.Geometry,
+func setupZeroCG(
+	moving, static []spatial.Geometry,
 	collisionSpecifications []*Collision,
+	reportDistances bool,
 	collisionBufferMM float64,
 ) (*collisionGraph, error) {
 	// create the reference collisionGraph
-	zeroCG, err := newCollisionGraph(moving, static, nil, true, collisionBufferMM)
+	zeroCG, err := newCollisionGraph(moving, static, nil, reportDistances, collisionBufferMM)
 	if err != nil {
 		return nil, err
 	}
@@ -218,7 +220,7 @@ func NewCollisionConstraint(
 	reportDistances bool,
 	collisionBufferMM float64,
 ) (StateConstraint, error) {
-	zeroCG, err := setupZeroCG(moving, static, collisionSpecifications, collisionBufferMM)
+	zeroCG, err := setupZeroCG(moving, static, collisionSpecifications, true, collisionBufferMM)
 	if err != nil {
 		return nil, err
 	}
@@ -273,7 +275,7 @@ func NewCollisionConstraintFS(
 	reportDistances bool,
 	collisionBufferMM float64,
 ) (StateFSConstraint, error) {
-	zeroCG, err := setupZeroCG(moving, static, collisionSpecifications, collisionBufferMM)
+	zeroCG, err := setupZeroCG(moving, static, collisionSpecifications, true, collisionBufferMM)
 	if err != nil {
 		return nil, err
 	}

pointcloud/basic_octree.go

@@ -17,12 +17,10 @@ const (
 	leafNodeFilled
 	// This value allows for high level of granularity in the octree while still allowing for fast access times
 	// even on a pi.
-	maxRecursionDepth = 250  // This gives us enough resolution to model the observable universe in planck lengths.
-	floatEpsilon      = 1e-6 // This is also effectively half of the minimum side length.
-	nodeRegionOverlap = floatEpsilon / 2
-	// TODO (RSDK-3767): pass these in a different way.
-	confidenceThreshold = 50    // value between 0-100, threshold sets the confidence level required for a point to be considered a collision
-	buffer              = 150.0 // max distance from base to point for it to be considered a collision in mm
+	maxRecursionDepth          = 250  // This gives us enough resolution to model the observable universe in planck lengths.
+	floatEpsilon               = 1e-6 // This is also effectively half of the minimum side length.
+	nodeRegionOverlap          = floatEpsilon / 2
+	defaultConfidenceThreshold = 50
 )
 
 // NodeType represents the possible types of nodes in an octree.
@@ -39,6 +37,9 @@ type BasicOctree struct {
 	size       int
 	meta       MetaData
 	label      string
+
+	// value between 0-100 that sets a threshold which is the confidence level required for a point to be considered a collision
+	confidenceThreshold int
 }
 
 // basicOctreeNode is a struct comprised of the type of node, children nodes (should they exist) and the pointcloud's
@@ -50,21 +51,19 @@ type basicOctreeNode struct {
 	maxVal   int
 }
 
-// NewBasicOctree creates a new basic octree with specified center, side and metadata.
-func NewBasicOctree(center r3.Vector, sideLength float64) (*BasicOctree, error) {
+// NewBasicOctree creates a new basic octree with specified center, side and confidenceThreshold.
+func NewBasicOctree(center r3.Vector, sideLength float64, confidenceThreshold int) (*BasicOctree, error) {
 	if sideLength <= 0 {
 		return nil, errors.Errorf("invalid side length (%.2f) for octree", sideLength)
 	}
-
-	octree := &BasicOctree{
-		node:       newLeafNodeEmpty(),
-		center:     center,
-		sideLength: sideLength,
-		size:       0,
-		meta:       NewMetaData(),
-	}
-
-	return octree, nil
+	return &BasicOctree{
+		node:                newLeafNodeEmpty(),
+		center:              center,
+		sideLength:          sideLength,
+		size:                0,
+		meta:                NewMetaData(),
+		confidenceThreshold: confidenceThreshold,
+	}, nil
 }
 
 // Size returns the number of points stored in the octree's metadata.
@@ -174,7 +173,7 @@ func (octree *BasicOctree) Transform(pose spatialmath.Pose) spatialmath.Geometry
 	newCenter := spatialmath.Compose(pose, spatialmath.NewPoseFromPoint(octree.center))
 
 	// New sidelength is the diagonal of octree to guarantee fit
-	newOctree, err := NewBasicOctree(newCenter.Point(), octree.sideLength*math.Sqrt(3))
+	newOctree, err := NewBasicOctree(newCenter.Point(), octree.sideLength*math.Sqrt(3), octree.confidenceThreshold)
 	if err != nil {
 		return nil
 	}
@@ -198,22 +197,21 @@ func (octree *BasicOctree) ToProtobuf() *commonpb.Geometry {
 	return nil
 }
 
-// CollidesWithGeometry will return whether a given geometry is in collision with a given point.
-// A point is in collision if its stored probability is >= confidenceThreshold and if it is at most buffer distance away.
-func (octree *BasicOctree) CollidesWithGeometry(
-	geom spatialmath.Geometry,
-	confidenceThreshold int,
-	buffer,
-	collisionBufferMM float64,
-) (bool, error) {
-	if octree.MaxVal() < confidenceThreshold {
+// CollidesWith checks if the given octree collides with the given geometry and returns true if it does.
+// A point is in collision if its stored probability is >= confidenceThreshold and if it is at most collisionBufferMM distance away.
+func (octree *BasicOctree) CollidesWith(geom spatialmath.Geometry, collisionBufferMM float64) (bool, error) {
+	if octree.MaxVal() < octree.confidenceThreshold {
 		return false, nil
 	}
 	switch octree.node.nodeType {
 	case internalNode:
 		ocbox, err := spatialmath.NewBox(
 			spatialmath.NewPoseFromPoint(octree.center),
-			r3.Vector{octree.sideLength + buffer, octree.sideLength + buffer, octree.sideLength + buffer},
+			r3.Vector{
+				X: octree.sideLength + collisionBufferMM,
+				Y: octree.sideLength + collisionBufferMM,
+				Z: octree.sideLength + collisionBufferMM,
+			},
 			"",
 		)
 		if err != nil {
@@ -229,7 +227,7 @@ func (octree *BasicOctree) CollidesWithGeometry(
 			return false, nil
 		}
 		for _, child := range octree.node.children {
-			collide, err = child.CollidesWithGeometry(geom, confidenceThreshold, buffer, collisionBufferMM)
+			collide, err = child.CollidesWith(geom, collisionBufferMM)
 			if err != nil {
 				return false, err
 			}
@@ -241,25 +239,11 @@ func (octree *BasicOctree) CollidesWithGeometry(
 	case leafNodeEmpty:
 		return false, nil
 	case leafNodeFilled:
-		ptGeom, err := spatialmath.NewSphere(spatialmath.NewPoseFromPoint(octree.node.point.P), buffer, "")
-		if err != nil {
-			return false, err
-		}
-
-		ptCollide, err := geom.CollidesWith(ptGeom, collisionBufferMM)
-		if err != nil {
-			return false, err
-		}
-		return ptCollide, nil
+		return geom.CollidesWith(spatialmath.NewPoint(octree.node.point.P, ""), collisionBufferMM)
 	}
 	return false, errors.New("unknown octree node type")
 }
 
-// CollidesWith checks if the given octree collides with the given geometry and returns true if it does.
-func (octree *BasicOctree) CollidesWith(geom spatialmath.Geometry, collisionBufferMM float64) (bool, error) {
-	return octree.CollidesWithGeometry(geom, confidenceThreshold, buffer, collisionBufferMM)
-}
-
 // DistanceFrom returns the distance from the given octree to the given geometry.
 // TODO (RSDK-3743): Implement BasicOctree Geometry functions.
 func (octree *BasicOctree) DistanceFrom(geom spatialmath.Geometry) (float64, error) {

pointcloud/basic_octree_test.go

@@ -17,7 +17,7 @@ import (
 
 // Helper function for generating a new empty octree.
 func createNewOctree(center r3.Vector, side float64) (*BasicOctree, error) {
-	basicOct, err := NewBasicOctree(center, side)
+	basicOct, err := NewBasicOctree(center, side, defaultConfidenceThreshold)
 	if err != nil {
 		return nil, err
 	}
@@ -98,6 +98,10 @@ func TestBasicOctreeNew(t *testing.T) {
 func TestBasicOctreeSet(t *testing.T) {
 	center := r3.Vector{X: 0, Y: 0, Z: 0}
 	side := 2.0
+	d1 := 99
+	d2 := 100
+	d3 := 75
+	d4 := 10
 
 	t.Run("Set point into empty leaf node into basic octree", func(t *testing.T) {
 		basicOct, err := createNewOctree(center, side)
@@ -119,17 +123,15 @@ func TestBasicOctreeSet(t *testing.T) {
 		basicOct, err := createNewOctree(center, side)
 		test.That(t, err, test.ShouldBeNil)
 
-		d1 := 1
 		err = basicOct.Set(r3.Vector{X: 0, Y: 0, Z: 0}, NewValueData(d1))
 		test.That(t, err, test.ShouldBeNil)
 		mp := basicOct.MaxVal()
 		test.That(t, mp, test.ShouldEqual, d1)
 
-		d2 := 2
 		err = basicOct.Set(r3.Vector{X: -.5, Y: 0, Z: 0}, NewValueData(d2))
 		test.That(t, err, test.ShouldBeNil)
 		test.That(t, basicOct.node.nodeType, test.ShouldResemble, internalNode)
-		test.That(t, basicOct.Size(), test.ShouldEqual, 2)
+		test.That(t, basicOct.Size(), test.ShouldEqual, side)
 		mp = basicOct.node.children[0].MaxVal()
 		test.That(t, mp, test.ShouldEqual, int(math.Max(float64(d1), float64(d2))))
 		mp = basicOct.MaxVal()
@@ -142,19 +144,16 @@ func TestBasicOctreeSet(t *testing.T) {
 		basicOct, err := createNewOctree(center, side)
 		test.That(t, err, test.ShouldBeNil)
 
-		d3 := 3
 		err = basicOct.Set(r3.Vector{X: 0, Y: 0, Z: 0}, NewValueData(d3))
 		test.That(t, err, test.ShouldBeNil)
 		mp := basicOct.MaxVal()
 		test.That(t, mp, test.ShouldEqual, d3)
 
-		d2 := 2
 		err = basicOct.Set(r3.Vector{X: -.5, Y: 0, Z: 0}, NewValueData(d2))
 		test.That(t, err, test.ShouldBeNil)
 		mp = basicOct.node.children[0].MaxVal()
 		test.That(t, mp, test.ShouldEqual, int(math.Max(float64(d2), float64(d3))))
 
-		d4 := 4
 		err = basicOct.Set(r3.Vector{X: -.4, Y: 0, Z: 0}, NewValueData(d4))
 		test.That(t, err, test.ShouldBeNil)
 		test.That(t, basicOct.node.nodeType, test.ShouldResemble, internalNode)
@@ -180,13 +179,11 @@ func TestBasicOctreeSet(t *testing.T) {
 		basicOct, err := createNewOctree(center, side)
 		test.That(t, err, test.ShouldBeNil)
 
-		d1 := 1
 		err = basicOct.Set(r3.Vector{X: 0, Y: 0, Z: 0}, NewValueData(d1))
 		test.That(t, err, test.ShouldBeNil)
 		mp := basicOct.MaxVal()
 		test.That(t, mp, test.ShouldEqual, d1)
 
-		d2 := 2
 		err = basicOct.Set(r3.Vector{X: -.5, Y: 0, Z: 0}, NewValueData(d2))
 		test.That(t, err, test.ShouldBeNil)
 		test.That(t, basicOct.size, test.ShouldEqual, 2)
@@ -200,14 +197,12 @@ func TestBasicOctreeSet(t *testing.T) {
 		basicOct, err := createNewOctree(center, side)
 		test.That(t, err, test.ShouldBeNil)
 
-		d1 := 1
 		err = basicOct.Set(r3.Vector{X: 0, Y: 0, Z: 0}, NewValueData(d1))
 		test.That(t, err, test.ShouldBeNil)
 		test.That(t, basicOct.node.point.D.Value(), test.ShouldEqual, d1)
 		mp := basicOct.MaxVal()
 		test.That(t, mp, test.ShouldEqual, d1)
 
-		d2 := 2
 		err = basicOct.Set(r3.Vector{X: 0, Y: 0, Z: 0}, NewValueData(d2))
 		test.That(t, err, test.ShouldBeNil)
 		test.That(t, basicOct.node.point.D.Value(), test.ShouldEqual, d2)
@@ -243,7 +238,6 @@ func TestBasicOctreeSet(t *testing.T) {
 
 		basicOct = createLopsidedOctree(basicOct, 0, maxRecursionDepth-1)
 
-		d1 := 1
 		err = basicOct.Set(r3.Vector{X: -1, Y: -1, Z: -1}, NewValueData(d1))
 		test.That(t, err, test.ShouldBeNil)
 		mp := basicOct.MaxVal()
@@ -296,9 +290,9 @@ func TestBasicOctreeAt(t *testing.T) {
 		test.That(t, err, test.ShouldBeNil)
 
 		pointsAndData := []PointAndData{
-			{P: r3.Vector{X: 0, Y: 0, Z: 0}, D: NewValueData(1)},
-			{P: r3.Vector{X: -.5, Y: 0, Z: 0}, D: NewValueData(2)},
-			{P: r3.Vector{X: -0.4, Y: 0, Z: 0}, D: NewValueData(3)},
+			{P: r3.Vector{X: 0, Y: 0, Z: 0}, D: NewValueData(51)},
+			{P: r3.Vector{X: -.5, Y: 0, Z: 0}, D: NewValueData(52)},
+			{P: r3.Vector{X: -0.4, Y: 0, Z: 0}, D: NewValueData(53)},
 		}
 
 		err = addPoints(basicOct, pointsAndData)
@@ -393,9 +387,9 @@ func TestBasicOctreeIterate(t *testing.T) {
 		test.That(t, err, test.ShouldBeNil)
 
 		pointsAndData := []PointAndData{
-			{P: r3.Vector{X: 0, Y: 0, Z: 0}, D: NewValueData(1)},
-			{P: r3.Vector{X: .5, Y: 0, Z: 0}, D: NewValueData(2)},
-			{P: r3.Vector{X: .6, Y: 0, Z: 0}, D: NewValueData(1)},
+			{P: r3.Vector{X: 0, Y: 0, Z: 0}, D: NewValueData(50)},
+			{P: r3.Vector{X: .5, Y: 0, Z: 0}, D: NewValueData(51)},
+			{P: r3.Vector{X: .6, Y: 0, Z: 0}, D: NewValueData(50)},
 		}
 
 		err = addPoints(basicOct, pointsAndData)
@@ -414,7 +408,7 @@ func TestBasicOctreeIterate(t *testing.T) {
 		// Partial iteration - applies function to only first point
 		total = 0
 		basicOct.Iterate(0, 0, func(p r3.Vector, d Data) bool {
-			if d.Value() == 1 {
+			if d.Value() == pointsAndData[0].D.Value() {
 				total += d.Value()
 				return true
 			}
@@ -570,7 +564,7 @@ func TestBasicOctreePointcloudIngestion(t *testing.T) {
 	center := getCenterFromPcMetaData(startPC.MetaData())
 	maxSideLength := getMaxSideLengthFromPcMetaData(startPC.MetaData())
 
-	basicOct, err := NewBasicOctree(center, maxSideLength)
+	basicOct, err := NewBasicOctree(center, maxSideLength, defaultConfidenceThreshold)
 	test.That(t, err, test.ShouldBeNil)
 
 	startPC.Iterate(0, 0, func(p r3.Vector, d Data) bool {
@@ -633,21 +627,21 @@ func testPCDToBasicOctree(t *testing.T, artifactPath string) {
 	validateBasicOctree(t, basicOct, basicOct.center, basicOct.sideLength)
 }
 
-func createPopulatedOctree(sign int) (*BasicOctree, error) {
+func createExampleOctree() (*BasicOctree, error) {
 	center := r3.Vector{X: 0, Y: 0, Z: 0}
 	side := 2.0
 	octree, err := createNewOctree(center, side)
 	if err != nil {
 		return nil, err
 	}
 	pointsAndData := []PointAndData{
-		{P: r3.Vector{X: 0, Y: 0, Z: 0}, D: NewValueData(2 * sign)},
-		{P: r3.Vector{X: .5, Y: 0, Z: 0}, D: NewValueData(3 * sign)},
-		{P: r3.Vector{X: .5, Y: 0, Z: .5}, D: NewValueData(10 * sign)},
-		{P: r3.Vector{X: .5, Y: .5, Z: 0}, D: NewValueData(1 * sign)},
-		{P: r3.Vector{X: .55, Y: .55, Z: 0}, D: NewValueData(4 * sign)},
-		{P: r3.Vector{X: -.55, Y: -.55, Z: 0}, D: NewValueData(5 * sign)},
-		{P: r3.Vector{X: .755, Y: .755, Z: 0}, D: NewValueData(6 * sign)},
+		{P: r3.Vector{X: 0, Y: 0, Z: 0}, D: NewValueData(52)},
+		{P: r3.Vector{X: .5, Y: 0, Z: 0}, D: NewValueData(53)},
+		{P: r3.Vector{X: .5, Y: 0, Z: .5}, D: NewValueData(60)},
+		{P: r3.Vector{X: .5, Y: .5, Z: 0}, D: NewValueData(51)},
+		{P: r3.Vector{X: .55, Y: .55, Z: 0}, D: NewValueData(54)},
+		{P: r3.Vector{X: -.55, Y: -.55, Z: 0}, D: NewValueData(55)},
+		{P: r3.Vector{X: .755, Y: .755, Z: 0}, D: NewValueData(56)},
 	}
 
 	err = addPoints(octree, pointsAndData)
@@ -659,16 +653,16 @@ func createPopulatedOctree(sign int) (*BasicOctree, error) {
 
 func TestCachedMaxProbability(t *testing.T) {
 	t.Run("get the max val from an octree", func(t *testing.T) {
-		octree, err := createPopulatedOctree(1)
+		octree, err := createExampleOctree()
 		test.That(t, err, test.ShouldBeNil)
 
 		validateBasicOctree(t, octree, octree.center, octree.sideLength)
 
 		mp := octree.MaxVal()
-		test.That(t, mp, test.ShouldEqual, 10)
+		test.That(t, mp, test.ShouldEqual, 60)
 
 		mp = octree.node.children[0].MaxVal()
-		test.That(t, mp, test.ShouldEqual, 5)
+		test.That(t, mp, test.ShouldEqual, 55)
 	})
 
 	t.Run("cannot set arbitrary values into the octree", func(t *testing.T) {
@@ -678,23 +672,10 @@ func TestCachedMaxProbability(t *testing.T) {
 			children: nil,
 			nodeType: leafNodeFilled,
 			point:    &PointAndData{P: r3.Vector{1, 2, 3}, D: d},
-			maxVal:   emptyProb,
+			maxVal:   defaultConfidenceThreshold,
 		}
 		test.That(t, node, test.ShouldResemble, filledNode)
 	})
-
-	t.Run("setting negative values", func(t *testing.T) {
-		octree, err := createPopulatedOctree(-1)
-		test.That(t, err, test.ShouldBeNil)
-
-		validateBasicOctree(t, octree, octree.center, octree.sideLength)
-
-		mp := octree.MaxVal()
-		test.That(t, mp, test.ShouldEqual, -1)
-
-		mp = octree.node.children[0].MaxVal()
-		test.That(t, mp, test.ShouldEqual, -2)
-	})
 }
 
 // Test the various geometry-specific interface methods.