Problems with Augmented Reality

edit

Hello, I am trying to create a program which uses augmented reality. However, I am currently stuck on drawing the object. This is my code so far:

#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"

using namespace std;
using namespace cv;

Mat camMat;
Mat distortion;

//Initializes augmented reality
void initAR()
{
    FileStorage fs("out_camera_data.xml", FileStorage::READ);

    fs["Camera_Matrix"] >> camMat;
    fs["Distortion_Coefficients"] >> distortion;
}

//Load 3d model
void loadModel(vector<Mat> squares, Mat colorImg)
{
    vector<Point3f> objectPt = { Point3f(-1, -1, 0), Point3f(-1, 1, 0), Point3f(1, 1, 0), Point3f(1, -1, 0) };
    Mat objectMat(objectPt);

    Mat rvec;
    Mat tvec;
    solvePnP(objectMat, squares[0], camMat, distortion, rvec, tvec);

    vector<Point3f> line3d[4];
    line3d[0] = { { 1, 1, 0 }, { 1, 0, 0 }, { 0, 0, 0 } };
    //line3d[1] = { { -1, 0, 0 },{ 1, 0, 1 } };
    //line3d[2] = { { 1, 0, 0 },{ -1, 0, 1 } };
    //line3d[3] = { { 1, 0, 0 },{ 1, 0, 1 } };

    vector<Point2f> line2d[4];
    projectPoints(line3d[0], rvec, tvec, camMat, distortion, line2d[0]);

    polylines(colorImg, line2d[0][0], true, Scalar(255, 0, 0), 2);
}

//Displays 3d object
void doAR(Mat colorImg)
{
    Mat bwImg;
    Mat blurImg;
    Mat threshImg;
    vector<vector<Point> > cnts;

    if (!colorImg.empty())
    {
        cvtColor(colorImg, bwImg, CV_BGR2GRAY);
        blur(bwImg, blurImg, Size(5, 5));
        threshold(blurImg, threshImg, 128.0, 255.0, THRESH_OTSU);
        findContours(threshImg, cnts, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);

        vector<Mat> squares;
        for (auto contour : cnts)
        {
            vector<Point> approx;
            approxPolyDP(contour, approx, arcLength(Mat(contour), true)*0.02, true);
            if (approx.size() == 4 && fabs(contourArea(Mat(approx))) > 1000 && isContourConvex(Mat(approx)))
            {
                Mat square;
                Mat(approx).convertTo(square, CV_32FC3);
                squares.push_back(square);
            }
        }

        if (squares.size() > 0)
        {
            loadModel(squares, colorImg);
        }

        cvNamedWindow("AR", 0);
        cvSetWindowProperty("AR", CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
        imshow("AR", colorImg);
        waitKey(1);
    }
}

I am trying to draw a triangle with the points in line3d. Can someone please tell me why this isn't drawing? If anyone needs to see more of my code, I will post more on my answer.

Thanks in advance.