まともに外部パラメータの推定ができるように修正.

diff --git a/src/OMakefile b/src/OMakefile
index ce67f35..42a4721 100644 (file)
--- a/src/OMakefile
+++ b/src/OMakefile
@@ -30,9 +30,9 @@
 # If so, define the subdirectory targets and uncomment this section.
 #
 
-CXXFLAGS = -Wall -O2 -g -shared $(shell pkg-config --cflags opencv)
+CXXFLAGS = -Wall -O2 -g $(shell pkg-config --cflags opencv)
 LDFLAGS = $(shell pkg-config --libs opencv)
-.SUBDIRS: cpp
+.SUBDIRS: cpp test
 
 ########################################################################
 # C configuration.

diff --git a/src/cpp/OMakefile b/src/cpp/OMakefile
index a2f2fca..bb8ecf8 100644 (file)
--- a/src/cpp/OMakefile
+++ b/src/cpp/OMakefile
@@ -1,7 +1,7 @@
 SRCS = pdcamera pdvideo pdtracker
 TARGET = libpaldema
 
-CXXFLAGS += 
+CXXFLAGS += -shared
 LDFLAGS += 
 
 .DEFAULT: $(StaticCXXLibrary $(TARGET), $(SRCS))

diff --git a/src/cpp/pdcamera.cpp b/src/cpp/pdcamera.cpp
index 0b4e92f..093faf8 100644 (file)
--- a/src/cpp/pdcamera.cpp
+++ b/src/cpp/pdcamera.cpp
@@ -31,13 +31,13 @@ Camera::Camera(int scrx, int scry, float aspectRatio) {
   this->p2 = 0.0;
   this->aspectRatio = aspectRatio;
   this->isDetected = false;
-  this->rotationVector     = cvCreateMat(3, 1, CV_32SC1);
-  this->rotationMatrix     = cvCreateMat(3, 3, CV_32SC1);
-  this->angleVector        = cvCreateMat(3, 1, CV_32SC1);
-  this->angleMatrix        = cvCreateMat(3, 3, CV_32SC1);
-  this->translationVector  = cvCreateMat(3, 1, CV_32SC1);
-  this->positionVector     = cvCreateMat(3, 1, CV_32SC1);
-  this->cameraMatrix       = cvCreateMat(4, 4, CV_32SC1);
+  this->rotationVector     = cvCreateMat(3, 1, CV_32F);
+  this->rotationMatrix     = cvCreateMat(3, 3, CV_32FC1);
+  this->angleVector        = cvCreateMat(3, 1, CV_32FC1);
+  this->angleMatrix        = cvCreateMat(3, 3, CV_32FC1);
+  this->translationVector  = cvCreateMat(3, 1, CV_32FC1);
+  this->positionVector     = cvCreateMat(3, 1, CV_32FC1);
+  this->cameraMatrix       = cvCreateMat(4, 4, CV_32FC1);
   cvSetZero(this->rotationVector);
   cvSetZero(this->translationVector);
   cvSetZero(this->positionVector);
@@ -57,22 +57,25 @@ Camera::~Camera() {
 
 void Camera::detectParameters(float* uv, float* xyz, int num) {
   // initialize each matrix
-  CvMat uvpoints  = this->_makeUVMatrix(uv, num);
-  CvMat xyzpoints = this->_makeXYZMatrix(xyz, num);
+  CvMat* uvpoints  = this->_makeUVMatrix(uv, num);
+  CvMat* xyzpoints = this->_makeXYZMatrix(xyz, num);
   
   // set matrix's parameters
-  CvMat intrinstic_dst = this->_makeIntrinsticMatrix();
-  CvMat distortion_dst = cvMat(4, 1, CV_32SC1);
-  CvMat point_counts = cvMat(1, 1, CV_32SC1);
-  cvSetZero(&distortion_dst);
-  cvSetZero(&point_counts);
-  cvmSet(&point_counts, 0, 0, num);
+  CvMat* intrinstic_dst = this->_makeIntrinsticMatrix();
+  CvMat* distortion_dst = cvCreateMat(4, 1, CV_32FC1);
+  cvSetZero(distortion_dst);
+  
+  CvMat point_counts;
+  int p_count[1];
+  p_count[0] = num;
+  cvInitMatHeader(&point_counts, 1, 1, CV_32SC1, p_count);
   
   // detect
-  cvCalibrateCamera2(&xyzpoints, &uvpoints, &point_counts,
-                     cvSize(this->scrx, this->scry), &intrinstic_dst,
-                     &distortion_dst, this->rotationVector,
-                     this->translationVector, CV_CALIB_FIX_ASPECT_RATIO);
+  cvCalibrateCamera2(xyzpoints, uvpoints, &point_counts,
+                     cvSize(this->scrx, this->scry), intrinstic_dst,
+                     distortion_dst, NULL, NULL, CV_CALIB_FIX_ASPECT_RATIO);
+  cvFindExtrinsicCameraParams2(xyzpoints, uvpoints, intrinstic_dst, distortion_dst,
+                               this->rotationVector, this->translationVector);
   
   // transform vector -> matrix
   cvRodrigues2(this->rotationVector, this->rotationMatrix);
@@ -87,14 +90,14 @@ void Camera::detectParameters(float* uv, float* xyz, int num) {
   cvmSet(this->cameraMatrix, 1, 3, cvmGet(this->translationVector, 1, 0));
   cvmSet(this->cameraMatrix, 2, 3, cvmGet(this->translationVector, 2, 0));
   
-  this->fx = cvmGet(&intrinstic_dst, 0, 0);
-  this->cx = cvmGet(&intrinstic_dst, 0, 2);
-  this->fy = cvmGet(&intrinstic_dst, 1, 1);
-  this->cy = cvmGet(&intrinstic_dst, 1, 2);
-  this->k1 = cvmGet(&distortion_dst, 0, 0);
-  this->k2 = cvmGet(&distortion_dst, 1, 0);
-  this->p1 = cvmGet(&distortion_dst, 2, 0);
-  this->p2 = cvmGet(&distortion_dst, 3, 0);
+  this->fx = cvmGet(intrinstic_dst, 0, 0);
+  this->cx = cvmGet(intrinstic_dst, 0, 2);
+  this->fy = cvmGet(intrinstic_dst, 1, 1);
+  this->cy = cvmGet(intrinstic_dst, 1, 2);
+  this->k1 = cvmGet(distortion_dst, 0, 0);
+  this->k2 = cvmGet(distortion_dst, 1, 0);
+  this->p1 = cvmGet(distortion_dst, 2, 0);
+  this->p2 = cvmGet(distortion_dst, 3, 0);
   
   cvGEMM(this->rotationMatrix, this->translationVector, -1.0,
          NULL, 0.0, this->positionVector, CV_GEMM_A_T);
@@ -102,36 +105,38 @@ void Camera::detectParameters(float* uv, float* xyz, int num) {
   cvRodrigues2(this->angleMatrix, this->angleVector);
   
   this->isDetected = true;
+  cvReleaseMat(&uvpoints);
+  cvReleaseMat(&xyzpoints);
+  cvReleaseMat(&distortion_dst);
+  cvReleaseMat(&intrinstic_dst);
 }
 
-CvMat Camera::_makeUVMatrix(float* uv, int num) {
-  CvMat uvpoints  = cvMat(num, 2, CV_32SC1);
-  cvSetZero(&uvpoints);
+CvMat* Camera::_makeUVMatrix(float* uv, int num) {
+  CvMat* uvpoints  = cvCreateMat(num, 2, CV_32FC1);
   for(int i=0; i<num; i++) {
-    cvmSet(&uvpoints, i, 0, uv[i*2 + 0]);
-    cvmSet(&uvpoints, i, 1, uv[i*2 + 1]);
+    cvmSet(uvpoints, i, 0, uv[i*2 + 0]);
+    cvmSet(uvpoints, i, 1, uv[i*2 + 1]);
   }
   return uvpoints;
 }
 
-CvMat Camera::_makeXYZMatrix(float* xyz, int num) {
-  CvMat xyzpoints = cvMat(num, 3, CV_32SC1);
-  cvSetZero(&xyzpoints);
+CvMat* Camera::_makeXYZMatrix(float* xyz, int num) {
+  CvMat* xyzpoints = cvCreateMat(num, 3, CV_32FC1);
   for(int i=0; i<num; i++) {
-    cvmSet(&xyzpoints, i, 0, xyz[i*3 + 0]);
-    cvmSet(&xyzpoints, i, 1, xyz[i*3 + 1]);
-    cvmSet(&xyzpoints, i, 2, xyz[i*3 + 2]);
+    cvmSet(xyzpoints, i, 0, xyz[i*3 + 0]);
+    cvmSet(xyzpoints, i, 1, xyz[i*3 + 1]);
+    cvmSet(xyzpoints, i, 2, xyz[i*3 + 2]);
   }
   return xyzpoints;
 }
 
-CvMat Camera::_makeIntrinsticMatrix() {
-  CvMat dst = cvMat(3, 3, CV_32SC1);
-  cvSetZero(&dst);
-  cvmSet(&dst, 0, 0, this->fx);
-  cvmSet(&dst, 0, 2, this->cx);
-  cvmSet(&dst, 1, 1, this->fy);
-  cvmSet(&dst, 1, 2, this->cy);
-  cvmSet(&dst, 2, 2, 1.0);
+CvMat* Camera::_makeIntrinsticMatrix() {
+  CvMat* dst = cvCreateMat(3, 3, CV_32FC1);
+  cvSetZero(dst);
+  cvmSet(dst, 0, 0, this->fx);
+  cvmSet(dst, 0, 2, this->cx);
+  cvmSet(dst, 1, 1, this->fy);
+  cvmSet(dst, 1, 2, this->cy);
+  cvmSet(dst, 2, 2, 1.0);
   return dst;
 }

diff --git a/src/cpp/pdcamera.h b/src/cpp/pdcamera.h
index 0ce8808..c447c21 100644 (file)
--- a/src/cpp/pdcamera.h
+++ b/src/cpp/pdcamera.h
@@ -24,9 +24,9 @@ class Camera {
     Camera(const Camera& other);
     Camera& operator=(const Camera& other);
   protected:
-    CvMat _makeUVMatrix(float* uv, int num);
-    CvMat _makeXYZMatrix(float* xyz, int num);
-    CvMat _makeIntrinsticMatrix();
+    CvMat* _makeUVMatrix(float* uv, int num);
+    CvMat* _makeXYZMatrix(float* xyz, int num);
+    CvMat* _makeIntrinsticMatrix();
   public:
     int scrx;
     int scry;