diff --git a/gocv/gocv_calib3d.go b/gocv/gocv_calib3d.go index 2ced4e3..b16b406 100644 --- a/gocv/gocv_calib3d.go +++ b/gocv/gocv_calib3d.go @@ -22,19 +22,56 @@ func GcvInitCameraMatrix2D(objPts, imgPts *mat64.Dense) (camMat *mat64.Dense) { imgPtsVec := NewGcvPoint2f32Vector(int64(nObjPts)) for i := 0; i < nObjPts; i++ { - objPtsVec.Set(i, NewGcvPoint3f32( - objPts.At(i, 0), objPts.At(i, 1), objPts.At(i, 2))) + objPtsVec.Set(i, NewGcvPoint3f32(objPts.Row(nil, i)...)) } for i := 0; i < nObjPts; i++ { - imgPtsVec.Set(i, NewGcvPoint2f32( - imgPts.At(i, 0), imgPts.At(i, 1))) + imgPtsVec.Set(i, NewGcvPoint2f32(imgPts.Row(nil, i)...)) } camMat = GcvMatToMat64(GcvInitCameraMatrix2D_(objPtsVec, imgPtsVec)) return camMat } +func GcvCalibrateCamera(objPts, imgPts, camMat *mat64.Dense) (calCamMat, rvec, tvec *mat64.Dense) { + nObjPts, objCol := objPts.Dims() + nImgPts, imgCol := imgPts.Dims() + + if objCol != 3 || imgCol != 2 || nObjPts != nImgPts { + panic("Invalid dimensions for objPts and imgPts") + } + + objPtsVec := NewGcvPoint3f32Vector(int64(nObjPts)) + imgPtsVec := NewGcvPoint2f32Vector(int64(nObjPts)) + + for i := 0; i < nObjPts; i++ { + objPtsVec.Set(i, NewGcvPoint3f32(objPts.Row(nil, i)...)) + } + + for i := 0; i < nObjPts; i++ { + imgPtsVec.Set(i, NewGcvPoint2f32(imgPts.Row(nil, i)...)) + } + + _camMat := Mat64ToGcvMat(camMat) + _rvec := NewGcvMat() + _tvec := NewGcvMat() + _imgSize := NewGcvSize2i(1920, 1080) + + GcvCalibrateCamera_( + objPtsVec, imgPtsVec, + _imgSize, _camMat, _rvec, _tvec) + + calCamMat = GcvMatToMat64(_camMat) + rvec = GcvMatToMat64(_rvec) + tvec = GcvMatToMat64(_tvec) + + return camMat, rvec, tvec +} + +// func mat64ToGcvPoint3f32Vector(mat *mat64.Dense) NewGcvPoint3f32Vector { + +// } + // func TestGcvCalibrateCamera(t *testing.T) { // objPts := NewGcvPoint3fVector(int64(4)) // objPts.Set(0, NewGcvPoint3f(0, 25, 0)) diff --git a/gocv/gocv_calib3d_test.go b/gocv/gocv_calib3d_test.go index 72115ef..ed8d65a 100644 --- a/gocv/gocv_calib3d_test.go +++ b/gocv/gocv_calib3d_test.go @@ -24,32 +24,23 @@ func TestGcvInitCameraMatrix2D(t *testing.T) { spew.Dump(camMat) } -// func TestGcvCalibrateCamera(t *testing.T) { -// objPts := NewGcvPoint3fVector(int64(4)) -// objPts.Set(0, NewGcvPoint3f(0, 25, 0)) -// objPts.Set(1, NewGcvPoint3f(0, -25, 0)) -// objPts.Set(2, NewGcvPoint3f(-47, 25, 0)) -// objPts.Set(3, NewGcvPoint3f(-47, -25, 0)) +func TestGcvCalibrateCamera(t *testing.T) { + objPts := mat64.NewDense(4, 3, []float64{ + 0, 25, 0, + 0, -25, 0, + -47, 25, 0, + -47, -25, 0}) -// imgPts := NewGcvPoint2fVector(int64(4)) -// imgPts.Set(0, NewGcvPoint2f(1136.4140625, 1041.89208984)) -// imgPts.Set(1, NewGcvPoint2f(1845.33190918, 671.39581299)) -// imgPts.Set(2, NewGcvPoint2f(302.73373413, 634.79998779)) -// imgPts.Set(3, NewGcvPoint2f(1051.46154785, 352.76107788)) + imgPts := mat64.NewDense(4, 2, []float64{ + 1136.4140625, 1041.89208984, + 1845.33190918, 671.39581299, + 302.73373413, 634.79998779, + 1051.46154785, 352.76107788}) -// imgSize := NewGcvSize2i(1920, 1080) + camMat := GcvInitCameraMatrix2D(objPts, imgPts) -// camMat := GcvInitCameraMatrix2D(objPts, imgPts) -// spew.Dump(camMat.GcvAtd(NewGcvSize2i(0, 0))) -// spew.Dump(camMat.GcvAtd(NewGcvSize2i(0, 1))) -// spew.Dump(camMat.GcvAtd(NewGcvSize2i(1, 1))) -// spew.Dump(camMat.GcvAtd(NewGcvSize2i(1, 2))) -// spew.Dump(camMat.GcvAtd(NewGcvSize2i(2, 2))) - -// rvec := NewMat() -// tvec := NewMat() - -// GcvCalibrateCamera(objPts, imgPts, imgSize, camMat, rvec, tvec) - -// MatToMat64(camMat) -// } + camMat, rvec, tvec := GcvCalibrateCamera(objPts, imgPts, camMat) + spew.Dump(camMat) + spew.Dump(rvec) + spew.Dump(tvec) +} diff --git a/gocv/gocv_core.go b/gocv/gocv_core.go index 279c7f2..49b2072 100644 --- a/gocv/gocv_core.go +++ b/gocv/gocv_core.go @@ -5,28 +5,35 @@ package gocv import "C" import "github.com/gonum/matrix/mat64" -func NewGcvPoint3f32(x, y, z float64) GcvPoint3f32_ { - return NewGcvPoint3f32_(float32(x), float32(y), float32(z)) +func NewGcvPoint3f32(pts ...float64) GcvPoint3f32_ { + // This make sure we have default values + safePts := getSafePts(pts, 3) + return NewGcvPoint3f32_(float32(safePts[0]), float32(safePts[1]), float32(safePts[2])) } -func NewGcvPoint3f64(x, y, z float64) GcvPoint3f64_ { - return NewGcvPoint3f64_(float64(x), float64(y), float64(z)) +func NewGcvPoint3f64(pts ...float64) GcvPoint3f64_ { + safePts := getSafePts(pts, 3) + return NewGcvPoint3f64_(safePts[0], safePts[1], safePts[2]) } -func NewGcvPoint2f32(x, y float64) GcvPoint2f32_ { - return NewGcvPoint2f32_(float32(x), float32(y)) +func NewGcvPoint2f32(pts ...float64) GcvPoint2f32_ { + safePts := getSafePts(pts, 2) + return NewGcvPoint2f32_(float32(safePts[0]), float32(safePts[1])) } -func NewGcvPoint2f64(x, y float64) GcvPoint2f64_ { - return NewGcvPoint2f64_(float64(x), float64(y)) +func NewGcvPoint2f64(pts ...float64) GcvPoint2f64_ { + safePts := getSafePts(pts, 2) + return NewGcvPoint2f64_(safePts[0], safePts[1]) } -func NewGcvSize2f32(x, y float64) GcvSize2f32_ { - return NewGcvSize2f32_(float32(x), float32(y)) +func NewGcvSize2f32(pts ...float64) GcvSize2f32_ { + safePts := getSafePts(pts, 2) + return NewGcvSize2f32_(float32(safePts[0]), float32(safePts[1])) } -func NewGcvSize2f64(x, y float64) GcvSize2f64_ { - return NewGcvSize2f64_(float64(x), float64(y)) +func NewGcvSize2f64(pts ...float64) GcvSize2f64_ { + safePts := getSafePts(pts, 2) + return NewGcvSize2f64_(safePts[0], safePts[1]) } // Convert Mat, which defined by SWIG, to *mat64.Dense. @@ -67,3 +74,11 @@ func Mat64ToGcvMat(mat *mat64.Dense) GcvMat { return Mat64ToGcvMat_(row, col, rawData) } + +func getSafePts(pts []float64, size int) []float64 { + // This make sure we have default values + safePts := make([]float64, size, size) + copy(safePts, pts) + + return safePts +} diff --git a/gocv/gocv_core_test.go b/gocv/gocv_core_test.go index c327e3b..2b0be85 100644 --- a/gocv/gocv_core_test.go +++ b/gocv/gocv_core_test.go @@ -8,7 +8,7 @@ import ( ) func TestNewGcvPoint3f32(t *testing.T) { - pt := NewGcvPoint3f32(3, 1, 2) + pt := NewGcvPoint3f32(3, 1) spew.Dump(pt) }