From fb125388820933f377444466731503fa38a9c624 Mon Sep 17 00:00:00 2001 From: Chih-Wei Chang Date: Mon, 16 Feb 2015 16:48:18 +0800 Subject: [PATCH] Wrap initCameraMatrix2D with mat64. --- gocv/gocv.go | 31 +++++++++++++++-- gocv/gocv_calib3d.cpp | 19 +++++------ gocv/gocv_calib3d.go | 66 +++++++++++++++++++++++++++++++++++++ gocv/gocv_calib3d.hpp | 7 ++-- gocv/gocv_core.i | 35 +++++++++++++++++--- gocv/gocv_test.go | 77 +++++++++++++++++++++---------------------- 6 files changed, 173 insertions(+), 62 deletions(-) create mode 100644 gocv/gocv_calib3d.go diff --git a/gocv/gocv.go b/gocv/gocv.go index 4f2e5f9..bfb12f9 100644 --- a/gocv/gocv.go +++ b/gocv/gocv.go @@ -3,8 +3,9 @@ package gocv // #cgo CXXFLAGS: -std=c++11 // #cgo darwin pkg-config: opencv import "C" +import "github.com/gonum/matrix/mat64" -func NewGcvPoint3f(x, y, z float32) GcvPoint3f_ { +func NewGcvPoint3f(x, y, z float64) GcvPoint3f_ { return NewGcvPoint3f_(float32(x), float32(y), float32(z)) } @@ -12,7 +13,7 @@ func NewGcvPoint3d(x, y, z float64) GcvPoint3d_ { return NewGcvPoint3d_(float64(x), float64(y), float64(z)) } -func NewGcvPoint2f(x, y float32) GcvPoint2f_ { +func NewGcvPoint2f(x, y float64) GcvPoint2f_ { return NewGcvPoint2f_(float32(x), float32(y)) } @@ -20,10 +21,34 @@ func NewGcvPoint2d(x, y float64) GcvPoint2d_ { return NewGcvPoint2d_(float64(x), float64(y)) } -func NewGcvSize2f(x, y float32) GcvSize2f_ { +func NewGcvSize2f(x, y float64) GcvSize2f_ { return NewGcvSize2f_(float32(x), float32(y)) } func NewGcvSize2d(x, y float64) GcvSize2d_ { return NewGcvSize2d_(float64(x), float64(y)) } + +// Convert Mat, which defined by SWIG, to mat64.Dense. +// The reason is the latter is much easier to handle +// in Go. +// GcvMat is assumed to be 2-dimensional matrix. +func MatToMat64(mat Mat) *mat64.Dense { + col := mat.GetCols() + row := mat.GetRows() + + data := []float64{} + + for i := 0; i < row; i++ { + for j := 0; j < col; j++ { + if fltPtr, ok := mat.GcvAtd(i, j).(*float64); ok { + data = append(data, *fltPtr) + } else { + panic("Non *float64 passed to MatToMat64") + } + + } + } + + return mat64.NewDense(row, col, data) +} diff --git a/gocv/gocv_calib3d.cpp b/gocv/gocv_calib3d.cpp index 5732e28..b636cef 100644 --- a/gocv/gocv_calib3d.cpp +++ b/gocv/gocv_calib3d.cpp @@ -5,7 +5,7 @@ #include "gocv_calib3d.hpp" -cv::Mat GcvInitCameraMatrix2D(VecPoint3f objPts, VecPoint2f imgPts) { +cv::Mat GcvInitCameraMatrix2D_(VecPoint3f objPts, VecPoint2f imgPts) { cv::Mat cameraMatrix; std::vector objPtsArr; @@ -15,30 +15,27 @@ cv::Mat GcvInitCameraMatrix2D(VecPoint3f objPts, VecPoint2f imgPts) { imgPtsArr.push_back(imgPts); cameraMatrix = cv::initCameraMatrix2D(objPtsArr, imgPtsArr, cv::Size(1920, 1080), 1); - std::cout << cameraMatrix.type() << std::endl; return cameraMatrix; } -double GcvCalibrateCamera(VecPoint3f objPts, VecPoint2f imgPts, - cv::Size imgSize, cv::Mat cameraMatrix) { +double GcvCalibrateCamera_(VecPoint3f objPts, VecPoint2f imgPts, + cv::Size imgSize, cv::Mat& cameraMatrix, + cv::Mat& rvec, cv::Mat& tvec) { std::vector objPtsArr; std::vector imgPtsArr; std::vector rvecs, tvecs; cv::Mat distCoeffs; - double rtn; objPtsArr.push_back(objPts); imgPtsArr.push_back(imgPts); - std::cout << "init Camera" << cameraMatrix << std::endl; - rtn = cv::calibrateCamera(objPtsArr, imgPtsArr, imgSize, - cameraMatrix, distCoeffs, rvecs, tvecs); + cameraMatrix, distCoeffs, + rvecs, tvecs); - std::cout << "final Camera" << cameraMatrix << std::endl; - std::cout << "final rvecs" << rvecs[0] << std::endl; - std::cout << "final tvecs" << tvecs[0] << std::endl; + rvec = rvecs[0]; + tvec = tvecs[0]; return rtn; } diff --git a/gocv/gocv_calib3d.go b/gocv/gocv_calib3d.go new file mode 100644 index 0000000..624b79a --- /dev/null +++ b/gocv/gocv_calib3d.go @@ -0,0 +1,66 @@ +package gocv + +// #cgo CXXFLAGS: -std=c++11 +// #cgo darwin pkg-config: opencv +import "C" +import "github.com/gonum/matrix/mat64" + +// GcvInitCameraMatrix2D takes one N-by-3 matrix and one +// N-by-2 Matrix as input. +// Each row in the input matrix represents a point in real +// world (objPts) or in image (imgPts). +// Return: the camera matrix. +func GcvInitCameraMatrix2D(objPts, imgPts *mat64.Dense) (camMat *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 := NewGcvPoint3fVector(int64(nObjPts)) + imgPtsVec := NewGcvPoint2fVector(int64(nObjPts)) + + for i := 0; i < nObjPts; i++ { + objPtsVec.Set(i, NewGcvPoint3f( + objPts.At(i, 0), objPts.At(i, 1), objPts.At(i, 2))) + } + + for i := 0; i < nObjPts; i++ { + imgPtsVec.Set(i, NewGcvPoint2f( + imgPts.At(i, 0), imgPts.At(i, 1))) + } + + camMat = MatToMat64(GcvInitCameraMatrix2D_(objPtsVec, imgPtsVec)) + return 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)) + +// 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)) + +// imgSize := NewGcvSize2i(1920, 1080) + +// 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) +// } diff --git a/gocv/gocv_calib3d.hpp b/gocv/gocv_calib3d.hpp index 3710c3c..09195d2 100644 --- a/gocv/gocv_calib3d.hpp +++ b/gocv/gocv_calib3d.hpp @@ -5,7 +5,8 @@ typedef std::vector VecPoint3f; typedef std::vector VecPoint2f; -cv::Mat GcvInitCameraMatrix2D(VecPoint3f objPts, VecPoint2f imgPts); +cv::Mat GcvInitCameraMatrix2D_(VecPoint3f objPts, VecPoint2f imgPts); -double GcvCalibrateCamera(VecPoint3f objPts, VecPoint2f imgPts, - cv::Size2i imgSize, cv::Mat cameraMatrix); +double GcvCalibrateCamera_(VecPoint3f objPts, VecPoint2f imgPts, + cv::Size2i imgSize, cv::Mat& cameraMatrix, + cv::Mat& rvec, cv::Mat& tvec); diff --git a/gocv/gocv_core.i b/gocv/gocv_core.i index 924c98b..ee63dc6 100644 --- a/gocv/gocv_core.i +++ b/gocv/gocv_core.i @@ -1,8 +1,6 @@ %include "std_vector.i" %{ -#include "opencv2/core/types_c.h" -#include "opencv2/core/version.hpp" #include "opencv2/core/core.hpp" %} @@ -256,12 +254,39 @@ namespace cv { void pop_back(size_t nelems=1); //! special versions for 2D arrays (especially convenient for referencing image pixels) + + template _Tp& at(int i0=0); + template const _Tp& at(int i0=0) const; + + template _Tp& at(int i0, int i1); + template const _Tp& at(int i0, int i1) const; + + template _Tp& at(int i0, int i1, int i2); + template const _Tp& at(int i0, int i1, int i2) const; + + template _Tp& at(const int* idx); + template const _Tp& at(const int* idx) const; + + template _Tp& at(const Vec& idx); + template const _Tp& at(const Vec& idx) const; template _Tp& at(cv::Point pt); template const _Tp& at(cv::Point pt) const; - %template(gcvAtd) at; - %template(gcvAtf) at; - }; + %template(gcvAtf) at; + %template(gcvAtd) at; + + /*! includes several bit-fields: + - the magic signature + - continuity flag + - depth + - number of channels + */ + int flags; + //! the matrix dimensionality, >= 2 + int dims; + //! the number of rows and columns or (-1, -1) when the matrix has more than 2 dimensions + int rows, cols; + }; } /* Additional STL types */ diff --git a/gocv/gocv_test.go b/gocv/gocv_test.go index 5b1dbd1..bd0d775 100644 --- a/gocv/gocv_test.go +++ b/gocv/gocv_test.go @@ -4,6 +4,7 @@ import ( "testing" "github.com/davecgh/go-spew/spew" + "github.com/gonum/matrix/mat64" ) func TestNewGcvPoint3f(t *testing.T) { @@ -28,52 +29,48 @@ func TestMat(t *testing.T) { } func TestGcvInitCameraMatrix2D(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)) + 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}) 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))) + 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 := 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)) - 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 := 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)) - imgSize := NewGcvSize2i(1920, 1080) +// imgSize := NewGcvSize2i(1920, 1080) - 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))) +// 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))) - GcvCalibrateCamera(objPts, imgPts, imgSize, camMat) - 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) +// }