diff --git a/pkg/steering/bbox_test.go b/pkg/steering/bbox_test.go index af58d91..b85e106 100644 --- a/pkg/steering/bbox_test.go +++ b/pkg/steering/bbox_test.go @@ -35,7 +35,7 @@ func init() { dataImages = make(map[string]*gocv.Mat, len(dataNames)) for _, dataName := range dataNames { - img, bb, err := load_data(dataName) + img, bb, err := loadData(dataName) if err != nil { zap.S().Panicf("unable to load data test: %v", err) } @@ -61,7 +61,7 @@ func (bb *BBox) toRect(imgWidth, imgHeight int) image.Rectangle { ) } -func load_data(dataName string) (*gocv.Mat, []BBox, error) { +func loadData(dataName string) (*gocv.Mat, []BBox, error) { contentBBoxes, err := os.ReadFile(fmt.Sprintf("test_data/bboxes-%s.json", dataName)) if err != nil { return nil, []BBox{}, fmt.Errorf("unable to load json file for bbox of '%v': %w", dataName, err) @@ -126,7 +126,7 @@ func saveImage(name string, img *gocv.Mat) error { } func DisplayImageAndBBoxes(dataName string) error { - img, bboxes, err := load_data(dataName) + img, bboxes, err := loadData(dataName) if err != nil { return fmt.Errorf("unable to load image and bboxes: %w", err) } diff --git a/pkg/steering/corrector.go b/pkg/steering/corrector.go index 734245a..22f2436 100644 --- a/pkg/steering/corrector.go +++ b/pkg/steering/corrector.go @@ -7,8 +7,37 @@ import ( ) type Corrector struct { + fixValues FixesTable } +/* +FixFromObjectPosition modify steering value according object positions + + 1. To compute steering correction, split in image in zones and define correction value for each zone + + Steering computed + : -1 -0.66 -0.33 0 0.33 0.66 1 + 0% |-----|-----|-----|-----|-----|-----| + : | 0 | 0 | 0 | 0 | 0 | 0 | + 20% |-----|-----|-----|-----|-----|-----| + : | 0 | 0 | 0 | 0 | 0 | 0 | + 40% |-----|-----|-----|-----|-----|-----| + : | 0 | 0 | 0.25|-0.25| 0 | 0 | + 60% |-----|-----|-----|-----|-----|-----| + : | 0 | 0.25| 0.5 |-0.5 |-0.25| 0 | + 80% |-----|-----|-----|-----|-----|-----| + : |-0.25|-0.5 | 1 | -1 |-0.5 | 0.25| + 100%|-----|-----|-----|-----|-----|-----| + + 2. For straight (current steering near of 0), search nearest object and if: + + * left and right values < 0: use correction from right value according image splitting + * left and right values > 0: use correction from left value according image splitting + * left < 0 and right values > 0: use (right + (right - left) / 2) value + + 3. If current steering != 0 (turn on left or right), shift right and left values proportionnaly to current steering and + apply 2. +*/ func (c *Corrector) FixFromObjectPosition(currentSteering float64, objects []*events.Object) float64 { // TODO, group rectangle @@ -22,6 +51,22 @@ func (c *Corrector) FixFromObjectPosition(currentSteering float64, objects []*ev return currentSteering } + if currentSteering > 0.1 && currentSteering < 0.1 { + + if nearest.Bottom < 0.4 { + // object is far, so no need to fix steering now + return currentSteering + } + + if nearest.Left < 0 && nearest.Right < 0 { + return currentSteering + c.fixValues.ValueOf(currentSteering, nearest.Right) + } + if nearest.Left > 0 && nearest.Right > 0 { + return currentSteering + c.fixValues.ValueOf(currentSteering, nearest.Left) + } + return currentSteering + c.fixValues.ValueOf(currentSteering, nearest.Left+(nearest.Right-nearest.Left)/2.) + } + // Search if current steering is near of Right or Left return currentSteering @@ -44,3 +89,28 @@ func (c *Corrector) nearObject(objects []*events.Object) (*events.Object, error) } return result, nil } + +type FixesTable struct { + values map[string]map[int]float64 +} + +func (f *FixesTable) ValueOf(steering float64, distance float32) float64 { + var key string + if steering < -0.66 { + key = "<-0.66" + } else if steering < -0.33 { + key = "-0.66:-0.33" + } else if steering < 0 { + key = "-0.33:0" + } else if steering < 0.33 { + key = "0:0.33" + } else if steering < 0.66 { + key = "0.33:0.66" + } else { + key = ">= 0.66" + } + + keyDistance := int(distance / 20) + return f.values[key][keyDistance] + +}