[RSDK-3647]Renaming Radius Clustering to Obstacles Pointcloud (viamro…

…botics#2664)

services/vision/radiusclustering/radius_clustering.go → services/vision/obstaclespointcloud/obstacles_pointcloud.go

@@ -1,6 +1,6 @@
-// Package radiusclustering uses the 3D radius clustering algorithm as defined in the
+// Package obstaclespointcloud uses the 3D radius clustering algorithm as defined in the
 // RDK vision/segmentation package as vision model.
-package radiusclustering
+package obstaclespointcloud
 
 import (
 	"context"
@@ -16,7 +16,7 @@ import (
 	"go.viam.com/rdk/vision/segmentation"
 )
 
-var model = resource.DefaultModelFamily.WithModel("radius_clustering_segmenter")
+var model = resource.DefaultModelFamily.WithModel("obstacles_pointcloud")
 
 func init() {
 	resource.RegisterService(vision.API, model, resource.Registration[vision.Service, *segmentation.RadiusClusteringConfig]{
@@ -29,26 +29,26 @@ func init() {
 			if err != nil {
 				return nil, err
 			}
-			return registerRCSegmenter(ctx, c.ResourceName(), attrs, actualR)
+			return registerOPSegmenter(ctx, c.ResourceName(), attrs, actualR)
 		},
 	})
 }
 
-// registerRCSegmenter creates a new 3D radius clustering segmenter from the config.
-func registerRCSegmenter(
+// registerOPSegmenter creates a new 3D radius clustering segmenter from the config.
+func registerOPSegmenter(
 	ctx context.Context,
 	name resource.Name,
 	conf *segmentation.RadiusClusteringConfig,
 	r robot.Robot,
 ) (vision.Service, error) {
-	_, span := trace.StartSpan(ctx, "service::vision::registerRadiusClustering")
+	_, span := trace.StartSpan(ctx, "service::vision::registerObstaclesPointcloud")
 	defer span.End()
 	if conf == nil {
-		return nil, errors.New("config for radius clustering segmenter cannot be nil")
+		return nil, errors.New("config for obstacles pointcloud segmenter cannot be nil")
 	}
 	err := conf.CheckValid()
 	if err != nil {
-		return nil, errors.Wrap(err, "radius clustering segmenter config error")
+		return nil, errors.Wrap(err, "obstacles pointcloud segmenter config error")
 	}
 	segmenter := segmentation.Segmenter(conf.RadiusClustering)
 	return vision.NewService(name, r, nil, nil, nil, segmenter)

services/vision/radiusclustering/radius_clustering_test.go → services/vision/obstaclespointcloud/obstacles_pointcloud_test.go

@@ -1,4 +1,4 @@
-package radiusclustering
+package obstaclespointcloud
 
 import (
 	"context"
@@ -36,22 +36,23 @@ func TestRadiusClusteringSegmentation(t *testing.T) {
 	params := &segmentation.RadiusClusteringConfig{
 		MinPtsInPlane:      100,
 		MinPtsInSegment:    3,
+		MaxDistFromPlane:   10,
 		ClusteringRadiusMm: 5.,
 		MeanKFiltering:     10.,
 	}
 	// bad registration, no parameters
 	name := vision.Named("test_rcs")
-	_, err := registerRCSegmenter(context.Background(), name, nil, r)
+	_, err := registerOPSegmenter(context.Background(), name, nil, r)
 	test.That(t, err, test.ShouldNotBeNil)
 	test.That(t, err.Error(), test.ShouldContainSubstring, "cannot be nil")
 	// bad registration, parameters out of bounds
 	params.ClusteringRadiusMm = -3.0
-	_, err = registerRCSegmenter(context.Background(), name, params, r)
+	_, err = registerOPSegmenter(context.Background(), name, params, r)
 	test.That(t, err, test.ShouldNotBeNil)
 	test.That(t, err.Error(), test.ShouldContainSubstring, "segmenter config error")
 	// successful registration
 	params.ClusteringRadiusMm = 5.0
-	seg, err := registerRCSegmenter(context.Background(), name, params, r)
+	seg, err := registerOPSegmenter(context.Background(), name, params, r)
 	test.That(t, err, test.ShouldBeNil)
 	test.That(t, seg.Name(), test.ShouldResemble, name)
 

services/vision/register/register.go

@@ -7,5 +7,5 @@ import (
 	_ "go.viam.com/rdk/services/vision/detectionstosegments"
 	_ "go.viam.com/rdk/services/vision/mlvision"
 	_ "go.viam.com/rdk/services/vision/obstacledistance"
-	_ "go.viam.com/rdk/services/vision/radiusclustering"
+	_ "go.viam.com/rdk/services/vision/obstaclespointcloud"
 )

vision/segmentation/plane_segmentation.go

@@ -66,6 +66,59 @@ func pointCloudSplit(cloud pc.PointCloud, inMap map[r3.Vector]bool) (pc.PointClo
 	return mapCloud, nonMapCloud, nil
 }
 
+// SegmentPlaneWRTGround segments the biggest 'ground' plane in the 3D Pointcloud.
+// nIterations is the number of iteration for ransac
+// nIter to choose? nIter = log(1-p)/log(1-(1-e)^s), where p is prob of success, e is outlier ratio, s is subset size (3 for plane).
+// dstThreshold is the float64 value for the maximum allowed distance to the found plane for a point to belong to it
+// This function returns a Plane struct, as well as the remaining points in a pointcloud
+// It also returns the equation of the found plane: [0]x + [1]y + [2]z + [3] = 0.
+// angleThrehold is the maximum acceptable angle between the groundVec and angle of the plane,
+// if the plane is at a larger angle than maxAngle, it will not be considered for segmentation, if set to 0 then not considered
+// normalVec is the normal vector of the plane representing the ground.
+func SegmentPlaneWRTGround(ctx context.Context, cloud pc.PointCloud, nIterations int, angleThreshold,
+	dstThreshold float64, normalVec r3.Vector,
+) (pc.Plane, pc.PointCloud, error) {
+	if cloud.Size() <= 3 { // if point cloud does not have even 3 points, return original cloud with no planes
+		return pc.NewEmptyPlane(), cloud, nil
+	}
+	//nolint:gosec
+	r := rand.New(rand.NewSource(1))
+	pts := GetPointCloudPositions(cloud)
+	nPoints := cloud.Size()
+
+	// First get all equations
+	equations := make([][4]float64, 0, nIterations)
+	for i := 0; i < nIterations; i++ {
+		// sample 3 Points from the slice of 3D Points
+		n1, n2, n3 := utils.SampleRandomIntRange(1, nPoints-1, r),
+			utils.SampleRandomIntRange(1, nPoints-1, r),
+			utils.SampleRandomIntRange(1, nPoints-1, r)
+		p1, p2, p3 := pts[n1], pts[n2], pts[n3]
+
+		// get 2 vectors that are going to define the plane
+		v1 := p2.Sub(p1)
+		v2 := p3.Sub(p1)
+		// cross product to get the normal unit vector to the plane (v1, v2)
+		cross := v1.Cross(v2)
+		planeVec := cross.Normalize()
+		// find current plane equation denoted as:
+		// cross[0]*x + cross[1]*y + cross[2]*z + d = 0
+		// to find d, we just need to pick a point and deduce d from the plane equation (vec orth to p1, p2, p3)
+		d := -planeVec.Dot(p2)
+
+		currentEquation := [4]float64{planeVec.X, planeVec.Y, planeVec.Z, d}
+
+		if angleThreshold != 0 {
+			if math.Acos(normalVec.Dot(planeVec)) <= angleThreshold*math.Pi/180.0 {
+				equations = append(equations, currentEquation)
+			}
+		} else {
+			equations = append(equations, currentEquation)
+		}
+	}
+	return findBestEq(ctx, cloud, len(equations), equations, pts, dstThreshold)
+}
+
 // SegmentPlane segments the biggest plane in the 3D Pointcloud.
 // nIterations is the number of iteration for ransac
 // nIter to choose? nIter = log(1-p)/log(1-(1-e)^s), where p is prob of success, e is outlier ratio, s is subset size (3 for plane).
@@ -106,6 +159,12 @@ func SegmentPlane(ctx context.Context, cloud pc.PointCloud, nIterations int, thr
 		equations = append(equations, currentEquation)
 	}
 
+	return findBestEq(ctx, cloud, nIterations, equations, pts, threshold)
+}
+
+func findBestEq(ctx context.Context, cloud pc.PointCloud, nIterations int, equations [][4]float64,
+	pts []r3.Vector, threshold float64,
+) (pc.Plane, pc.PointCloud, error) {
 	// Then find the best equation in parallel. It ends up being faster to loop
 	// by equations (iterations) and then points due to what I (erd) think is
 	// memory locality exploitation.
@@ -156,6 +215,8 @@ func SegmentPlane(ctx context.Context, cloud pc.PointCloud, nIterations int, thr
 	}
 	bestEquation = bestResults[bestIdx].equation
 
+	nPoints := cloud.Size()
+
 	planeCloud := pc.NewWithPrealloc(bestInliers)
 	nonPlaneCloud := pc.NewWithPrealloc(nPoints - bestInliers)
 	planeCloudCenter := r3.Vector{}
@@ -188,27 +249,48 @@ func SegmentPlane(ctx context.Context, cloud pc.PointCloud, nIterations int, thr
 // PlaneSegmentation is an interface used to find geometric planes in a 3D space.
 type PlaneSegmentation interface {
 	FindPlanes(ctx context.Context) ([]pc.Plane, pc.PointCloud, error)
+	FindGroundPlane(ctx context.Context) (pc.Plane, pc.PointCloud, error)
 }
 
 type pointCloudPlaneSegmentation struct {
-	cloud       pc.PointCloud
-	threshold   float64
-	minPoints   int
-	nIterations int
+	cloud             pc.PointCloud
+	distanceThreshold float64
+	minPoints         int
+	nIterations       int
+	angleThreshold    float64
+	normalVec         r3.Vector
 }
 
 // NewPointCloudPlaneSegmentation initializes the plane segmentation with the necessary parameters to find the planes
 // threshold is the float64 value for the maximum allowed distance to the found plane for a point to belong to it.
 // minPoints is the minimum number of points necessary to be considered a plane.
 func NewPointCloudPlaneSegmentation(cloud pc.PointCloud, threshold float64, minPoints int) PlaneSegmentation {
-	return &pointCloudPlaneSegmentation{cloud, threshold, minPoints, 2000}
+	return &pointCloudPlaneSegmentation{
+		cloud:             cloud,
+		distanceThreshold: threshold,
+		minPoints:         minPoints,
+		nIterations:       2000,
+		angleThreshold:    0,
+		normalVec:         r3.Vector{X: 0, Y: 0, Z: 1},
+	}
+}
+
+// NewPointCloudGroundPlaneSegmentation initializes the plane segmentation with the necessary parameters to find
+// ground like planes, meaning they are less than angleThreshold away from the plane corresponding to normaLVec
+// distanceThreshold is the float64 value for the maximum allowed distance to the found plane for a
+// point to belong to it.
+// minPoints is the minimum number of points necessary to be considered a plane.
+func NewPointCloudGroundPlaneSegmentation(cloud pc.PointCloud, distanceThreshold float64, minPoints int,
+	angleThreshold float64, normalVec r3.Vector,
+) PlaneSegmentation {
+	return &pointCloudPlaneSegmentation{cloud, distanceThreshold, minPoints, 2000, angleThreshold, normalVec}
 }
 
 // FindPlanes takes in a point cloud and outputs an array of the planes and a point cloud of the leftover points.
 func (pcps *pointCloudPlaneSegmentation) FindPlanes(ctx context.Context) ([]pc.Plane, pc.PointCloud, error) {
 	planes := make([]pc.Plane, 0)
 	var err error
-	plane, nonPlaneCloud, err := SegmentPlane(ctx, pcps.cloud, pcps.nIterations, pcps.threshold)
+	plane, nonPlaneCloud, err := SegmentPlane(ctx, pcps.cloud, pcps.nIterations, pcps.distanceThreshold)
 	if err != nil {
 		return nil, nil, err
 	}
@@ -223,7 +305,7 @@ func (pcps *pointCloudPlaneSegmentation) FindPlanes(ctx context.Context) ([]pc.P
 	var lastNonPlaneCloud pc.PointCloud
 	for {
 		lastNonPlaneCloud = nonPlaneCloud
-		smallerPlane, smallerNonPlaneCloud, err := SegmentPlane(ctx, nonPlaneCloud, pcps.nIterations, pcps.threshold)
+		smallerPlane, smallerNonPlaneCloud, err := SegmentPlane(ctx, nonPlaneCloud, pcps.nIterations, pcps.distanceThreshold)
 		if err != nil {
 			return nil, nil, err
 		}
@@ -243,6 +325,24 @@ func (pcps *pointCloudPlaneSegmentation) FindPlanes(ctx context.Context) ([]pc.P
 	return planes, nonPlaneCloud, nil
 }
 
+// FindGroundPlane takes in a point cloud and outputs an array of a ground like plane and a point cloud of the leftover points.
+func (pcps *pointCloudPlaneSegmentation) FindGroundPlane(ctx context.Context) (pc.Plane, pc.PointCloud, error) {
+	var err error
+	plane, nonPlaneCloud, err := SegmentPlaneWRTGround(ctx, pcps.cloud, pcps.nIterations,
+		pcps.angleThreshold, pcps.distanceThreshold, pcps.normalVec)
+	if err != nil {
+		return nil, nil, err
+	}
+	planeCloud, err := plane.PointCloud()
+	if err != nil {
+		return nil, nil, err
+	}
+	if planeCloud.Size() <= pcps.minPoints {
+		return nil, pcps.cloud, nil
+	}
+	return plane, nonPlaneCloud, nil
+}
+
 // VoxelGridPlaneConfig contains the parameters needed to create a Plane from a VoxelGrid.
 type VoxelGridPlaneConfig struct {
 	WeightThresh   float64 `json:"weight_threshold"`
@@ -288,6 +388,11 @@ func (vgps *voxelGridPlaneSegmentation) FindPlanes(ctx context.Context) ([]pc.Pl
 	return planes, nonPlaneCloud, nil
 }
 
+// FindGroundPlane is yet to be implemented.
+func (vgps *voxelGridPlaneSegmentation) FindGroundPlane(ctx context.Context) (pc.Plane, pc.PointCloud, error) {
+	return nil, nil, errors.New("function not yet implemented")
+}
+
 // SplitPointCloudByPlane divides the point cloud in two point clouds, given the equation of a plane.
 // one point cloud will have all the points above the plane and the other with all the points below the plane.
 // Points exactly on the plane are not included!

vision/segmentation/plane_segmentation_test.go

@@ -24,7 +24,7 @@ func init() {
 func TestPlaneConfig(t *testing.T) {
 	cfg := VoxelGridPlaneConfig{}
 	// invalid weight threshold
-	cfg.WeightThresh = -1.
+	cfg.WeightThresh = -2.
 	err := cfg.CheckValid()
 	test.That(t, err.Error(), test.ShouldContainSubstring, "weight_threshold cannot be less than 0")
 	// invalid angle threshold
@@ -48,8 +48,40 @@ func TestPlaneConfig(t *testing.T) {
 	test.That(t, err, test.ShouldBeNil)
 }
 
+func TestSegmentPlaneWRTGround(t *testing.T) {
+	// get depth map
+	d, err := rimage.NewDepthMapFromFile(
+		context.Background(),
+		artifact.MustPath("vision/segmentation/pointcloudsegmentation/align-test-1615172036.png"))
+	test.That(t, err, test.ShouldBeNil)
+
+	// Pixel to Meter
+	sensorParams, err := transform.NewDepthColorIntrinsicsExtrinsicsFromJSONFile(intel515ParamsPath)
+	test.That(t, err, test.ShouldBeNil)
+	depthIntrinsics := &sensorParams.DepthCamera
+	cloud := depthadapter.ToPointCloud(d, depthIntrinsics)
+	test.That(t, err, test.ShouldBeNil)
+	// Segment Plane
+	nIter := 3000
+	groundNormVec := r3.Vector{0, 1, 0}
+	angleThresh := 30.0
+	plane, _, err := SegmentPlaneWRTGround(context.Background(), cloud, nIter, angleThresh, 0.5, groundNormVec)
+	eq := plane.Equation()
+	test.That(t, err, test.ShouldBeNil)
+
+	p1 := r3.Vector{-eq[3] / eq[0], 0, 0}
+	p2 := r3.Vector{0, -eq[3] / eq[1], 0}
+	p3 := r3.Vector{0, 0, -eq[3] / eq[2]}
+
+	v1 := p2.Sub(p1).Normalize()
+	v2 := p3.Sub(p1).Normalize()
+
+	planeNormVec := v1.Cross(v2)
+	planeNormVec = planeNormVec.Normalize()
+	test.That(t, math.Acos(planeNormVec.Dot(groundNormVec)), test.ShouldBeLessThanOrEqualTo, angleThresh*math.Pi/180)
+}
+
 func TestSegmentPlane(t *testing.T) {
-	t.Parallel()
 	// Intel Sensor Extrinsic data from manufacturer
 	// Intel sensor depth 1024x768 to  RGB 1280x720
 	// Translation Vector : [-0.000828434,0.0139185,-0.0033418]

vision/segmentation/radius_clustering.go

@@ -17,11 +17,14 @@ import (
 // RadiusClusteringConfig specifies the necessary parameters for 3D object finding.
 type RadiusClusteringConfig struct {
 	resource.TriviallyValidateConfig
-	MinPtsInPlane      int     `json:"min_points_in_plane"`
-	MinPtsInSegment    int     `json:"min_points_in_segment"`
-	ClusteringRadiusMm float64 `json:"clustering_radius_mm"`
-	MeanKFiltering     int     `json:"mean_k_filtering"`
-	Label              string  `json:"label,omitempty"`
+	MinPtsInPlane      int       `json:"min_points_in_plane"`
+	MaxDistFromPlane   float64   `json:"max_dist_from_plane_mm"`
+	NormalVec          r3.Vector `json:"ground_plane_normal_vec"`
+	AngleTolerance     float64   `json:"ground_angle_tolerance_degs"`
+	MinPtsInSegment    int       `json:"min_points_in_segment"`
+	ClusteringRadiusMm float64   `json:"clustering_radius_mm"`
+	MeanKFiltering     int       `json:"mean_k_filtering"`
+	Label              string    `json:"label,omitempty"`
 }
 
 // CheckValid checks to see in the input values are valid.
@@ -35,6 +38,18 @@ func (rcc *RadiusClusteringConfig) CheckValid() error {
 	if rcc.ClusteringRadiusMm <= 0 {
 		return errors.Errorf("clustering_radius_mm must be greater than 0, got %v", rcc.ClusteringRadiusMm)
 	}
+	if rcc.MaxDistFromPlane == 0 {
+		rcc.MaxDistFromPlane = 100
+	}
+	if rcc.MaxDistFromPlane <= 0 {
+		return errors.Errorf("max_dist_from_plane must be greater than 0, got %v", rcc.MaxDistFromPlane)
+	}
+	if rcc.AngleTolerance > 180 || rcc.AngleTolerance < 0 {
+		return errors.Errorf("max_angle_of_plane must between 0 & 180 (inclusive), got %v", rcc.AngleTolerance)
+	}
+	if rcc.NormalVec.Norm2() == 0 {
+		rcc.NormalVec = r3.Vector{X: 0, Y: 0, Z: 1}
+	}
 	return nil
 }
 
@@ -74,9 +89,9 @@ func (rcc *RadiusClusteringConfig) RadiusClustering(ctx context.Context, src cam
 	if err != nil {
 		return nil, err
 	}
-	ps := NewPointCloudPlaneSegmentation(cloud, 10, rcc.MinPtsInPlane)
+	ps := NewPointCloudGroundPlaneSegmentation(cloud, rcc.MaxDistFromPlane, rcc.MinPtsInPlane, rcc.AngleTolerance, rcc.NormalVec)
 	// if there are found planes, remove them, and keep all the non-plane points
-	_, nonPlane, err := ps.FindPlanes(ctx)
+	_, nonPlane, err := ps.FindGroundPlane(ctx)
 	if err != nil {
 		return nil, err
 	}