Augmented reality with Aruco and SceneKit

edit

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 ...