Added marker pose estimation. Not tested yet.

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){
+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<int>      vCodes;
+	nxtar::markers_vector vMarkers;
 	cv::Mat               temp;
 
-	log(TAG, "getMarkerCodesAndLocations(): Requesting native data.");
-
 	// Get the native object addresses.
+	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++){
+	for(size_t i = 0; i < vMarkers.size() && i < MAX_MARKERS; i++){
-		_codes[i] = (jint)vCodes[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<jfloat>(0);
+		_tr[p + 1] = vMarkers[i].translation.at<jfloat>(1);
+		_tr[p + 2] = vMarkers[i].translation.at<jfloat>(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<jfloat>(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);
 }
 
 /**
@@ -80,9 +107,8 @@ JNIEXPORT jboolean JNICALL Java_ve_ucv_ciens_ccg_nxtar_MainActivity_findCalibrat
 	bool                 found;
 	cv::Mat              temp;
 
-	log(TAG, "findCalibrationPattern(): Requesting native data.");
-
 	// Get the native object addresses.
+	log(TAG, "findCalibrationPattern(): Requesting native data.");
 	cv::Mat& myuv    = *(cv::Mat*)addrMatIn;
 	cv::Mat& mbgr    = *(cv::Mat*)addrMatOut;
 	jfloat * _points = env->GetFloatArrayElements(points, 0);

jni/marker.cpp

@@ -27,7 +27,6 @@ namespace nxtar{
 
 typedef std::vector<cv::Point3f>             points_vector_3D;
 typedef std::vector<std::vector<cv::Point> > contours_vector;
-typedef std::vector<Marker>                  markers_vector;
 
 /******************************************************************************
  * PRIVATE CONSTANTS                                                          *
@@ -92,16 +91,15 @@ void warpMarker(Marker &, cv::Mat &, cv::Mat &);
  * PUBLIC API                                                                 *
  ******************************************************************************/
 
-void getAllMarkers(std::vector<int> & 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<int> & 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<int> & 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_<float> 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<uchar>(i, j)=in.at<uchar>(in.cols-j - 1, i);
+			out.at<uchar>(i, j) = in.at<uchar>(in.cols-j - 1, i);
 		}
 	}
 

jni/marker.hpp

@@ -22,20 +22,30 @@
 
 namespace nxtar{
 
-typedef std::vector<cv::Point2f> points_vector;
+class Marker;
 
+typedef std::vector<cv::Point2f> points_vector;
+typedef std::vector<Marker>      markers_vector;
+
+/**
+ * A class representing a marker with the geometric transformations needed to
+ * render something on top of it.
+ */
 class Marker{
 public:
 	~Marker();
-	points_vector points;
+
 	int           code;
+	points_vector points;
+	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<int> &, 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<points_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

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.AndroidFunctionalityWrapper;
-import ve.ucv.ciens.ccg.nxtar.interfaces.OSFunctionalityProvider;
+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;
 
 /**
  * <p>The main activity of the application.</p>
@@ -49,7 +51,7 @@ import com.badlogic.gdx.controllers.mappings.Ouya;
  * <p>Provides operating system services to the LibGDX platform
  * independant code, and handles OpenCV initialization and api calls.</p>
  */
-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);
 
 	/**
 	 * <p>Wrapper for the findCalibrationPattern native function.</p>
@@ -298,7 +304,7 @@ public class MainActivity extends AndroidApplication implements OSFunctionalityP
 	/**
 	 * <p>Implementation of the findMarkersInFrame method.</p>
 	 * 
-	 * <p>This implementation finds up to 15 markers in the input
+	 * <p>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.</p>
 	 * 
@@ -307,29 +313,53 @@ 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];
+			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();
 
+				// Fill the codes array with -1 to indicate markers that were not found;
+				for(int i : codes)
+					codes[i] = -1;
+
 				// Decode the input image and convert it to an OpenCV matrix.
 				tFrame = BitmapFactory.decodeByteArray(frame, 0, frame.length);
 				Utils.bitmapToMat(tFrame, inImg);
 
-			// Find up to 15 markers in the input image.
+				// Find the markers in the input image.
-			getMarkerCodesAndLocations(inImg.getNativeObjAddr(), outImg.getNativeObjAddr(), codes);
+				getMarkerCodesAndLocations(inImg.getNativeObjAddr(), outImg.getNativeObjAddr(), codes, cameraMatrix.getNativeObjAddr(), distortionCoeffs.getNativeObjAddr(), translations, rotations);
 
 				// 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);
 
-			// Create the output data structure.
+				// Create and fill the output data structure.
-			CVMarkerData data = new CVMarkerData();
+				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];
+
+				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();
@@ -337,7 +367,10 @@ public class MainActivity extends AndroidApplication implements OSFunctionalityP
 				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;
 	}
 }