Revision history [back]

I have applied the same code to my Aruco project to get the pose of the camera in a suitable form for SceneKit. It kinda works. The camera appears in the correct place relative to a node (box) I've placed in the scene. But it moves about a lot as if the pose is very unstable.

Within the openCV part I have drawn boxes around the tracking marker and they are completely stable.

Have you any idea what might be going wrong here?

// Create a SceneKit camera pose to send back to our model
-(void) updateCameraProjection:(ArucoPayload *)payload withRotation:(Vec3d)rvec andTranslation:(Vec3d)tvec {

Mat RotX(3, 3, cv::DataType<double>::type);
setIdentity(RotX);
RotX.at<double>(4) = -1; //cos(180) = -1
RotX.at<double>(8) = -1;

Mat R;
Rodrigues(rvec, R);
R = R.t();  // rotation of inverse
Mat rvecWorld, rvecSceneKit;
Rodrigues(R, rvecWorld); // rvec in world coords
rvecSceneKit = RotX * rvecWorld; // rvec scenekit

Mat tvecWorld = -R * (Mat)tvec, tvecSceneKit; // tvec world
tvecSceneKit = RotX * tvecWorld; // tvec SceneKit

// Apply rotation
Mat rotation;
Rodrigues(rvecSceneKit, rotation);

Mat transform_matrix;
transform_matrix.create(4, 4, CV_64FC1);
transform_matrix( cv::Range(0,3), cv::Range(0,3) ) = rotation * 1;
transform_matrix.at<double>(0, 3) = tvecSceneKit.at<double>(0,0);
transform_matrix.at<double>(1, 3) = tvecSceneKit.at<double>(1,0);
transform_matrix.at<double>(2, 3) = tvecSceneKit.at<double>(2,0);
transform_matrix.at<double>(3, 3) = 1;

payload.cameraTransform = [self transformToSceneKitMatrix:transform_matrix];
}

-(SCNMatrix4) transformToSceneKitMatrix:(cv::Mat&) openCVTransformation {

SCNMatrix4 scnCameraPose = SCNMatrix4Identity;

scnCameraPose.m11 = (float) openCVTransformation.at<double>(0, 0);
scnCameraPose.m12 = (float) openCVTransformation.at<double>(1, 0);
scnCameraPose.m13 = (float) openCVTransformation.at<double>(2, 0);
scnCameraPose.m14 = (float) openCVTransformation.at<double>(3, 0);

scnCameraPose.m21 = (float)openCVTransformation.at<double>(0, 1);
scnCameraPose.m22 = (float)openCVTransformation.at<double>(1, 1);
scnCameraPose.m23 = (float)openCVTransformation.at<double>(2, 1);
scnCameraPose.m24 = (float)openCVTransformation.at<double>(3, 1);

scnCameraPose.m31 = (float)openCVTransformation.at<double>(0, 2);
scnCameraPose.m32 = (float)openCVTransformation.at<double>(1, 2);
scnCameraPose.m33 = (float)openCVTransformation.at<double>(2, 2);
scnCameraPose.m34 = (float)openCVTransformation.at<double>(3, 2);

//copy the translation row
scnCameraPose.m41 = (float)openCVTransformation.at<double>(0, 3);
scnCameraPose.m42 = (float)openCVTransformation.at<double>(1, 3);
scnCameraPose.m43 = (float)openCVTransformation.at<double>(2, 3);
scnCameraPose.m44 = (float)openCVTransformation.at<double>(3, 3);

return scnCameraPose;

}