Use testify to make real assertions.

gocv/gocv_calib3d.cpp

@@ -30,12 +30,27 @@ double GcvCalibrateCamera_(VecPoint3f objPts, VecPoint2f imgPts,
         objPtsArr.push_back(objPts);
         imgPtsArr.push_back(imgPts);
 
+        std::cout << "objPts " << std::endl << objPtsArr[0] << std::endl;
+        std::cout << "imgPts " << std::endl << imgPtsArr[0] << std::endl;
+        std::cout << "imgSize " << std::endl << imgSize << std::endl;
+        std::cout << "Before CamMat " << std::endl << cameraMatrix << std::endl;
+
         rtn = cv::calibrateCamera(objPtsArr, imgPtsArr, imgSize,
                                   cameraMatrix, distCoeffs,
-                                  rvecs, tvecs);
+                                  rvecs, tvecs, 14575);
 
         rvec = rvecs[0];
         tvec = tvecs[0];
 
+        std::cout << "After CamMat " << std::endl << cameraMatrix << std::endl;
+        std::cout << "distCoeffs " << std::endl << distCoeffs << std::endl;
+        std::cout << "rvec " << std::endl << rvec << std::endl;
+        std::cout << "tvec " << std::endl << tvec << std::endl;
+        std::cout << "rms " << std::endl << rtn << std::endl;
+
         return rtn;
 }
+
+void GcvRodrigues_(cv::Mat src, cv::Mat& dst) {
+        cv::Rodrigues(src, dst);
+}

gocv/gocv_calib3d.go

@@ -65,7 +65,17 @@ func GcvCalibrateCamera(objPts, imgPts, camMat *mat64.Dense) (calCamMat, rvec, t
 	rvec = GcvMatToMat64(_rvec)
 	tvec = GcvMatToMat64(_tvec)
 
-	return camMat, rvec, tvec
+	return calCamMat, rvec, tvec
+}
+
+// Same as cv::Rodrigues
+func GcvRodrigues(src *mat64.Dense) (dst *mat64.Dense) {
+	gcvSrc := Mat64ToGcvMat(src)
+	gcvDst := NewGcvMat()
+	GcvRodrigues_(gcvSrc, gcvDst)
+	dst = GcvMatToMat64(gcvDst)
+
+	return dst
 }
 
 // func mat64ToGcvPoint3f32Vector(mat *mat64.Dense) NewGcvPoint3f32Vector {

gocv/gocv_calib3d.hpp

@@ -10,3 +10,5 @@ cv::Mat GcvInitCameraMatrix2D_(VecPoint3f objPts, VecPoint2f imgPts);
 double GcvCalibrateCamera_(VecPoint3f objPts, VecPoint2f imgPts,
                            cv::Size2i imgSize, cv::Mat& cameraMatrix,
                            cv::Mat& rvec, cv::Mat& tvec);
+
+void GcvRodrigues_(cv::Mat src, cv::Mat& dst);

gocv/gocv_calib3d_test.go

@@ -3,8 +3,8 @@ package gocv
 import (
 	"testing"
 
-	"github.com/davecgh/go-spew/spew"
 	"github.com/gonum/matrix/mat64"
+	"github.com/stretchr/testify/assert"
 )
 
 func TestGcvInitCameraMatrix2D(t *testing.T) {
@@ -12,16 +12,20 @@ func TestGcvInitCameraMatrix2D(t *testing.T) {
 		0, 25, 0,
 		0, -25, 0,
 		-47, 25, 0,
-		-47, -25, 0})
+		-47, -25, 0,
+	})
 
 	imgPts := mat64.NewDense(4, 2, []float64{
 		1136.4140625, 1041.89208984,
 		1845.33190918, 671.39581299,
 		302.73373413, 634.79998779,
-		1051.46154785, 352.76107788})
+		1051.46154785, 352.76107788,
+	})
 
 	camMat := GcvInitCameraMatrix2D(objPts, imgPts)
-	spew.Dump(camMat)
+	assert.Equal(t, camMat.Row(nil, 0), []float64{4828.129063751587, 0, 959.5})
+	assert.Equal(t, camMat.Row(nil, 1), []float64{0, 4828.129063751587, 539.5})
+	assert.Equal(t, camMat.Row(nil, 2), []float64{0, 0, 1})
 }
 
 func TestGcvCalibrateCamera(t *testing.T) {
@@ -29,18 +33,45 @@ func TestGcvCalibrateCamera(t *testing.T) {
 		0, 25, 0,
 		0, -25, 0,
 		-47, 25, 0,
-		-47, -25, 0})
+		-47, -25, 0,
+	})
 
 	imgPts := mat64.NewDense(4, 2, []float64{
 		1136.4140625, 1041.89208984,
 		1845.33190918, 671.39581299,
 		302.73373413, 634.79998779,
-		1051.46154785, 352.76107788})
+		1051.46154785, 352.76107788,
+	})
 
 	camMat := GcvInitCameraMatrix2D(objPts, imgPts)
 
 	camMat, rvec, tvec := GcvCalibrateCamera(objPts, imgPts, camMat)
-	spew.Dump(camMat)
+
-	spew.Dump(rvec)
+	assert.Equal(t, camMat.Row(nil, 0), []float64{5.47022369e+03, 0.00000000e+00, 9.59500000e+02})
-	spew.Dump(tvec)
+	assert.Equal(t, camMat.Row(nil, 1), []float64{0.00000000e+00, 5.47022369e+03, 5.39500000e+02})
+	assert.Equal(t, camMat.Row(nil, 2), []float64{0.00000000e+00, 0.00000000e+00, 1.00000000e+00})
+
+	assert.Equal(t, rvec.Col(nil, 0), []float64{-0.99458984, 0.54674764, -2.69721055})
+	assert.Equal(t, tvec.Col(nil, 0), []float64{-23.25417757, -12.6155423, -227.64212085})
+}
+
+func TestGcvRodrigues(t *testing.T) {
+	rvec := mat64.NewDense(3, 1, []float64{
+		-0.99458984,
+		0.54674764,
+		-2.69721055,
+	})
+	rmat := GcvRodrigues(rvec)
+
+	assert.InDelta(t, rmat.At(0, 0), -0.74853587, 1e-6)
+	assert.InDelta(t, rmat.At(0, 1), 0.07139127, 1e-6)
+	assert.InDelta(t, rmat.At(0, 2), 0.65923997, 1e-6)
+
+	assert.InDelta(t, rmat.At(1, 0), -0.32247419, 1e-6)
+	assert.InDelta(t, rmat.At(1, 1), -0.90789575, 1e-6)
+	assert.InDelta(t, rmat.At(1, 2), -0.26783521, 1e-6)
+
+	assert.InDelta(t, rmat.At(2, 0), 0.57940008, 1e-6)
+	assert.InDelta(t, rmat.At(2, 1), -0.41307214, 1e-6)
+	assert.InDelta(t, rmat.At(2, 2), 0.70261437, 1e-6)
 }