refactor: move package to pkg

pkg/part/opencv.go

@@ -0,0 +1,179 @@
+package part
+
+import (
+	"github.com/cyrilix/robocar-protobuf/go/events"
+	"go.uber.org/zap"
+	"gocv.io/x/gocv"
+	"image"
+	"image/color"
+)
+
+const FILLED = -1
+
+type RoadDetector struct {
+	kernelSize                               int
+	morphoIterations                         int
+	approxPolyEpsilonFactor                  float64
+	previousBoundingBox                      *image.Rectangle
+	previousRoad                             *[]image.Point
+	thresholdLowerBound, thresholdUpperBound gocv.Mat
+}
+
+func (rd *RoadDetector) Close() error {
+	var err error
+	err = nil
+	if err1 := rd.thresholdLowerBound.Close(); err1 != nil {
+		zap.S().Errorf("unable to close thresholdLowerBound resource: %v", err1)
+		err = err1
+	}
+	if err2 := rd.thresholdUpperBound.Close(); err2 != nil {
+		zap.S().Errorf("unable to close thresholdUpperBound resource: %v", err2)
+		err = err2
+	}
+	return err
+}
+
+func NewRoadDetector() *RoadDetector {
+
+	return &RoadDetector{
+		kernelSize:              4,
+		morphoIterations:        3,
+		approxPolyEpsilonFactor: 0.01,
+		thresholdLowerBound:     gocv.NewMatFromScalar(gocv.NewScalar(120.0, 120.0, 120.0, 120.0), gocv.MatTypeCV8U),
+		thresholdUpperBound:     gocv.NewMatFromScalar(gocv.NewScalar(250.0, 250.0, 250.0, 250.0), gocv.MatTypeCV8U),
+	}
+}
+
+func (rd *RoadDetector) DetectRoadContour(imgGray *gocv.Mat, horizonRow int) *gocv.PointVector {
+
+	kernel := gocv.NewMatWithSizeFromScalar(gocv.NewScalar(1, 1, 1, 1), rd.kernelSize, rd.kernelSize, gocv.MatTypeCV8U)
+
+	img := imgGray.Clone()
+	defer func() {
+		if err := img.Close(); err != nil {
+			zap.S().Warnf("unable to close mat resource: %v", err)
+		}
+	}()
+
+	for i := rd.morphoIterations; i > 0; i-- {
+		gocv.Dilate(img, &img, kernel)
+	}
+	for i := rd.morphoIterations; i > 0; i-- {
+		gocv.Erode(img, &img, kernel)
+	}
+	gocv.Dilate(img, &img, kernel)
+
+	gocv.Threshold(img, &img, 180, 255, gocv.ThresholdBinaryInv)
+
+	// Draw black rectangle above horizon
+	horizon := gocv.NewMatWithSize(1, 4, gocv.MatTypeCV32S)
+	horizon.SetIntAt(0, 0, 0)                     // X1
+	horizon.SetIntAt(0, 1, int32(horizonRow))     // Y1
+	horizon.SetIntAt(0, 2, int32(imgGray.Cols())) // X2
+	horizon.SetIntAt(0, 3, int32(horizonRow))     // Y2
+	rectangle := image.Rect(0, 0, int(horizon.GetIntAt(0, 2)), int(horizon.GetIntAt(0, 3)))
+	gocv.Rectangle(&img, rectangle, color.RGBA{0, 0, 0, 0}, FILLED)
+
+	return rd.detectRoadContour(&img)
+}
+
+func (rd *RoadDetector) detectRoadContour(imgInversed *gocv.Mat) *gocv.PointVector {
+
+	var (
+		epsilon float64
+		cntr    gocv.PointVector
+	)
+
+	ptsVec := gocv.FindContours(*imgInversed, gocv.RetrievalExternal, gocv.ChainApproxSimple)
+	defer ptsVec.Close()
+
+	if ptsVec.Size() == 0 {
+		emptyContours := gocv.NewPointVector()
+		return &emptyContours
+	} else if ptsVec.Size() == 1 {
+		epsilon = rd.approxPolyEpsilonFactor * gocv.ArcLength(ptsVec.At(0), true)
+		cntr = ptsVec.At(0)
+	} else {
+		// Search biggest contour
+		peris := make([]float64, ptsVec.Size())
+		maxArcIdx := 0
+		maxArcValue := 0.
+		//for i, c := range cntrs {
+		for i := 0; i< ptsVec.Size(); i++ {
+			c := ptsVec.At(i)
+			peri := gocv.ArcLength(c, true)
+			peris[i] = peri
+			if peri > maxArcValue {
+				maxArcValue = peri
+				maxArcIdx = i
+			}
+			cntr = ptsVec.At(maxArcIdx)
+		}
+		epsilon = rd.approxPolyEpsilonFactor * peris[maxArcIdx]
+	}
+	approx := gocv.ApproxPolyDP(cntr, epsilon, true)
+	return &approx
+}
+
+var EllipseNotFound = events.Ellipse{Confidence: 0.}
+
+func (rd *RoadDetector) ComputeEllipsis(road *gocv.PointVector) *events.Ellipse {
+	if road.Size() < 5 {
+		return &EllipseNotFound
+	}
+
+	rotatedRect := gocv.FitEllipse(*road)
+
+	trust := rd.computeTrustFromCenter(&rotatedRect.Center)
+	zap.S().Debugf("Trust: %v", trust)
+
+	return &events.Ellipse{
+		Center: &events.Point{
+			X: int32(rotatedRect.Center.X),
+			Y: int32(rotatedRect.Center.Y),
+		},
+		Width:      int32(rotatedRect.Width),
+		Height:     int32(rotatedRect.Height),
+		Angle:      float32(rotatedRect.Angle),
+		Confidence: rd.computeTrustFromCenter(&rotatedRect.Center),
+	}
+}
+
+func (rd *RoadDetector) computeTrustFromCenter(ellipsisCenter *image.Point) float32 {
+	safeMinX := 48
+	safeMaxX := 115
+	safeMinY := 69
+	safeMaxY := 119
+
+	if safeMinX <= ellipsisCenter.X && ellipsisCenter.X <= safeMaxX && safeMinY <= ellipsisCenter.Y && ellipsisCenter.Y <= safeMaxY {
+		return 1.0
+	}
+
+	if safeMinX <= ellipsisCenter.X && ellipsisCenter.X <= safeMaxX {
+		return rd.computeTrustOnAxis(safeMaxY, safeMinY, ellipsisCenter.Y)
+	}
+
+	if safeMinY <= ellipsisCenter.Y && ellipsisCenter.Y <= safeMaxY {
+		return rd.computeTrustOnAxis(safeMaxX, safeMinX, ellipsisCenter.X)
+	}
+
+	return rd.computeTrustOnAxis(safeMaxY, safeMinY, ellipsisCenter.Y) * rd.computeTrustOnAxis(safeMaxX, safeMinX, ellipsisCenter.X)
+}
+
+func (rd *RoadDetector) computeTrustOnAxis(safeMax, safeMin, value int) float32 {
+	trust := 1.
+	if value > safeMax {
+		trust = 1. / float64(value-safeMax)
+	} else if value < safeMin {
+		trust = 1. / float64(safeMin-value)
+	}
+	trust = trust * 10.
+	if trust > 0.9 {
+		trust = 0.9
+	}
+	if trust < 0. {
+		trust = 0.
+	}
+	return float32(trust)
+
+}

