From c7772cec7c827794d0da8a5014b5611f038ffa23 Mon Sep 17 00:00:00 2001 From: deadprogram Date: Wed, 18 Oct 2023 20:14:36 +0200 Subject: [PATCH] calib3d: add FisheyeCalibrate, FisheyeDistortPoints, and CheckChessboard functions Signed-off-by: deadprogram --- ROADMAP.md | 13 +++-- calib3d.cpp | 13 +++++ calib3d.go | 37 ++++++++++++ calib3d.h | 3 + calib3d_test.go | 150 ++++++++++++++++++++++++++++++++++++++++++++++-- 5 files changed, 204 insertions(+), 12 deletions(-) diff --git a/ROADMAP.md b/ROADMAP.md index ab296065..f686506c 100644 --- a/ROADMAP.md +++ b/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) diff --git a/calib3d.cpp b/calib3d.cpp index 2ba6a9cc..1f9a8c9a 100644 --- a/calib3d.cpp +++ b/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); diff --git a/calib3d.go b/calib3d.go index d2b7687f..69c6a680 100644 --- a/calib3d.go +++ b/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 diff --git a/calib3d.h b/calib3d.h index 1ad40669..01e57c45 100644 --- a/calib3d.h +++ b/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); diff --git a/calib3d_test.go b/calib3d_test.go index 62a05845..0579fda3 100644 --- a/calib3d_test.go +++ b/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() {