Augmented reality with Aruco and SceneKit - ios

Question

I'm trying to make augmented reality demo project with simple 3d object in the center of marker. I need to make it with OpenCV and SceneKit.
My steps are:
I obtain the corners of marker using Aruco
with cv::solvePnP get the tvec and rvec.
convert the tvec and rvec from OpenCv's Coordinate System to the SceneKit Coordinate System.
apply the converted rotation and translation to the camera node.
The Problem is:
The object is not centered on the marker. Rotation of object looks good. But is not positioned where it should be.
SolvePnp code:
cv::Mat intrinMat(3,3,cv::DataType<double>::type);
//From ARKit (ARFrame camera.intrinsics) - iphone 6s plus
intrinMat.at<double>(0,0) = 1662.49;
intrinMat.at<double>(0,1) = 0.0;
intrinMat.at<double>(0,2) = 0.0;
intrinMat.at<double>(1,0) = 0.0;
intrinMat.at<double>(1,1) = 1662.49;
intrinMat.at<double>(1,2) = 0.0;
intrinMat.at<double>(2,0) = 960.0 / 2;
intrinMat.at<double>(2,1) = 540.0 / 2;
intrinMat.at<double>(2,2) = 0.0;
double marker_dim = 3;
cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
CVPixelBufferLockBaseAddress(pixelBuffer, 0);
void *baseaddress = CVPixelBufferGetBaseAddressOfPlane(pixelBuffer, 0);
CGFloat width = CVPixelBufferGetWidth(pixelBuffer);
CGFloat height = CVPixelBufferGetHeight(pixelBuffer);
cv::Mat mat(height, width, CV_8UC1, baseaddress, 0); //CV_8UC1
cv::rotate(mat, mat, cv::ROTATE_90_CLOCKWISE);
std::vector<int> ids;
std::vector<std::vector<cv::Point2f>> corners;
cv::aruco::detectMarkers(mat,dictionary,corners,ids);
if(ids.size() > 0) {
cv::Mat colorMat;
cv::cvtColor(mat, colorMat, CV_GRAY2RGB);
cv::aruco::drawDetectedMarkers(colorMat, corners, ids, cv::Scalar(0,255,24));
cv::Mat distCoeffs = cv::Mat::zeros(8, 1, cv::DataType<double>::type); //zero out distortion for now
//MARK: solvepnp
std::vector<cv::Point3f> object_points;
object_points = {cv::Point3f(-marker_dim , marker_dim , 0),
cv::Point3f(marker_dim , marker_dim , 0),
cv::Point3f(marker_dim , -marker_dim , 0),
cv::Point3f(-marker_dim , -marker_dim , 0)};
std::vector<cv::Point_<float>> image_points = std::vector<cv::Point2f>{corners[0][0], corners[0][1], corners[0][2], corners[0][3]};
std::cout << "object points: " << object_points << std::endl;
std::cout << "image points: " << image_points << std::endl;
cv::Mat rvec, tvec;
cv::solvePnP(object_points, image_points, intrinMat, distCoeffs, rvec, tvec);
cv::aruco::drawAxis(colorMat, intrinMat, distCoeffs, rvec, tvec, 3);
cv::Mat rotation, transform_matrix;
cv::Mat RotX(3, 3, cv::DataType<double>::type);
cv::setIdentity(RotX);
RotX.at<double>(4) = -1; //cos(180) = -1
RotX.at<double>(8) = -1;
cv::Mat R;
cv::Rodrigues(rvec, R);
std::cout << "rvecs: " << rvec << std::endl;
std::cout << "cv::Rodrigues(rvecs, R);: " << R << std::endl;
R = R.t(); // rotation of inverse
std::cout << "R = R.t() : " << R << std::endl;
cv::Mat rvecConverted;
Rodrigues(R, rvecConverted); //
std::cout << "rvec in world coords:\n" << rvecConverted << std::endl;
rvecConverted = RotX * rvecConverted;
std::cout << "rvec scenekit :\n" << rvecConverted << std::endl;
Rodrigues(rvecConverted, rotation);
std::cout << "-R: " << -R << std::endl;
std::cout << "tvec: " << tvec << std::endl;
cv::Mat tvecConverted = -R * tvec;
std::cout << "tvec in world coords:\n" << tvecConverted << std::endl;
tvecConverted = RotX * tvecConverted;
std::cout << "tvec scenekit :\n" << tvecConverted << std::endl;
SCNVector4 rotationVector = SCNVector4Make(rvecConverted.at<double>(0), rvecConverted.at<double>(1), rvecConverted.at<double>(2), norm(rvecConverted));
SCNVector3 translationVector = SCNVector3Make(tvecConverted.at<double>(0), tvecConverted.at<double>(1), tvecConverted.at<double>(2));
std::cout << "rotation :\n" << rotation << std::endl;
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) = tvecConverted.at<double>(0,0);
transform_matrix.at<double>(1, 3) = tvecConverted.at<double>(1,0);
transform_matrix.at<double>(2, 3) = tvecConverted.at<double>(2,0);
transform_matrix.at<double>(3, 3) = 1;
TransformModel *model = [TransformModel new];
model.rotationVector = rotationVector;
model.translationVector = translationVector;
return model;
}
swift code:
func initSceneKit() {
let scene = SCNScene()
cameraNode = SCNNode()
let camera = SCNCamera()
camera.zFar = 1000
camera.zNear = 0.1
cameraNode.camera = camera
scene.rootNode.addChildNode(cameraNode)
let scnView = sceneView!
scnView.scene = scene
scnView.autoenablesDefaultLighting = true
scnView.backgroundColor = UIColor.clear
let box = SCNBox(width: 10, height: 10 , length: 10, chamferRadius: 0)
boxNode = SCNNode(geometry: box)
boxNode.position = SCNVector3(0,0,0)
scene.rootNode.addChildNode(boxNode)
sceneView.pointOfView = cameraNode
}
func initCamera() {
let device = AVCaptureDevice.default(AVCaptureDevice.DeviceType.builtInWideAngleCamera, for: .video, position: AVCaptureDevice.Position.back)
let deviceInput = try! AVCaptureDeviceInput(device: device!)
self.session = AVCaptureSession()
self.session.sessionPreset = AVCaptureSession.Preset.iFrame960x540
self.session.addInput(deviceInput)
let sessionOutput: AVCaptureVideoDataOutput = AVCaptureVideoDataOutput()
let outputQueue = DispatchQueue(label: "VideoDataOutputQueue", attributes: [])
sessionOutput.setSampleBufferDelegate(self, queue: outputQueue)
self.session.addOutput(sessionOutput)
self.previewLayer = AVCaptureVideoPreviewLayer(session: self.session)
self.previewLayer.backgroundColor = UIColor.black.cgColor
self.previewLayer.videoGravity = AVLayerVideoGravity.resizeAspect
self.previewView.layer.addSublayer(self.previewLayer)
self.session.startRunning()
view.setNeedsLayout()
}
func captureOutput(_ output: AVCaptureOutput, didOutput sampleBuffer: CMSampleBuffer, from connection: AVCaptureConnection) {
let pixelBuffer: CVPixelBuffer = CMSampleBufferGetImageBuffer(sampleBuffer)!
//QR detection
guard let transQR = OpenCVWrapper.arucoTransformMatrix(from: pixelBuffer) else {
return
}
DispatchQueue.main.async(execute: {
self.setCameraMatrix(transQR)
})
}
func setCameraMatrix(_ transformModel: TransformModel) {
cameraNode.rotation = transformModel.rotationVector
cameraNode.position = transformModel.translationVector
// cameraNode.transform = transformModel.transform
}
image of my result
Repo on github with my project: https://github.com/danilovdorin/ArucoAugmentedReality