calib3d: add FisheyeCalibrate, FisheyeDistortPoints, and CheckChessbo…

…ard functions

Signed-off-by: deadprogram <ron@hybridgroup.com>

ROADMAP.md

@@ -126,11 +126,12 @@ Your pull requests will be greatly appreciated!
 
 - [ ] **calib3d. Camera Calibration and 3D Reconstruction - WORK STARTED**. The following functions still need implementation:
     - [ ] **Camera Calibration - WORK STARTED** The following functions still need implementation:
-        - [X] [calibrateCamera](https://docs.opencv.org/master/d9/d0c/group__calib3d.html)
-        - [ ] [calibrateCameraRO](https://docs.opencv.org/master/d9/d0c/group__calib3d.html)
-        - [ ] [calibrateHandEye](https://docs.opencv.org/master/d9/d0c/group__calib3d.html)
+        - [X] [calibrateCamera](https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga3207604e4b1a1758aa66acb6ed5aa65d)
+        - [ ] [calibrateCameraRO](https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#gacb6b35670216b24b67c70fcd21519ead)
+        - [ ] [calibrateHandEye](https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#gaebfc1c9f7434196a374c382abf43439b)
+        - [ ] [calibrateRobotWorldHandEye](https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga41b1a8dd70eae371eba707d101729c36)
         - [ ] [calibrationMatrixValues](https://docs.opencv.org/master/d9/d0c/group__calib3d.html)
-        - [ ] [checkChessboard](https://docs.opencv.org/master/d9/d0c/group__calib3d.html)
+        - [X] [checkChessboard](https://docs.opencv.org/master/d9/d0c/group__calib3d.html)
         - [ ] [composeRT](https://docs.opencv.org/master/d9/d0c/group__calib3d.html)
         - [ ] [computeCorrespondEpilines](https://docs.opencv.org/master/d9/d0c/group__calib3d.html)
         - [X] [convertPointsFromHomogeneous](https://docs.opencv.org/master/d9/d0c/group__calib3d.html)
@@ -179,8 +180,8 @@ Your pull requests will be greatly appreciated!
         - [ ] [validateDisparity](https://docs.opencv.org/master/d9/d0c/group__calib3d.html)
 
     - [ ] **Fisheye - WORK STARTED** The following functions still need implementation:
-        - [ ] [calibrate](https://docs.opencv.org/master/db/d58/group__calib3d__fisheye.html#gad626a78de2b1dae7489e152a5a5a89e1)
-        - [ ] [distortPoints](https://docs.opencv.org/master/db/d58/group__calib3d__fisheye.html#ga75d8877a98e38d0b29b6892c5f8d7765)
+        - [X] [calibrate](https://docs.opencv.org/master/db/d58/group__calib3d__fisheye.html#gad626a78de2b1dae7489e152a5a5a89e1)
+        - [X] [distortPoints](https://docs.opencv.org/master/db/d58/group__calib3d__fisheye.html#ga75d8877a98e38d0b29b6892c5f8d7765)
         - [ ] [projectPoints](https://docs.opencv.org/master/db/d58/group__calib3d__fisheye.html#gab1ad1dc30c42ee1a50ce570019baf2c4)
         - [ ] [stereoCalibrate](https://docs.opencv.org/master/db/d58/group__calib3d__fisheye.html#gadbb3a6ca6429528ef302c784df47949b)
         - [ ] [stereoRectify](https://docs.opencv.org/master/db/d58/group__calib3d__fisheye.html#gac1af58774006689056b0f2ef1db55ecc)

calib3d.cpp

@@ -1,5 +1,13 @@
 #include "calib3d.h"
 
+double Fisheye_Calibrate(Points3fVector objectPoints, Points2fVector imagePoints, Size size, Mat k, Mat d, Mat rvecs, Mat tvecs, int flags) {
+    cv::Size sz(size.width, size.height);
+    return cv::fisheye::calibrate(*objectPoints, *imagePoints, sz, *k, *d, *rvecs, *tvecs, flags);
+}
+
+void Fisheye_DistortPoints(Mat undistorted, Mat distorted, Mat k, Mat d) {
+    cv::fisheye::distortPoints(*undistorted, *distorted, *k, *d);
+}
 
 void Fisheye_UndistortImage(Mat distorted, Mat undistorted, Mat k, Mat d) {
     cv::fisheye::undistortImage(*distorted, *undistorted, *k, *d);
@@ -49,6 +57,11 @@ void UndistortPoints(Mat distorted, Mat undistorted, Mat k, Mat d, Mat r, Mat p)
     cv::undistortPoints(*distorted, *undistorted, *k, *d, *r, *p);
 }
 
+bool CheckChessboard(Mat image, Size size) {
+    cv::Size sz(size.width, size.height);
+    return cv::checkChessboard(*image, sz);
+}
+
 bool FindChessboardCorners(Mat image, Size patternSize, Mat corners, int flags) {
     cv::Size sz(patternSize.width, patternSize.height);
     return cv::findChessboardCorners(*image, sz, *corners, flags);

calib3d.go

@@ -55,6 +55,27 @@ const (
 	CalibFixPrincipalPoint
 )
 
+// FisheyeCalibrate performs camera calibration.
+//
+// For further details, please see:
+// https://docs.opencv.org/4.x/db/d58/group__calib3d__fisheye.html#gad626a78de2b1dae7489e152a5a5a89e1
+func FisheyeCalibrate(objectPoints Points3fVector, imagePoints Points2fVector, size image.Point, k, d, rvecs, tvecs *Mat, flags CalibFlag) float64 {
+	sz := C.struct_Size{
+		width:  C.int(size.X),
+		height: C.int(size.Y),
+	}
+
+	return float64(C.Fisheye_Calibrate(objectPoints.p, imagePoints.p, sz, k.p, d.p, rvecs.p, tvecs.p, C.int(flags)))
+}
+
+// FisheyeDistortPoints distorts 2D points using fisheye model.
+//
+// For further details, please see:
+// https://docs.opencv.org/master/db/d58/group__calib3d__fisheye.html#gab738cdf90ceee97b2b52b0d0e7511541
+func FisheyeDistortPoints(undistorted Mat, distorted *Mat, k, d Mat) {
+	C.Fisheye_DistortPoints(undistorted.Ptr(), distorted.Ptr(), k.Ptr(), d.Ptr())
+}
+
 // FisheyeUndistortImage transforms an image to compensate for fisheye lens distortion
 func FisheyeUndistortImage(distorted Mat, undistorted *Mat, k, d Mat) {
 	C.Fisheye_UndistortImage(distorted.Ptr(), undistorted.Ptr(), k.Ptr(), d.Ptr())
@@ -137,6 +158,10 @@ func CalibrateCamera(objectPoints Points3fVector, imagePoints Points2fVector, im
 	return float64(res)
 }
 
+// Undistort transforms an image to compensate for lens distortion.
+//
+// For further details, please see:
+// https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga69f2545a8b62a6b0fc2ee060dc30559d
 func Undistort(src Mat, dst *Mat, cameraMatrix Mat, distCoeffs Mat, newCameraMatrix Mat) {
 	C.Undistort(src.Ptr(), dst.Ptr(), cameraMatrix.Ptr(), distCoeffs.Ptr(), newCameraMatrix.Ptr())
 }
@@ -149,6 +174,18 @@ func UndistortPoints(src Mat, dst *Mat, cameraMatrix, distCoeffs, rectificationT
 	C.UndistortPoints(src.Ptr(), dst.Ptr(), cameraMatrix.Ptr(), distCoeffs.Ptr(), rectificationTransform.Ptr(), newCameraMatrix.Ptr())
 }
 
+// CheckChessboard renders the detected chessboard corners.
+//
+// For further details, please see:
+// https://docs.opencv.org/master/d9/d0c/group__calib3d.html#ga6a10b0bb120c4907e5eabbcd22319022
+func CheckChessboard(image Mat, size image.Point) bool {
+	sz := C.struct_Size{
+		width:  C.int(size.X),
+		height: C.int(size.Y),
+	}
+	return bool(C.CheckChessboard(image.Ptr(), sz))
+}
+
 // CalibCBFlag value for chessboard calibration
 // For more details, please see:
 // https://docs.opencv.org/master/d9/d0c/group__calib3d.html#ga93efa9b0aa890de240ca32b11253dd4a

calib3d.h

@@ -12,6 +12,8 @@ extern "C" {
 #include "core.h"
 
 //Calib
+double Fisheye_Calibrate(Points3fVector objectPoints, Points2fVector imagePoints, Size size, Mat k, Mat d, Mat rvecs, Mat tvecs, int flags);
+void Fisheye_DistortPoints(Mat undistorted, Mat distorted, Mat k, Mat d);
 void Fisheye_UndistortImage(Mat distorted, Mat undistorted, Mat k, Mat d);
 void Fisheye_UndistortImageWithParams(Mat distorted, Mat undistorted, Mat k, Mat d, Mat knew, Size size);
 void Fisheye_UndistortPoints(Mat distorted, Mat undistorted, Mat k, Mat d, Mat R, Mat P);
@@ -22,6 +24,7 @@ Mat GetOptimalNewCameraMatrixWithParams(Mat cameraMatrix,Mat distCoeffs,Size siz
 double CalibrateCamera(Points3fVector objectPoints, Points2fVector imagePoints, Size imageSize, Mat cameraMatrix, Mat distCoeffs, Mat rvecs, Mat tvecs, int flag);
 void Undistort(Mat src, Mat dst, Mat cameraMatrix, Mat distCoeffs, Mat newCameraMatrix);
 void UndistortPoints(Mat distorted, Mat undistorted, Mat k, Mat d, Mat r, Mat p);
+bool CheckChessboard(Mat image, Size sz);
 bool FindChessboardCorners(Mat image, Size patternSize, Mat corners, int flags);
 bool FindChessboardCornersSB(Mat image, Size patternSize, Mat corners, int flags);
 bool FindChessboardCornersSBWithMeta(Mat image, Size patternSize, Mat corners, int flags, Mat meta);

calib3d_test.go

@@ -8,6 +8,134 @@ import (
 	"testing"
 )
 
+func TestFisheyeCalibrate(t *testing.T) {
+	img := IMRead("images/chessboard_4x6_distort.png", IMReadGrayScale)
+	if img.Empty() {
+		t.Error("Invalid read of chessboard image")
+		return
+	}
+	defer img.Close()
+
+	corners := NewMat()
+	defer corners.Close()
+
+	size := image.Pt(4, 6)
+	found := FindChessboardCorners(img, size, &corners, 0)
+	if !found {
+		t.Error("chessboard pattern not found")
+		return
+	}
+	if corners.Empty() {
+		t.Error("chessboard pattern not found")
+		return
+	}
+
+	imagePoints := NewPoint2fVectorFromMat(corners)
+	defer imagePoints.Close()
+
+	objectPoints := NewPoint3fVector()
+	defer objectPoints.Close()
+
+	for j := 0; j < size.Y; j++ {
+		for i := 0; i < size.X; i++ {
+			objectPoints.Append(Point3f{
+				X: float32(100 * i),
+				Y: float32(100 * j),
+				Z: 0,
+			})
+		}
+	}
+
+	k := NewMat()
+	defer k.Close()
+	d := NewMat()
+	defer d.Close()
+	rvecs := NewMat()
+	defer rvecs.Close()
+	tvecs := NewMat()
+	defer tvecs.Close()
+
+	objectPointsVector := NewPoints3fVector()
+	objectPointsVector.Append(objectPoints)
+	defer objectPointsVector.Close()
+
+	imagePointsVector := NewPoints2fVector()
+	imagePointsVector.Append(imagePoints)
+	defer imagePointsVector.Close()
+
+	FisheyeCalibrate(
+		objectPointsVector, imagePointsVector, image.Pt(img.Cols(), img.Rows()),
+		&k, &d, &rvecs, &tvecs, 0,
+	)
+
+	if rvecs.Empty() {
+		t.Error("rvecs result is empty")
+		return
+	}
+
+	if tvecs.Empty() {
+		t.Error("tvecs result is empty")
+		return
+	}
+}
+
+func TestFisheyeDistortPoints(t *testing.T) {
+	k := NewMatWithSize(3, 3, MatTypeCV64F)
+	defer k.Close()
+
+	k.SetDoubleAt(0, 0, 1094.7249578198823)
+	k.SetDoubleAt(0, 1, 0)
+	k.SetDoubleAt(0, 2, 959.4907612030962)
+
+	k.SetDoubleAt(1, 0, 0)
+	k.SetDoubleAt(1, 1, 1094.9945708128778)
+	k.SetDoubleAt(1, 2, 536.4566143451868)
+
+	k.SetDoubleAt(2, 0, 0)
+	k.SetDoubleAt(2, 1, 0)
+	k.SetDoubleAt(2, 2, 1)
+
+	d := NewMatWithSize(1, 4, MatTypeCV64F)
+	defer d.Close()
+
+	d.SetDoubleAt(0, 0, -0.05207412392075069)
+	d.SetDoubleAt(0, 1, -0.089168300192224)
+	d.SetDoubleAt(0, 2, 0.10465607695792184)
+	d.SetDoubleAt(0, 3, -0.045693446831115585)
+
+	// transform 3 points in one go (X and Y values of points go in each channel)
+	src := NewMatWithSize(3, 1, MatTypeCV64FC2)
+	defer src.Close()
+
+	dst := NewMat()
+	defer dst.Close()
+
+	// This camera matrix is 1920x1080. Points where x < 960 and y < 540 should move toward the top left (x and y get smaller)
+	// The centre point should be mostly unchanged
+	// Points where x > 960 and y > 540 should move toward the bottom right (x and y get bigger)
+
+	// The index being used for col here is actually the channel (i.e. the point's x/y dimensions)
+	// (since there's only 1 column so the formula: (colNumber * numChannels + channelNumber) reduces to
+	// (0 * 2) + channelNumber
+	// so col = 0 is the x coordinate and col = 1 is the y coordinate
+
+	src.SetDoubleAt(0, 0, 480)
+	src.SetDoubleAt(0, 1, 270)
+
+	src.SetDoubleAt(1, 0, 960)
+	src.SetDoubleAt(1, 1, 540)
+
+	src.SetDoubleAt(2, 0, 1440)
+	src.SetDoubleAt(2, 1, 810)
+
+	FisheyeDistortPoints(src, &dst, k, d)
+
+	if dst.Empty() {
+		t.Error("final image is empty")
+		return
+	}
+}
+
 func TestFisheyeUndistorImage(t *testing.T) {
 	img := IMRead("images/fisheye_sample.jpg", IMReadUnchanged)
 	if img.Empty() {
@@ -48,7 +176,6 @@ func TestFisheyeUndistorImage(t *testing.T) {
 		t.Error("final image is empty")
 		return
 	}
-	// IMWrite("images/fisheye_sample-u.jpg", dest)
 }
 
 func TestFisheyeUndistorImageWithParams(t *testing.T) {
@@ -100,7 +227,6 @@ func TestFisheyeUndistorImageWithParams(t *testing.T) {
 		t.Error("final image is empty")
 		return
 	}
-	// IMWrite("images/fisheye_sample-up.jpg", dest)
 }
 
 func TestInitUndistortRectifyMap(t *testing.T) {
@@ -137,8 +263,7 @@ func TestInitUndistortRectifyMap(t *testing.T) {
 	d.SetDoubleAt(0, 2, -2.62985819e-03)
 	d.SetDoubleAt(0, 3, 2.05841873e-04)
 	d.SetDoubleAt(0, 4, -2.35021914e-02)
-	//FisheyeUndistortImage(img, &dest, k, d)
-	//img.Reshape()
+
 	newC, roi := GetOptimalNewCameraMatrixWithParams(k, d, image.Point{X: img.Cols(), Y: img.Rows()}, (float64)(1), image.Point{X: img.Cols(), Y: img.Rows()}, false)
 	if newC.Empty() {
 		t.Error("final image is empty")
@@ -152,7 +277,7 @@ func TestInitUndistortRectifyMap(t *testing.T) {
 	defer mapx.Close()
 	mapy := NewMat()
 	defer mapy.Close()
-	//dest := NewMat()
+
 	InitUndistortRectifyMap(k, d, r, newC, image.Point{X: img.Cols(), Y: img.Rows()}, 5, mapx, mapy)
 
 	Remap(img, &dest, &mapx, &mapy, InterpolationDefault, BorderConstant, color.RGBA{0, 0, 0, 0})
@@ -210,7 +335,6 @@ func TestUndistort(t *testing.T) {
 		t.Error("final image is empty")
 		return
 	}
-	//IMWrite("images/distortion_up.jpg", dest)
 }
 
 func TestUndistortPoint(t *testing.T) {
@@ -351,8 +475,22 @@ func TestFisheyeUndistortPoint(t *testing.T) {
 	if dst.GetDoubleAt(0, 0) == 0 {
 		t.Error("expected destination Mat to be populated")
 	}
+}
+
+func TestCheckChessboard(t *testing.T) {
+	img := IMRead("images/chessboard_4x6.png", IMReadGrayScale)
+	if img.Empty() {
+		t.Error("Invalid read of chessboard image")
+		return
+	}
+	defer img.Close()
 
+	if !CheckChessboard(img, image.Point{X: 4, Y: 6}) {
+		t.Error("chessboard pattern not found")
+		return
+	}
 }
+
 func TestFindAndDrawChessboard(t *testing.T) {
 	img := IMRead("images/chessboard_4x6.png", IMReadUnchanged)
 	if img.Empty() {