Corrector implementation

pkg/steering/bbox.go

@@ -1,6 +1,7 @@
 package steering
 
 import (
+	"github.com/cyrilix/robocar-protobuf/go/events"
 	"gocv.io/x/gocv"
 	"image"
 )
@@ -14,3 +15,43 @@ func GroupBBoxes(bboxes []image.Rectangle) []image.Rectangle {
 	}
 	return gocv.GroupRectangles(bboxes, 1, 0.2)
 }
+func GroupObjects(objects []*events.Object, imgWidth, imgHeight int) []*events.Object {
+	if len(objects) == 0 {
+		return []*events.Object{}
+	}
+	if len(objects) == 1 {
+		return []*events.Object{objects[0]}
+	}
+
+	rectangles := make([]image.Rectangle, 0, len(objects))
+	for _, o := range objects {
+		rectangles = append(rectangles, *objectToRect(o, imgWidth, imgHeight))
+	}
+	grp := gocv.GroupRectangles(rectangles, 1, 0.2)
+	result := make([]*events.Object, 0, len(grp))
+	for _, r := range grp {
+		result = append(result, rectToObject(&r, imgWidth, imgHeight))
+	}
+	return result
+}
+
+func objectToRect(object *events.Object, imgWidth, imgHeight int) *image.Rectangle {
+	r := image.Rect(
+		int(object.Left*float32(imgWidth)),
+		int(object.Top*float32(imgHeight)),
+		int(object.Right*float32(imgWidth)),
+		int(object.Bottom*float32(imgHeight)),
+	)
+	return &r
+}
+
+func rectToObject(r *image.Rectangle, imgWidth, imgHeight int) *events.Object {
+	return &events.Object{
+		Type:       events.TypeObject_ANY,
+		Left:       float32(r.Min.X) / float32(imgWidth),
+		Top:        float32(r.Min.Y) / float32(imgHeight),
+		Right:      float32(r.Max.X) / float32(imgWidth),
+		Bottom:     float32(r.Max.Y) / float32(imgHeight),
+		Confidence: -1,
+	}
+}

pkg/steering/bbox_test.go

@@ -3,6 +3,7 @@ package steering
 import (
 	"encoding/json"
 	"fmt"
+	"github.com/cyrilix/robocar-protobuf/go/events"
 	"go.uber.org/zap"
 	"gocv.io/x/gocv"
 	"image"
@@ -25,6 +26,7 @@ type BBox struct {
 
 var (
 	dataBBoxes  map[string][]image.Rectangle
+	dataObjects map[string][]*events.Object
 	dataImages  map[string]*gocv.Mat
 )
 
@@ -32,6 +34,7 @@ func init() {
 	// TODO: empty img without bbox
 	dataNames := []string{"01", "02", "03", "04"}
 	dataBBoxes = make(map[string][]image.Rectangle, len(dataNames))
+	dataObjects = make(map[string][]*events.Object, len(dataNames))
 	dataImages = make(map[string]*gocv.Mat, len(dataNames))
 
 	for _, dataName := range dataNames {
@@ -40,6 +43,7 @@ func init() {
 			zap.S().Panicf("unable to load data test: %v", err)
 		}
 		dataBBoxes[dataName] = bboxesToRectangles(bb, img.Cols(), img.Rows())
+		dataObjects[dataName] = bboxesToObjects(bb)
 		dataImages[dataName] = img
 	}
 }
@@ -52,6 +56,20 @@ func bboxesToRectangles(bboxes []BBox, imgWidth, imgHeiht int) []image.Rectangle
 	return rects
 }
 
+func bboxesToObjects(bboxes []BBox) []*events.Object {
+	objects := make([]*events.Object, 0, len(bboxes))
+	for _, bb := range bboxes {
+		objects = append(objects, &events.Object{
+			Type:       events.TypeObject_ANY,
+			Left:       bb.Left,
+			Top:        bb.Top,
+			Right:      bb.Right,
+			Bottom:     bb.Bottom,
+			Confidence: bb.Confidence,
+		})
+	}
+	return objects
+}
 func (bb *BBox) toRect(imgWidth, imgHeight int) image.Rectangle {
 	return image.Rect(
 		int(bb.Left*float32(imgWidth)),
@@ -228,3 +246,59 @@ func TestGroupBBoxes(t *testing.T) {
 		})
 	}
 }
+func TestGroupObjects(t *testing.T) {
+	type args struct {
+		dataName string
+	}
+	tests := []struct {
+		name string
+		args args
+		want []*events.Object
+	}{
+		{
+			name: "groupbbox-01",
+			args: args{
+				dataName: "01",
+			},
+			want: []*events.Object{
+				{Left: 0.26660156, Top: 0.1706543, Right: 0.5258789, Bottom: 0.47583008, Confidence: 0.4482422},
+			},
+		},
+		{
+			name: "groupbbox-02",
+			args: args{
+				dataName: "02",
+			},
+			want: []*events.Object{
+				{Left: 0.15625, Top: 0.108333334, Right: 0.6875, Bottom: 0.6666667, Confidence: -1},
+			},
+		},
+		{
+			name: "groupbbox-03",
+			args: args{
+				dataName: "03",
+			},
+			want: []*events.Object{
+				{Top: 0.14166667, Right: 0.21875, Bottom: 0.64166665, Confidence: -1},
+			},
+		},
+		{
+			name: "groupbbox-04",
+			args: args{
+				dataName: "04",
+			},
+			want: []*events.Object{
+				{Left: 0.80625, Top: 0.083333336, Right: 0.99375, Bottom: 0.53333336, Confidence: -1},
+			},
+		},
+	}
+	for _, tt := range tests {
+		t.Run(tt.name, func(t *testing.T) {
+			img := dataImages[tt.args.dataName]
+			got := GroupObjects(dataObjects[tt.args.dataName], img.Cols(), img.Rows())
+			if !reflect.DeepEqual(got, tt.want) {
+				t.Errorf("GroupObjects() = %v, want %v", got, tt.want)
+			}
+		})
+	}
+}