pkg/part/opencv_test.go

@@ -0,0 +1,204 @@
+package part
+
+import (
+	"fmt"
+	"github.com/cyrilix/robocar-protobuf/go/events"
+	"go.uber.org/zap"
+	"gocv.io/x/gocv"
+	"image"
+	"image/color"
+	"testing"
+)
+
+func toGray(imgColor gocv.Mat) *gocv.Mat {
+	imgGray := gocv.NewMatWithSize(imgColor.Rows(), imgColor.Cols(), gocv.MatTypeCV8UC1)
+	gocv.CvtColor(imgColor, &imgGray, gocv.ColorRGBToGray)
+	return &imgGray
+}
+
+func image1() *gocv.Mat {
+	img := gocv.IMRead("testdata/image.jpg", gocv.IMReadColor)
+	return &img
+}
+
+func image2() *gocv.Mat {
+	img := gocv.IMRead("testdata/image2.jpg", gocv.IMReadColor)
+	return &img
+}
+
+func image3() *gocv.Mat {
+	img := gocv.IMRead("testdata/image3.jpg", gocv.IMReadColor)
+	return &img
+}
+
+func image4() *gocv.Mat {
+	img := gocv.IMRead("testdata/image4.jpg", gocv.IMReadColor)
+	return &img
+}
+
+func image5() *gocv.Mat {
+	img := gocv.IMRead("testdata/image5.jpg", gocv.IMReadColor)
+	return &img
+}
+
+func TestRoadDetection_DetectRoadContour(t *testing.T) {
+	rd := NewRoadDetector()
+
+	img1 := image1()
+	defer img1.Close()
+	img2 := image2()
+	defer img2.Close()
+	img3 := image3()
+	defer img3.Close()
+	img4 := image4()
+	defer img4.Close()
+	img5 := image5()
+	defer img5.Close()
+
+	cases := []struct {
+		name            string
+		img             *gocv.Mat
+		horizon         int
+		expectedContour []image.Point
+	}{
+		{"image1", img1, 20,
+			[]image.Point{image.Point{0, 45}, image.Point{0, 127}, image.Point{144, 127}, image.Point{95, 21}, image.Point{43, 21}},
+		},
+		{"image2", img2, 20,
+			[]image.Point{{159, 69}, {128, 53}, {125, 41}, {113, 42}, {108, 21}, {87, 21}, {79, 41}, {72, 30}, {44, 39}, {29, 34}, {0, 67}, {0, 127}, {159, 127}, {152, 101}},
+		},
+		{"image3", img3, 20,
+			[]image.Point{{97, 21}, {59, 127}, {159, 127}, {159, 36}, {138, 21}},
+		},
+		{"image4", img4, 20,
+			[]image.Point{{0, 21}, {0, 77}, {68, 22}, {0, 96}, {0, 127}, {159, 127}, {159, 21}},
+		},
+		{"image5", img5, 20,
+			[]image.Point{{159, 32}, {100, 36}, {29, 60}, {0, 79}, {0, 127}, {159, 127}},
+		},
+	}
+
+	for _, c := range cases {
+		imgGray := toGray(*c.img)
+		contours := rd.DetectRoadContour(imgGray, c.horizon)
+
+		zap.S().Infof("[%v] contour: %v", c.name, *contours)
+		expected := gocv.NewPointVectorFromPoints(c.expectedContour)
+
+		if contours.Size() != expected.Size() {
+			t.Errorf("[%v] bad contour size: %v point(s), wants %v", c.name, contours.Size(), expected.Size())
+		}
+		for idx := 0; idx< expected.Size(); idx++ {
+			pt := expected.At(idx)
+		if pt != contours.At(idx) {
+				t.Errorf("[%v] bad point: %v, wants %v", c.name, contours.At(idx), pt)
+			}
+		}
+		debugContour(*c.img, contours, fmt.Sprintf("/tmp/%v.jpg", c.name))
+
+		expected.Close()
+		imgGray.Close()
+		contours.Close()
+	}
+}
+
+func debugContour(img gocv.Mat, contour *gocv.PointVector, imgPath string) {
+	imgColor := img.Clone()
+	defer imgColor.Close()
+
+	ptsVec := gocv.NewPointsVector()
+	defer ptsVec.Close()
+	ptsVec.Append(*contour)
+	gocv.DrawContours(&imgColor, ptsVec, 0, color.RGBA{
+		R: 0,
+		G: 255,
+		B: 0,
+		A: 255,
+	}, 1)
+	gocv.IMWrite(imgPath, imgColor)
+}
+
+func TestRoadDetector_ComputeEllipsis(t *testing.T) {
+	rd := NewRoadDetector()
+
+	cases := []struct {
+		name            string
+		contour         []image.Point
+		expectedEllipse events.Ellipse
+	}{
+		{"image1",
+			[]image.Point{image.Point{0, 45}, image.Point{0, 127}, image.Point{144, 127}, image.Point{95, 21}, image.Point{43, 21}},
+			events.Ellipse{
+				Center: &events.Point{
+					X: 71,
+					Y: 87,
+				},
+				Width:      139,
+				Height:     176,
+				Angle:      92.66927,
+				Confidence: 1.,
+			},
+		},
+		{"image2",
+			[]image.Point{{159, 69}, {128, 53}, {125, 41}, {113, 42}, {108, 21}, {87, 21}, {79, 41}, {72, 30}, {44, 39}, {29, 34}, {0, 67}, {0, 127}, {159, 127}, {152, 101}},
+			events.Ellipse{
+				Center: &events.Point{
+					X: 77,
+					Y: 102,
+				},
+				Width:      152,
+				Height:     168,
+				Angle:      94.70433,
+				Confidence: 1.,
+			},
+		},
+		{"image3",
+			[]image.Point{{97, 21}, {59, 127}, {159, 127}, {159, 36}, {138, 21}},
+			events.Ellipse{
+				Center: &events.Point{
+					X: 112,
+					Y: 86,
+				},
+				Width:      122,
+				Height:     140,
+				Angle:      20.761106,
+				Confidence: 1.,
+			},
+		},
+		{"image4",
+			[]image.Point{{0, 21}, {0, 77}, {68, 22}, {0, 96}, {0, 127}, {159, 127}, {159, 21}},
+			events.Ellipse{
+				Center: &events.Point{
+					X: 86,
+					Y: 78,
+				},
+				Width:      154,
+				Height:     199,
+				Angle:      90.45744,
+				Confidence: 1.,
+			},
+		},
+		{"image5",
+			[]image.Point{{159, 32}, {100, 36}, {29, 60}, {0, 79}, {0, 127}, {159, 127}},
+			events.Ellipse{
+				Center: &events.Point{
+					X: 109,
+					Y: 87,
+				},
+				Width:      103,
+				Height:     247,
+				Angle:      79.6229,
+				Confidence: 1.0,
+			},
+		},
+	}
+
+	for _, c := range cases {
+		ct := gocv.NewPointVectorFromPoints(c.contour)
+		ellipse := rd.ComputeEllipsis(&ct)
+		ct.Close()
+		if ellipse.String() != c.expectedEllipse.String() {
+			t.Errorf("ComputeEllipsis(%v): %v, wants %v", c.name, ellipse.String(), c.expectedEllipse.String())
+		}
+	}
+}

