From efbcac602a41e3640054e1e23d1982ae317b35d9 Mon Sep 17 00:00:00 2001 From: Cyrille Nofficial Date: Sat, 27 Aug 2022 19:03:09 +0200 Subject: [PATCH] Corrector implementation --- pkg/steering/bbox.go | 41 ++++++++++++++++++ pkg/steering/bbox_test.go | 78 +++++++++++++++++++++++++++++++++- pkg/steering/corrector.go | 34 +++++++++------ pkg/steering/corrector_test.go | 13 +++--- 4 files changed, 144 insertions(+), 22 deletions(-) diff --git a/pkg/steering/bbox.go b/pkg/steering/bbox.go index 3a9db76..05dbb34 100644 --- a/pkg/steering/bbox.go +++ b/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, + } +} diff --git a/pkg/steering/bbox_test.go b/pkg/steering/bbox_test.go index b85e106..d760b19 100644 --- a/pkg/steering/bbox_test.go +++ b/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" @@ -24,14 +25,16 @@ type BBox struct { } var ( - dataBBoxes map[string][]image.Rectangle - dataImages map[string]*gocv.Mat + dataBBoxes map[string][]image.Rectangle + dataObjects map[string][]*events.Object + dataImages map[string]*gocv.Mat ) 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) + } + }) + } +} diff --git a/pkg/steering/corrector.go b/pkg/steering/corrector.go index af863e6..fead644 100644 --- a/pkg/steering/corrector.go +++ b/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 + 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) { diff --git a/pkg/steering/corrector_test.go b/pkg/steering/corrector_test.go index 333b4e2..19a9b8e 100644 --- a/pkg/steering/corrector_test.go +++ b/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{ - gridMap: &defaultGridMap, - objectMoveFactors: &defaultObjectFactors, - } + c := NewCorrector(&defaultGridMap, &defaultObjectFactors) if got := c.AdjustFromObjectPosition(tt.args.currentSteering, tt.args.objects); got != tt.want { t.Errorf("AdjustFromObjectPosition() = %v, want %v", got, tt.want) }