From b0151e85e9a48364f3e78f8f4bfb9b6cd7293596 Mon Sep 17 00:00:00 2001 From: unknown Date: Tue, 13 May 2014 18:30:32 -0430 Subject: [PATCH] Added marker pose estimation. Not tested yet. --- jni/cv_proc.cpp | 68 +++++++++----- jni/marker.cpp | 40 ++++++-- jni/marker.hpp | 24 ++++- src/ve/ucv/ciens/ccg/nxtar/MainActivity.java | 97 +++++++++++++------- 4 files changed, 166 insertions(+), 63 deletions(-) diff --git a/jni/cv_proc.cpp b/jni/cv_proc.cpp index 3ecd1ff..9c38d88 100644 --- a/jni/cv_proc.cpp +++ b/jni/cv_proc.cpp @@ -21,6 +21,9 @@ #include "marker.hpp" //#define LOG_ENABLED +#define MAX_MARKERS 5 +#define TRANSLATION_VECTOR_POINTS 3 +#define ROTATION_MATRIX_SIZE 9 #define POINTS_PER_CALIBRATION_SAMPLE 54 #define CALIBRATION_SAMPLES 10 @@ -37,17 +40,21 @@ extern "C"{ /** * JNI wrapper around the nxtar::getAllMarkers() method. */ -JNIEXPORT void JNICALL Java_ve_ucv_ciens_ccg_nxtar_MainActivity_getMarkerCodesAndLocations(JNIEnv* env, jobject jobj, jlong addrMatIn, jlong addrMatOut, jintArray codes){ - char codeMsg[128]; - std::vector vCodes; - cv::Mat temp; - - log(TAG, "getMarkerCodesAndLocations(): Requesting native data."); +JNIEXPORT void JNICALL Java_ve_ucv_ciens_ccg_nxtar_MainActivity_getMarkerCodesAndLocations(JNIEnv* env, jobject jobj, jlong addrMatIn, jlong addrMatOut, jintArray codes, jlong camMat, jlong distMat, jfloatArray translations, jfloatArray rotations){ + char codeMsg[128]; + std::vector vCodes; + nxtar::markers_vector vMarkers; + cv::Mat temp; // Get the native object addresses. - cv::Mat& myuv = *(cv::Mat*)addrMatIn; - cv::Mat& mbgr = *(cv::Mat*)addrMatOut; + log(TAG, "getMarkerCodesAndLocations(): Requesting native data."); + cv::Mat& myuv = *(cv::Mat*)addrMatIn; + cv::Mat& mbgr = *(cv::Mat*)addrMatOut; + cv::Mat& mCam = *(cv::Mat*)camMat; + cv::Mat& mDist = *(cv::Mat*)distMat; jint * _codes = env->GetIntArrayElements(codes, 0); + jfloat * _tr = env->GetFloatArrayElements(translations, 0); + jfloat * _rt = env->GetFloatArrayElements(rotations, 0); // Convert the input image to the BGR color space. log(TAG, "getMarkerCodesAndLocations(): Converting color space before processing."); @@ -55,14 +62,32 @@ JNIEXPORT void JNICALL Java_ve_ucv_ciens_ccg_nxtar_MainActivity_getMarkerCodesAn // Find all markers in the input image. log(TAG, "getMarkerCodesAndLocations(): Finding markers."); - nxtar::getAllMarkers(vCodes, temp); + nxtar::getAllMarkers(vMarkers, temp); + nxtar::estimateMarkerPosition(vMarkers, mCam, mDist); // Copy the marker codes to the output vector. log(TAG, "getMarkerCodesAndLocations(): Copying marker codes."); - for(int i = 0; i < vCodes.size() && i < 15; i++){ - _codes[i] = (jint)vCodes[i]; + for(size_t i = 0; i < vMarkers.size() && i < MAX_MARKERS; i++){ + _codes[i] = (jint)vMarkers[i].code; } - vCodes.clear(); + + // Copy the geometric transformations to the output vectors. + for(int i = 0, p = 0; i < vMarkers.size(); i++, p += 3){ + _tr[p ] = vMarkers[i].translation.at(0); + _tr[p + 1] = vMarkers[i].translation.at(1); + _tr[p + 2] = vMarkers[i].translation.at(2); + } + + for(int k = 0; k < vMarkers.size(); k++){ + for(int row = 0; row < 3; row++){ + for(int col = 0; col < 3; col++){ + _rt[col + (row * 3) + (9 * k)] = vMarkers[k].rotation.at(row, col); + } + } + } + + // Clear marker memory. + vMarkers.clear(); // Convert the output image back to the RGB color space. cv::cvtColor(temp, mbgr, CV_BGR2RGB); @@ -70,6 +95,8 @@ JNIEXPORT void JNICALL Java_ve_ucv_ciens_ccg_nxtar_MainActivity_getMarkerCodesAn // Release native data. log(TAG, "getMarkerCodesAndLocations(): Releasing native data."); env->ReleaseIntArrayElements(codes, _codes, 0); + env->ReleaseFloatArrayElements(translations, _tr, 0); + env->ReleaseFloatArrayElements(rotations, _rt, 0); } /** @@ -77,14 +104,13 @@ JNIEXPORT void JNICALL Java_ve_ucv_ciens_ccg_nxtar_MainActivity_getMarkerCodesAn */ JNIEXPORT jboolean JNICALL Java_ve_ucv_ciens_ccg_nxtar_MainActivity_findCalibrationPattern(JNIEnv* env, jobject jobj, jlong addrMatIn, jlong addrMatOut, jfloatArray points){ nxtar::points_vector v_points; - bool found; - cv::Mat temp; - - log(TAG, "findCalibrationPattern(): Requesting native data."); + bool found; + cv::Mat temp; // Get the native object addresses. - cv::Mat& myuv = *(cv::Mat*)addrMatIn; - cv::Mat& mbgr = *(cv::Mat*)addrMatOut; + log(TAG, "findCalibrationPattern(): Requesting native data."); + cv::Mat& myuv = *(cv::Mat*)addrMatIn; + cv::Mat& mbgr = *(cv::Mat*)addrMatOut; jfloat * _points = env->GetFloatArrayElements(points, 0); // Convert the input image to the BGR color space. @@ -118,13 +144,13 @@ JNIEXPORT jboolean JNICALL Java_ve_ucv_ciens_ccg_nxtar_MainActivity_findCalibrat * JNI wrapper around the nxtar::getCameraParameters() method. */ JNIEXPORT jdouble JNICALL Java_ve_ucv_ciens_ccg_nxtar_MainActivity_calibrateCameraParameters(JNIEnv* env, jobject jobj, jlong addrMatIn, jlong addrMatOut, jlong addrMatFrame, jfloatArray points){ - double error; + double error; std::vector imagePoints; // Get native object addresses. log(TAG, "calibrateCameraParameters(): Requesting native data."); - cv::Mat& mIn = *(cv::Mat*)addrMatIn; - cv::Mat& mOut = *(cv::Mat*)addrMatOut; + cv::Mat& mIn = *(cv::Mat*)addrMatIn; + cv::Mat& mOut = *(cv::Mat*)addrMatOut; cv::Mat& mFrame = *(cv::Mat*)addrMatFrame; jfloat * _points = env->GetFloatArrayElements(points, 0); diff --git a/jni/marker.cpp b/jni/marker.cpp index f2a4618..ba51285 100644 --- a/jni/marker.cpp +++ b/jni/marker.cpp @@ -27,7 +27,6 @@ namespace nxtar{ typedef std::vector points_vector_3D; typedef std::vector > contours_vector; -typedef std::vector markers_vector; /****************************************************************************** * PRIVATE CONSTANTS * @@ -92,16 +91,15 @@ void warpMarker(Marker &, cv::Mat &, cv::Mat &); * PUBLIC API * ******************************************************************************/ -void getAllMarkers(std::vector & codes, cv::Mat & img){ +void getAllMarkers(markers_vector & valid_markers, cv::Mat & img){ cv::Mat gray, thresh, cont, mark; contours_vector contours; markers_vector markers; - markers_vector valid_markers; #ifdef DESKTOP std::ostringstream oss; #endif - codes.clear(); + valid_markers.clear(); // Find all marker candidates in the input image. // 1) First, convert the image to grayscale. @@ -147,7 +145,6 @@ void getAllMarkers(std::vector & codes, cv::Mat & img){ // Fix the detected corners to better approximate the markers. And // push their codes to the output vector. cv::cornerSubPix(gray, valid_markers[i].points, cvSize(10, 10), cvSize(-1, -1), TERM_CRITERIA); - codes.push_back(valid_markers[i].code); } // Render the detected markers on top of the input image. @@ -158,7 +155,6 @@ void getAllMarkers(std::vector & codes, cv::Mat & img){ // Clear the local vectors. markers.clear(); contours.clear(); - valid_markers.clear(); } bool findCalibrationPattern(points_vector & corners, cv::Mat & img){ @@ -204,6 +200,35 @@ double getCameraParameters(cv::Mat & camera_matrix, cv::Mat & dist_coeffs, std:: return cv::calibrateCamera(object_points, image_points, image_size, camera_matrix, dist_coeffs, rvecs, tvecs, 0, TERM_CRITERIA); } +void estimateMarkerPosition(markers_vector & markers, cv::Mat & camMatrix, cv::Mat & distCoeff){ + cv::Mat rVec, rAux, tAux; + cv::Mat_ tVec, rotation(3,3); + points_vector_3D objectPoints; + + // Assemble a unitary CCW oriented reference polygon. + objectPoints.push_back(cv::Point3f(-0.5f, -0.5f, 0.0f)); + objectPoints.push_back(cv::Point3f(-0.5f, 0.5f, 0.0f)); + objectPoints.push_back(cv::Point3f( 0.5f, 0.5f, 0.0f)); + objectPoints.push_back(cv::Point3f( 0.5f, -0.5f, 0.0f)); + + for(size_t i = 0; i < markers.size(); i++){ + // Obtain the translation and rotation vectors. + cv::solvePnP(objectPoints, markers[i].points, camMatrix, distCoeff, rAux, tAux); + + // Convert the obtained vectors to float. + rAux.convertTo(rVec, CV_32F); + tAux.convertTo(tVec, CV_32F); + + // Convert the rotation vector to a rotation matrix. + cv::Rodrigues(rVec, rotation); + + // Make the rotation and translation relative to the "camera" and save + // the results to the output marker. + markers[i].rotation = cv::Mat(rotation.t()); + markers[i].translation = cv::Mat(-tVec); + } +} + /****************************************************************************** * PRIVATE HELPER FUNCTIONS * ******************************************************************************/ @@ -357,6 +382,7 @@ void warpMarker(Marker & m, cv::Mat & in, cv::Mat & out){ /** * Calculate the hamming distance of a 5x5 bit matrix. + * Function by Daniel Lelis Baggio for "Mastering OpenCV with Practical Computer Vision Projects". */ int hammDistMarker(cv::Mat bits){ int ids[4][5] = { @@ -397,7 +423,7 @@ cv::Mat rotate(cv::Mat in){ in.copyTo(out); for (int i = 0; i < in.rows; i++){ for (int j = 0; j < in.cols; j++){ - out.at(i, j)=in.at(in.cols-j - 1, i); + out.at(i, j) = in.at(in.cols-j - 1, i); } } diff --git a/jni/marker.hpp b/jni/marker.hpp index 2f0c02b..2bae666 100644 --- a/jni/marker.hpp +++ b/jni/marker.hpp @@ -22,20 +22,30 @@ namespace nxtar{ -typedef std::vector points_vector; +class Marker; +typedef std::vector points_vector; +typedef std::vector markers_vector; + +/** + * A class representing a marker with the geometric transformations needed to + * render something on top of it. + */ class Marker{ public: ~Marker(); + + int code; points_vector points; - int code; + cv::Mat translation; + cv::Mat rotation; }; /** * Detect all 5x5 markers in the input image and return their codes in the * output vector. */ -void getAllMarkers(std::vector &, cv::Mat &); +void getAllMarkers(markers_vector &, cv::Mat &); /** * Find a chessboard calibration pattern in the input image. Returns true @@ -51,6 +61,14 @@ bool findCalibrationPattern(points_vector &, cv::Mat &); */ double getCameraParameters(cv::Mat &, cv::Mat &, std::vector &, cv::Size); +/** + * Obtains the necesary geometric transformations necessary to move a reference + * unitary polygon to the position and rotation of the markers passed as input. + * The obtained transformations are given relative to a camera centered in the + * origin and are saved inside the input markers. + */ +void estimateMarkerPosition(markers_vector &, cv::Mat &, cv::Mat &); + } // namespace nxtar #endif // MARKER_HPP diff --git a/src/ve/ucv/ciens/ccg/nxtar/MainActivity.java b/src/ve/ucv/ciens/ccg/nxtar/MainActivity.java index 7c12214..e777b79 100644 --- a/src/ve/ucv/ciens/ccg/nxtar/MainActivity.java +++ b/src/ve/ucv/ciens/ccg/nxtar/MainActivity.java @@ -24,8 +24,8 @@ import org.opencv.android.Utils; import org.opencv.core.Mat; import org.opencv.imgproc.Imgproc; -import ve.ucv.ciens.ccg.nxtar.interfaces.CVProcessor; -import ve.ucv.ciens.ccg.nxtar.interfaces.OSFunctionalityProvider; +import ve.ucv.ciens.ccg.nxtar.interfaces.AndroidFunctionalityWrapper; +import ve.ucv.ciens.ccg.nxtar.interfaces.ImageProcessor; import ve.ucv.ciens.ccg.nxtar.utils.ProjectConstants; import android.content.Context; import android.content.pm.ActivityInfo; @@ -42,6 +42,8 @@ import com.badlogic.gdx.Gdx; import com.badlogic.gdx.backends.android.AndroidApplication; import com.badlogic.gdx.backends.android.AndroidApplicationConfiguration; import com.badlogic.gdx.controllers.mappings.Ouya; +import com.badlogic.gdx.math.Matrix3; +import com.badlogic.gdx.math.Vector3; /** *