pkg/part/part.go

@@ -0,0 +1,144 @@
+package part
+
+import (
+	"github.com/cyrilix/robocar-base/service"
+	"github.com/cyrilix/robocar-protobuf/go/events"
+	mqtt "github.com/eclipse/paho.mqtt.golang"
+	"github.com/golang/protobuf/proto"
+	"go.uber.org/zap"
+	"gocv.io/x/gocv"
+	"log"
+)
+
+type RoadPart struct {
+	client                 mqtt.Client
+	frameChan              chan frameToProcess
+	readyForNext           chan interface{}
+	cancel                 chan interface{}
+	roadDetector           *RoadDetector
+	horizon                int
+	cameraTopic, roadTopic string
+}
+
+func NewRoadPart(client mqtt.Client, horizon int, cameraTopic, roadTopic string) *RoadPart {
+	return &RoadPart{
+		client:       client,
+		frameChan:    make(chan frameToProcess),
+		cancel:       make(chan interface{}),
+		roadDetector: NewRoadDetector(),
+		horizon:      horizon,
+		cameraTopic:  cameraTopic,
+		roadTopic:    roadTopic,
+	}
+}
+
+func (r *RoadPart) Start() error {
+	log := zap.S()
+	registerCallBacks(r)
+
+	var frame = frameToProcess{}
+	defer func() {
+		if err := frame.Close(); err != nil {
+			log.Errorf("unable to close msg: %v", err)
+		}
+	}()
+
+	for {
+		select {
+		case f := <-r.frameChan:
+			log.Debug("new msg")
+			oldFrame := frame
+			frame = f
+			if err := oldFrame.Close(); err != nil {
+				log.Errorf("unable to close msg: %v", err)
+			}
+			log.Debug("process msg")
+			go r.processFrame(&frame)
+		case <-r.cancel:
+			log.Infof("Stop service")
+			return nil
+		}
+	}
+}
+
+var registerCallBacks = func(r *RoadPart) {
+	err := service.RegisterCallback(r.client, r.cameraTopic, r.OnFrame)
+	if err != nil {
+		log.Panicf("unable to register callback to topic %v:%v", r.cameraTopic, err)
+	}
+}
+
+func (r *RoadPart) Stop() {
+	defer func() {
+		if err := r.roadDetector.Close(); err != nil {
+			zap.S().Errorf("unable to close roadDetector: %v", err)
+		}
+	}()
+	close(r.readyForNext)
+	close(r.cancel)
+	service.StopService("road", r.client, r.roadTopic)
+}
+
+func (r *RoadPart) OnFrame(_ mqtt.Client, msg mqtt.Message) {
+	var frameMsg events.FrameMessage
+	err := proto.Unmarshal(msg.Payload(), &frameMsg)
+	if err != nil {
+		zap.S().Errorf("unable to unmarshal %T message: %v", frameMsg, err)
+		return
+	}
+
+	img, err := gocv.IMDecode(frameMsg.GetFrame(), gocv.IMReadUnchanged)
+	if err != nil {
+		zap.S().Errorf("unable to decode image: %v", err)
+		return
+	}
+	frame := frameToProcess{
+		ref: frameMsg.GetId(),
+		Mat: img,
+	}
+	r.frameChan <- frame
+}
+
+type frameToProcess struct {
+	ref *events.FrameRef
+	gocv.Mat
+}
+
+func (r *RoadPart) processFrame(frame *frameToProcess) {
+	img := frame.Mat
+	imgGray := gocv.NewMatWithSize(img.Rows(), img.Cols(), gocv.MatTypeCV8UC1)
+	defer func() {
+		if err := imgGray.Close(); err != nil {
+			zap.S().Warnf("unable to close Mat resource: %v", err)
+		}
+	}()
+	gocv.CvtColor(img, &imgGray, gocv.ColorRGBToGray)
+
+	road := r.roadDetector.DetectRoadContour(&imgGray, r.horizon)
+	defer road.Close()
+
+	ellipse := r.roadDetector.ComputeEllipsis(road)
+
+	cntr := make([]*events.Point, 0, road.Size())
+	for i:=0;i< road.Size(); i++ {
+		pt := road.At(i)
+		cntr = append(cntr, &events.Point{X: int32(pt.X), Y: int32(pt.Y)})
+	}
+
+	msg := events.RoadMessage{
+		Contour:  cntr,
+		Ellipse:  ellipse,
+		FrameRef: frame.ref,
+	}
+
+	payload, err := proto.Marshal(&msg)
+	if err != nil {
+		zap.S().Errorf("unable to marshal %T to protobuf: %err", msg, err)
+		return
+	}
+	publish(r.client, r.roadTopic, &payload)
+}
+
+var publish = func(client mqtt.Client, topic string, payload *[]byte) {
+	client.Publish(topic, 0, false, *payload)
+}

