robocars/robocar-steering
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Corrector implementation

Browse Source
This commit is contained in:
Cyrille Nofficial 2022-08-27 19:03:09 +02:00
parent bd4c5d45a4
commit efbcac602a
4 changed files with 144 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
pkg/steering/bbox.go
View File

@ -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,
}
}

74
pkg/steering/bbox_test.go
View File

@ -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)
}
})
}
}

30
pkg/steering/corrector.go
View File

@ -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) {

13
pkg/steering/corrector_test.go
View File

@ -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)
}