The main activity of the application.

@@ -49,7 +51,7 @@ import com.badlogic.gdx.controllers.mappings.Ouya; *

Provides operating system services to the LibGDX platform * independant code, and handles OpenCV initialization and api calls.

*/ -public class MainActivity extends AndroidApplication implements OSFunctionalityProvider, CVProcessor{ +public class MainActivity extends AndroidApplication implements AndroidFunctionalityWrapper, ImageProcessor{ /** * Tag used for logging. */ @@ -115,9 +117,13 @@ public class MainActivity extends AndroidApplication implements OSFunctionalityP * * @param inMat INPUT. The image to analize. * @param outMat OUTPUT. The image with the markers highlighted. - * @param codes OUTPUT. The codes for each marker detected. Must be 15 elements long. + * @param codes OUTPUT. The codes for each marker detected. Must be {@link ProjectConstants.MAXIMUM_NUMBER_OF_MARKERS} elements long. + * @param camMat INPUT. The intrinsic camera matrix. + * @param distMat INPUT. The distortion coefficients of the camera. + * @param translations OUTPUT. The markers pose translations. Must be {@link ProjectConstants.MAXIMUM_NUMBER_OF_MARKERS} * 3 elements long. + * @param rotations OUTPUT. The markers pose rotations. Must be {@link ProjectConstants.MAXIMUM_NUMBER_OF_MARKERS} * 9 elements long. */ - private native void getMarkerCodesAndLocations(long inMat, long outMat, int[] codes); + private native void getMarkerCodesAndLocations(long inMat, long outMat, int[] codes, long camMat, long distMat, float[] translations, float[] rotations); /** *

Wrapper for the findCalibrationPattern native function.

@@ -298,7 +304,7 @@ public class MainActivity extends AndroidApplication implements OSFunctionalityP /** *

Implementation of the findMarkersInFrame method.

* - *

This implementation finds up to 15 markers in the input + *

This implementation finds up to {@link ProjectConstants.MAXIMUM_NUMBER_OF_MARKERS} markers in the input * image and returns their codes and locations in the CVMarkerData * structure. The markers are higlihted in the input image.

* @@ -307,37 +313,64 @@ public class MainActivity extends AndroidApplication implements OSFunctionalityP * detected marker codes and their respective locations. */ @Override - public CVMarkerData findMarkersInFrame(byte[] frame){ + public MarkerData findMarkersInFrame(byte[] frame){ if(ocvOn){ - int codes[] = new int[15]; - Bitmap tFrame, mFrame; - Mat inImg = new Mat(); - Mat outImg = new Mat(); + if(cameraCalibrated){ + int[] codes = new int[ProjectConstants.MAXIMUM_NUMBER_OF_MARKERS]; + float[] translations = new float[ProjectConstants.MAXIMUM_NUMBER_OF_MARKERS * 3]; + float[] rotations = new float[ProjectConstants.MAXIMUM_NUMBER_OF_MARKERS * 9]; + MarkerData data; + Bitmap tFrame, mFrame; + Mat inImg = new Mat(); + Mat outImg = new Mat(); - // Decode the input image and convert it to an OpenCV matrix. - tFrame = BitmapFactory.decodeByteArray(frame, 0, frame.length); - Utils.bitmapToMat(tFrame, inImg); + // Fill the codes array with -1 to indicate markers that were not found; + for(int i : codes) + codes[i] = -1; - // Find up to 15 markers in the input image. - getMarkerCodesAndLocations(inImg.getNativeObjAddr(), outImg.getNativeObjAddr(), codes); + // Decode the input image and convert it to an OpenCV matrix. + tFrame = BitmapFactory.decodeByteArray(frame, 0, frame.length); + Utils.bitmapToMat(tFrame, inImg); - // Encode the output image as a JPEG image. - mFrame = Bitmap.createBitmap(outImg.cols(), outImg.rows(), Bitmap.Config.RGB_565); - Utils.matToBitmap(outImg, mFrame); - mFrame.compress(CompressFormat.JPEG, 100, outputStream); + // Find the markers in the input image. + getMarkerCodesAndLocations(inImg.getNativeObjAddr(), outImg.getNativeObjAddr(), codes, cameraMatrix.getNativeObjAddr(), distortionCoeffs.getNativeObjAddr(), translations, rotations); - // Create the output data structure. - CVMarkerData data = new CVMarkerData(); - data.outFrame = outputStream.toByteArray(); - data.markerCodes = codes; + // Encode the output image as a JPEG image. + mFrame = Bitmap.createBitmap(outImg.cols(), outImg.rows(), Bitmap.Config.RGB_565); + Utils.matToBitmap(outImg, mFrame); + mFrame.compress(CompressFormat.JPEG, 100, outputStream); - // Clean up memory. - tFrame.recycle(); - mFrame.recycle(); - outputStream.reset(); + // Create and fill the output data structure. + data = new MarkerData(); + data.outFrame = outputStream.toByteArray(); + data.markerCodes = codes; + data.rotationMatrices = new Matrix3[ProjectConstants.MAXIMUM_NUMBER_OF_MARKERS]; + data.translationVectors = new Vector3[ProjectConstants.MAXIMUM_NUMBER_OF_MARKERS]; - return data; + for(int i = 0, p = 0; i < ProjectConstants.MAXIMUM_NUMBER_OF_MARKERS; i++, p += 3){ + data.translationVectors[i] = new Vector3(translations[p], translations[p + 1], translations[p + 2]); + } + // TODO: Check that the matrix is being copied correctly. + for(int k = 0; k < ProjectConstants.MAXIMUM_NUMBER_OF_MARKERS; k++){ + data.rotationMatrices[k] = new Matrix3(); + for(int row = 0; row < 3; row++){ + for(int col = 0; col < 3; col++){ + data.rotationMatrices[k].val[col + (row * 3)] = rotations[col + (row * 3) + (9 * k)]; + } + } + } + + // Clean up memory. + tFrame.recycle(); + mFrame.recycle(); + outputStream.reset(); + + return data; + }else{ + Gdx.app.debug(TAG, CLASS_NAME + ".findMarkersInFrame(): The camera has not been calibrated."); + return null; + } }else{ Gdx.app.debug(TAG, CLASS_NAME + ".findMarkersInFrame(): OpenCV is not ready or failed to load."); return null; @@ -358,13 +391,13 @@ public class MainActivity extends AndroidApplication implements OSFunctionalityP * calibration points array is null. */ @Override - public CVCalibrationData findCalibrationPattern(byte[] frame){ + public CalibrationData findCalibrationPattern(byte[] frame){ if(ocvOn){ boolean found; float points[] = new float[ProjectConstants.CALIBRATION_PATTERN_POINTS * 2]; Bitmap tFrame, mFrame; Mat inImg = new Mat(), outImg = new Mat(); - CVCalibrationData data = new CVCalibrationData(); + CalibrationData data = new CalibrationData(); // Decode the input frame and convert it to an OpenCV Matrix. tFrame = BitmapFactory.decodeByteArray(frame, 0, frame.length); @@ -488,7 +521,7 @@ public class MainActivity extends AndroidApplication implements OSFunctionalityP * @return True if and only if OpenCV initialized succesfully and calibrateCamera has been called previously. */ @Override - public boolean cameraIsCalibrated() { + public boolean isCameraCalibrated() { return ocvOn && cameraCalibrated; } }