// find points in common from points1 and points2
// since points1[0] and points2[0] might not be equal...
FloatBuffer objPts1 = points1[0].getFloatBuffer();
FloatBuffer imgPts1 = points1[1].getFloatBuffer();
IntBuffer imgCount1 = points1[2].getIntBuffer();
FloatBuffer objPts2 = points2[0].getFloatBuffer();
FloatBuffer imgPts2 = points2[1].getFloatBuffer();
IntBuffer imgCount2 = points2[2].getIntBuffer();
assert(imgCount1.capacity() == imgCount2.capacity());
CvMat objectPointsMat = CvMat.create(1, Math.min(objPts1.capacity(), objPts2.capacity()), CV_32F, 3);
CvMat imagePoints1Mat = CvMat.create(1, Math.min(imgPts1.capacity(), imgPts2.capacity()), CV_32F, 2);
CvMat imagePoints2Mat = CvMat.create(1, Math.min(imgPts1.capacity(), imgPts2.capacity()), CV_32F, 2);
CvMat pointCountsMat = CvMat.create(1, imgCount1.capacity(), CV_32S, 1);
FloatBuffer objectPoints = objectPointsMat.getFloatBuffer();
FloatBuffer imagePoints1 = imagePoints1Mat.getFloatBuffer();
FloatBuffer imagePoints2 = imagePoints2Mat.getFloatBuffer();
IntBuffer pointCounts = pointCountsMat .getIntBuffer();
int end1 = 0, end2 = 0;
for (int i = 0; i < imgCount1.capacity(); i++) {
int start1 = end1;
int start2 = end2;
end1 = start1 + imgCount1.get(i);
end2 = start2 + imgCount2.get(i);
int count = 0;
for (int j = start1; j < end1; j++) {
float x1 = objPts1.get(j*3 );
float y1 = objPts1.get(j*3+1);
float z1 = objPts1.get(j*3+2);
for (int k = start2; k < end2; k++) {
float x2 = objPts2.get(k*3 );
float y2 = objPts2.get(k*3+1);
float z2 = objPts2.get(k*3+2);
if (x1 == x2 && y1 == y2 && z1 == z2) {
objectPoints.put(x1);
objectPoints.put(y1);
objectPoints.put(z1);
imagePoints1.put(imgPts1.get(j*2));
imagePoints1.put(imgPts1.get(j*2+1));
imagePoints2.put(imgPts2.get(k*2));
imagePoints2.put(imgPts2.get(k*2+1));
count++;
break;
}
}
}
if (count > 0) {
pointCounts.put(count);
}
}
objectPointsMat.cols(objectPoints.position()/3);
imagePoints1Mat.cols(imagePoints1.position()/2);
imagePoints2Mat.cols(imagePoints2.position()/2);
pointCountsMat .cols(pointCounts .position());
// place our ProjectiveDevice at the origin...
d.R = CvMat.create(3, 3); d.R.put(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
d.T = CvMat.create(3, 1); d.T.put(0.0, 0.0, 0.0);