refactor: new road detection implementation

cmd/rc-road/rc-road.go

@@ -3,21 +3,27 @@ package main
 import (
 	"flag"
 	"github.com/cyrilix/robocar-base/cli"
-	"github.com/cyrilix/robocar-road/pkg/part"
+	"github.com/cyrilix/robocar-road/road"
 	"go.uber.org/zap"
+	"gocv.io/x/gocv"
+	"image"
 	"log"
+	"math"
 	"os"
 )
 
 const (
 	DefaultClientId = "robocar-road"
-	DefaultHorizon  = 20
+	DefaultHorizon  = 110
 )
 
 func main() {
 	var mqttBroker, username, password, clientId string
 	var cameraTopic, roadTopic string
 	var horizon int
+	var whiteThresholdLow, whiteThresholdHigh int
+	var cannyThresholdLow, cannyThresholdHigh int
+	var imgWidth, imgHeight int
 
 	err := cli.SetIntDefaultValueFromEnv(&horizon, "HORIZON", DefaultHorizon)
 	if err != nil {
@@ -33,6 +39,23 @@ func main() {
 	flag.StringVar(&cameraTopic, "mqtt-topic-camera", os.Getenv("MQTT_TOPIC_CAMERA"), "Mqtt topic that contains camera frame values, use MQTT_TOPIC_CAMERA if args not set")
 	flag.IntVar(&horizon, "horizon", horizon, "Limit horizon in pixels from top, use HORIZON if args not set")
 
+	flag.IntVar(&imgWidth, "image-width", 160, "Video pixels width")
+	flag.IntVar(&imgHeight, "image-height", 128, "Video pixels height")
+
+	flag.IntVar(&whiteThresholdLow, "white-threshold-low", 20, "White pixels threshold, low limit")
+	flag.IntVar(&whiteThresholdHigh, "white-threshold-high", 255, "White pixels threshold, high limit")
+
+	flag.IntVar(&cannyThresholdLow, "canny-threshold-low", 100, "White pixels threshold, low limit")
+	flag.IntVar(&cannyThresholdHigh, "canny-threshold-high", 250, "White pixels threshold, high limit")
+
+	var houghLinesRho, houghLinesThreshold, houghLinesMinLineLength, houghLinesMaxLineGap int
+	var houghLinesTheta float64
+	flag.IntVar(&houghLinesRho, "hough-lines-rho", 2, "distance resolution in pixels of the Hough grid")
+	flag.Float64Var(&houghLinesTheta, "hough-lines-theta", 1*math.Pi/180, "angular resolution in radians of the Hough grid, default Pi/180")
+	flag.IntVar(&houghLinesThreshold, "hough-lines-threshold", 15, "minimum number of votes (intersections in Hough grid cell)")
+	flag.IntVar(&houghLinesMinLineLength, "hough-lines-min-line-length", 10, "minimum number of pixels making up a line")
+	flag.IntVar(&houghLinesMaxLineGap, "hough-lines-max-lines-gap", 20, "maximum gap in pixels between connectable line segments")
+
 	logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level")
 	flag.Parse()
 
@@ -60,7 +83,21 @@ func main() {
 	}
 	defer client.Disconnect(50)
 
-	p := part.NewRoadPart(client, horizon, cameraTopic, roadTopic)
+	p := road.NewPart(client,
+		cameraTopic, roadTopic,
+		road.NewDetector(
+			road.WithWhiteFilter(whiteThresholdLow, whiteThresholdHigh),
+			road.WithYellowFilter(
+				gocv.NewMatFromScalar(gocv.Scalar{Val1: 90., Val2: 100., Val3: 100.}, gocv.MatTypeCV8U),
+				gocv.NewMatFromScalar(gocv.Scalar{Val1: 110., Val2: 255., Val3: 255.}, gocv.MatTypeCV8U),
+			),
+			road.WithCanny(cannyThresholdLow, cannyThresholdHigh),
+			road.WithGaussianBlur(3),
+			road.WithRegionOfInterest(imgWidth, imgHeight, horizon),
+			road.WithPointOnRoad(image.Point{X: imgWidth / 2, Y: imgHeight - 30}),
+			road.WithHoughLines(houghLinesRho, float32(houghLinesTheta), houghLinesThreshold, houghLinesMinLineLength, houghLinesMaxLineGap),
+		),
+	)
 	defer p.Stop()
 
 	cli.HandleExit(p)

cmd/road-debug/road-debug.go

@@ -0,0 +1,65 @@
+package main
+
+import (
+	"flag"
+	"github.com/cyrilix/robocar-road/road"
+	"go.uber.org/zap"
+	"gocv.io/x/gocv"
+	"image"
+	"image/color"
+	"log"
+	"os"
+)
+
+func main() {
+	var imgName string
+	flag.StringVar(&imgName, "image", "", "path to image file")
+
+	logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level")
+	flag.Parse()
+
+	if imgName == "" {
+		zap.S().Errorf("bad image value")
+		flag.PrintDefaults()
+		os.Exit(1)
+	}
+
+	if len(os.Args) <= 1 {
+		flag.PrintDefaults()
+		os.Exit(1)
+	}
+
+	config := zap.NewDevelopmentConfig()
+	config.Level = zap.NewAtomicLevelAt(*logLevel)
+	lgr, err := config.Build()
+	if err != nil {
+		log.Fatalf("unable to init logger: %v", err)
+	}
+	defer func() {
+		if err := lgr.Sync(); err != nil {
+			log.Printf("unable to Sync logger: %v\n", err)
+		}
+	}()
+	zap.ReplaceGlobals(lgr)
+
+	d := road.NewDetector(160, 120)
+
+	img := gocv.IMRead(imgName, gocv.IMReadColor)
+	defer func(img *gocv.Mat) {
+		err := img.Close()
+		if err != nil {
+			zap.S().Warnf("unable to close image: %v", err)
+		}
+	}(&img)
+	if img.Empty() {
+		zap.S().Errorf("image %s is not a valid image", imgName)
+		os.Exit(1)
+	}
+	roadLimits := d.Detect(&img)
+
+	gocv.FillPoly(&img, gocv.NewPointsVectorFromPoints([][]image.Point{roadLimits}), color.RGBA{0, 0, 255, 128})
+	window := gocv.NewWindow("Road")
+	window.IMShow(img)
+	window.WaitKey(0)
+
+}