pkg/steering/corrector.go

@@ -8,9 +8,21 @@ import (
 	"os"
 )
 
+func NewCorrector(gridMap *GridMap, objectMoveFactors *GridMap) *Corrector {
+	return &Corrector{
+		gridMap:           gridMap,
+		objectMoveFactors: objectMoveFactors,
+		deltaMiddle:       0.1,
+		imgWidth:          160,
+		imgHeight:         120,
+	}
+}
+
 type Corrector struct {
 	gridMap             *GridMap
 	objectMoveFactors   *GridMap
+	deltaMiddle         float64
+	imgWidth, imgHeight int
 }
 
 /*
@@ -50,23 +62,21 @@ AdjustFromObjectPosition modify steering value according object positions
     :   | ... | ... | ... | ... | ... | ... |
 */
 func (c *Corrector) AdjustFromObjectPosition(currentSteering float64, objects []*events.Object) float64 {
-	// TODO, group rectangle
-
-	var deltaMiddle = 0.1
-
 	if len(objects) == 0 {
 		return currentSteering
 	}
+	grpObjs := GroupObjects(objects, c.imgWidth, c.imgHeight)
+
 	// get nearest object
-	nearest, err := c.nearObject(objects)
+	nearest, err := c.nearObject(grpObjs)
 	if err != nil {
 		zap.S().Warnf("unexpected error on nearest seach object, ignore objects: %v", err)
 		return currentSteering
 	}
 
-	if currentSteering > -1*deltaMiddle && currentSteering < deltaMiddle {
+	if currentSteering > -1*c.deltaMiddle && currentSteering < c.deltaMiddle {
 		// Straight
-		return currentSteering + c.computeDeviation(currentSteering, nearest)
+		return currentSteering + c.computeDeviation(nearest)
 	} else {
 		// Turn to right or left, so search to avoid collision with objects on the right
 		// Apply factor to object to move it at middle. This factor is function of distance
@@ -83,7 +93,7 @@ func (c *Corrector) AdjustFromObjectPosition(currentSteering float64, objects []
 			Bottom:     nearest.Bottom,
 			Confidence: nearest.Confidence,
 		}
-		result := currentSteering + c.computeDeviation(currentSteering, &objMoved)
+		result := currentSteering + c.computeDeviation(&objMoved)
 		if result < -1. {
 			result = -1.
 		}
@@ -94,7 +104,7 @@ func (c *Corrector) AdjustFromObjectPosition(currentSteering float64, objects []
 	}
 }
 
-func (c *Corrector) computeDeviation(currentSteering float64, nearest *events.Object) float64 {
+func (c *Corrector) computeDeviation(nearest *events.Object) float64 {
 	var delta float64
 	var err error
 
@@ -110,7 +120,7 @@ func (c *Corrector) computeDeviation(currentSteering float64, nearest *events.Ob
 		zap.S().Warnf("unable to compute delta to apply to steering, skip correction: %v", err)
 		delta = 0
 	}
-	return currentSteering + delta
+	return delta
 }
 
 func (c *Corrector) nearObject(objects []*events.Object) (*events.Object, error) {

pkg/steering/corrector_test.go

@@ -155,7 +155,7 @@ func TestCorrector_AdjustFromObjectPosition(t *testing.T) {
 				currentSteering: -0.9,
 				objects:         []*events.Object{&objectOnMiddleNear},
 			},
-			want: -0.9,
+			want: -1,
 		},
 		{
 			name: "run to right with 1 near object",
@@ -163,7 +163,7 @@ func TestCorrector_AdjustFromObjectPosition(t *testing.T) {
 				currentSteering: 0.9,
 				objects:         []*events.Object{&objectOnMiddleNear},
 			},
-			want: 0.9,
+			want: 1.,
 		},
 		{
 			name: "run to right with 1 near object on the right",
@@ -171,7 +171,7 @@ func TestCorrector_AdjustFromObjectPosition(t *testing.T) {
 				currentSteering: 0.9,
 				objects:         []*events.Object{&objectOnRightNear},
 			},
-			want: 0.1,
+			want: 1.,
 		},
 		{
 			name: "run to left with 1 near object on the left",
@@ -179,15 +179,12 @@ func TestCorrector_AdjustFromObjectPosition(t *testing.T) {
 				currentSteering: -0.9,
 				objects:         []*events.Object{&objectOnLeftNear},
 			},
-			want: -0.1,
+			want: -0.65,
 		},
 	}
 	for _, tt := range tests {
 		t.Run(tt.name, func(t *testing.T) {
-			c := &Corrector{
+			c := NewCorrector(&defaultGridMap, &defaultObjectFactors)
-				gridMap:           &defaultGridMap,
-				objectMoveFactors: &defaultObjectFactors,
-			}
 			if got := c.AdjustFromObjectPosition(tt.args.currentSteering, tt.args.objects); got != tt.want {
 				t.Errorf("AdjustFromObjectPosition() = %v, want %v", got, tt.want)
 			}