Revision history [back]

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