pkg/part/part_test.go

@@ -0,0 +1,118 @@
+package part
+
+import (
+	"fmt"
+	"github.com/cyrilix/robocar-base/testtools"
+	"github.com/cyrilix/robocar-protobuf/go/events"
+	mqtt "github.com/eclipse/paho.mqtt.golang"
+	"github.com/golang/protobuf/proto"
+	"github.com/golang/protobuf/ptypes/timestamp"
+	"go.uber.org/zap"
+	"io/ioutil"
+	"sync"
+	"testing"
+	"time"
+)
+
+func TestRoadPart_OnFrame(t *testing.T) {
+	oldRegister := registerCallBacks
+	oldPublish := publish
+	defer func() {
+		registerCallBacks = oldRegister
+		publish = oldPublish
+	}()
+
+	registerCallBacks = func(_ *RoadPart) {}
+
+	var muEventsPublished sync.Mutex
+	eventsPublished := make(map[string][]byte)
+	publish = func(client mqtt.Client, topic string, payload *[]byte) {
+		muEventsPublished.Lock()
+		defer muEventsPublished.Unlock()
+		eventsPublished[topic] = *payload
+	}
+
+	cameraTopic := "topic/camera"
+	roadTopic := "topic/road"
+
+	rp := NewRoadPart(nil, 20, cameraTopic, roadTopic)
+	go func() {
+		if err := rp.Start(); err != nil {
+			t.Errorf("unable to start roadPart: %v", err)
+			t.FailNow()
+		}
+	}()
+
+	cases := []struct {
+		name            string
+		msg             mqtt.Message
+		expectedCntr    []*events.Point
+		expectedEllipse events.Ellipse
+	}{
+		{
+			name:            "image1",
+			msg:             loadFrame(t, cameraTopic, "image"),
+			expectedCntr:    []*events.Point{&events.Point{X: 0, Y: int32(45)}, &events.Point{X: 0, Y: 127}, &events.Point{X: 144, Y: 127}, &events.Point{X: 95, Y: 21}, &events.Point{X: 43, Y: 21}},
+			expectedEllipse: events.Ellipse{Center: &events.Point{X: 71, Y: 87}, Width: 139, Height: 176, Angle: 92.66927, Confidence: 1.},
+		},
+	}
+
+	for _, c := range cases {
+		rp.OnFrame(nil, c.msg)
+
+		time.Sleep(20 * time.Millisecond)
+
+		var roadMsg events.RoadMessage
+		err := proto.Unmarshal(eventsPublished[roadTopic], &roadMsg)
+		if err != nil {
+			t.Errorf("unable to unmarshal response, bad return type: %v", err)
+			continue
+		}
+
+		if len(roadMsg.Contour) != len(c.expectedCntr) {
+			t.Errorf("[%v] bad nb point in road contour: %v, wants %v", c.name, len(roadMsg.Contour), len(c.expectedCntr))
+		}
+		for idx, pt := range roadMsg.Contour {
+			if pt.String() != c.expectedCntr[idx].String() {
+				t.Errorf("[%v] bad point at position %v: %v, wants %v", c.name, idx, pt, c.expectedCntr[idx])
+			}
+		}
+		if roadMsg.Ellipse.String() != c.expectedEllipse.String() {
+			t.Errorf("[%v] bad ellipse: %v, wants %v", c.name, roadMsg.Ellipse, c.expectedEllipse)
+		}
+		frameRef := frameRefFromPayload(c.msg.Payload())
+		if frameRef.String() != roadMsg.GetFrameRef().String() {
+			t.Errorf("[%v] invalid frameRef: %v, wants %v", c.name, roadMsg.GetFrameRef(), frameRef)
+		}
+	}
+}
+
+func frameRefFromPayload(payload []byte) *events.FrameRef {
+	var msg events.FrameMessage
+	err := proto.Unmarshal(payload, &msg)
+	if err != nil {
+		zap.S().Errorf("unable to unmarshal %T msg: %v", msg, err)
+	}
+	return msg.GetId()
+}
+
+func loadFrame(t *testing.T, topic string, name string) mqtt.Message {
+	img, err := ioutil.ReadFile(fmt.Sprintf("testdata/%s.jpg", name))
+	if err != nil {
+		t.Fatalf("unable to load data test image: %v", err)
+		return nil
+	}
+	now := time.Now()
+	msg := events.FrameMessage{
+		Id: &events.FrameRef{
+			Name: name,
+			Id:   name,
+			CreatedAt: &timestamp.Timestamp{
+				Seconds: now.Unix(),
+				Nanos:   int32(now.Nanosecond()),
+			},
+		},
+		Frame: img,
+	}
+	return testtools.NewFakeMessageFromProtobuf(topic, &msg)
+}