Using Aruco markers (OpenCV) to map camera pose to SceneKit

Question

Trying to map the camera position from Aruco tracking back to the camera position in SceneKit. It's almost working but the tracking is really unstable and it appears the conversion to the SceneKit camera pose is incorrect as it's floating over the marker in the camera view and moving about as I move the camera around. Can anyone see what I may be doing wrong here in the conversion back to the sceneKit camera translation and position vectors?

@interface ArucoPayload : NSObject

@property BOOL valid; // Used to determine if the tracking was valid and hides the scenekit nodes if not
@property UIImage *image;
@property CGRect boardSize;
@property SCNVector4 rotationVector;
@property SCNVector3 translationVector;
@end

Mat rvec(3, 1, DataType<double>::type);
Mat tvec(3, 1, DataType<double>::type);

...
aruco::estimatePoseBoard(corners, markerIds, gridBoard, self.camMatrix, self.distCoeffs, rvec, tvec);
[self updateCameraProjection:payload withRotation:rvec andTranslation:tvec];
...

-(void) updateCameraProjection:(ArucoPayload *)payload withRotation:(Mat)rvec andTranslation:(Mat)tvec {

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

    cv::Mat R;
    cv::Rodrigues(rvec, R);

    R = R.t();
    Mat rvecConverted;
    Rodrigues(R, rvecConverted); 
    rvecConverted = RotX * rvecConverted;

    Mat tvecConverted = -R * tvec;
    tvecConverted = RotX * tvecConverted;

    payload.rotationVector = SCNVector4Make(rvecConverted.at<double>(0), rvecConverted.at<double>(1), rvecConverted.at<double>(2), norm(rvecConverted));
    payload.translationVector = SCNVector3Make(tvecConverted.at<double>(0), tvecConverted.at<double>(1), tvecConverted.at<double>(2));
}

func updateCameraPosition(payload:ArucoPayload) {

    if(payload.valid) {

        sceneView.scene?.rootNode.isHidden = false

        // Add nodes first time we get an updated position
        if(sceneView.scene?.rootNode.childNodes.count == 1) {

            // Add box node
            addBoxNode(to: sceneView, payload: payload)
        }

        cameraNode.rotation = payload.rotationVector
        cameraNode.position = payload.translationVector

    } else {

        sceneView.scene?.rootNode.isHidden = true
    }
}

The drawing done in OpenCV is correct and my axis and frame around the Aruco board is tracking accurately as can be seen in the video.

Any help is much appreciated. Here is the video of the scene. The yellow object which should be locked to the position of the marker seems very unstable.

https://youtu.be/ZvKtZ3DNdrk

Camera Calibration:

// Wait until we have captured enough frames
if(self.numberOfFramesForCalibration == 0) {

    NSLog(@"Starting calibration with 20 images");

    vector< vector< Point2f > > allCornersConcatenated;
    vector< int > allIdsConcatenated;
    vector< int > markerCounterPerFrame;
    Mat cameraMatrix, distCoeffs;
    vector< Mat > rvecs, tvecs;
    double repError;
    int calibrationFlags = 0;

    // prepare data for calibration
    markerCounterPerFrame.reserve(allCorners.size());
    for(unsigned int i = 0; i < allCorners.size(); i++) {
        markerCounterPerFrame.push_back((int)allCorners[i].size());
        for(unsigned int j = 0; j < allCorners[i].size(); j++) {
            allCornersConcatenated.push_back(allCorners[i][j]);
            allIdsConcatenated.push_back(allIds[i][j]);
        }
    }

    // calibrate camera
    repError = aruco::calibrateCameraAruco(allCornersConcatenated, allIdsConcatenated,
                                           markerCounterPerFrame, self.data.board, imgSize, cameraMatrix,
                                           distCoeffs, rvecs, tvecs);

    bool saveOk = [self saveCameraParams:imgSize aspect:1 flags:calibrationFlags matrix:cameraMatrix coeff:distCoeffs avgErr:repError];
    if(saveOk) {

        self.calibrationRequired = false;

    }
}   

Answer 1

Unfortunately, this isn’t a complete example, so it’s unclear where the error is. Anyway, there still seems to be a possible issue in what you’ve posted, so maybe my talking through my confusion will be helpful.

RotX seems intended to account for axis differences between OpenCV (X right, Y down, Z in) and SceneKit (X right, Y up, Z out).

tvec and rvec represent the world origin relative to the OpenCV camera.

R.t() is the same as R.inv() for orthonormal rotations, so R is now the inverse matrix of the original rvec via R = R.t().

So Mat tvecConverted = -R * tvec; (or more clearly R * -tvec) is the camera in world coordinates. Rodrigues(R, rvecConverted); seems a similar transform to the world frame for rvec.

Then, each are multiplied by RotX to take OpenCV coordinates to SceneKit coordinates and are assigned to payload.

UPDATE: After your code update, we see the values assigned to payload are ultimately assigned to cameraNode position and rotation values. Since a SceneKit SCNCamera always points along the negative Z-axis of its parent SCNNode with the same orientation, positioning and orienting the parent SCNNode positions and orients the camera itself. That suggests tvecConverted and rvecConverted above are correct, since the camera's SCNNode seems parented to the root node.

However, there's still the matter of projecting from the ScenKit camera's space to the pixel space of the display, which doesn't seem to be in the code excerpt that you posted. I suspect this will need to match the intrinsic camera matrix and distortion used when drawing on the OpenCV side for them to align correctly.

In the SCNCamera docs page, the projectionTransform property is described like this:

This transformation expresses the combination of all the camera’s geometric properties: projection type (perspective or orthographic), field of view, depth limits, and orthographic scale (if applicable). SceneKit uses this transformation to convert points in the camera node’s coordinate space to the renderer’s 2D space when rendering and processing events.

You can use this transformation directly if your app needs to convert between view and renderer coordinates for other purposes. Alternatively, if you compute your own projection transform matrix, you can set this property to override the transformation synthesized from the camera’s geometric properties.

Since you haven't posted code related to camera intrinsics, it's unclear if this is the issue or not. However, I suspect aligning the intrinsics between OpenCV and SceneKit will solve the problem. Also, SceneKit transforms are evaluated with respect to a node's pivot property, so you'll want to be careful if you're using that (e.g. to place a SCNBox relative to its corner rather than its center.)