C++ (Cpp) Pose::GetMatrixGL Examples

Example #1

File: DetectAlvar.cpp Project: campbeb6/RedhawkQuad

void videocallback(IplImage *image)
{
    static IplImage *rgba;
    bool flip_image = (image->origin?true:false);
    if (flip_image) {
        cvFlip(image);
        image->origin = !image->origin;
    }

    if (init) {
        init = false;
        cout<<"Loading calibration: "<<calibrationFilename.str();
        if (cam.SetCalib(calibrationFilename.str().c_str(), image->width, image->height)) {
            cout<<" [Ok]"<<endl;
        } else {
            cam.SetRes(image->width, image->height);
            cout<<" [Fail]"<<endl;
        }
        double p[16];
        cam.GetOpenglProjectionMatrix(p,image->width,image->height);
        GlutViewer::SetGlProjectionMatrix(p);
        for (int i=0; i<32; i++) {
            d[i].SetScale(marker_size);
        }
        rgba = CvTestbed::Instance().CreateImageWithProto("RGBA", image, 0, 4);
    }
    static MarkerDetector<MarkerData> marker_detector;
    marker_detector.SetMarkerSize(marker_size); // for marker ids larger than 255, set the content resolution accordingly
    //static MarkerDetector<MarkerArtoolkit> marker_detector;
    //marker_detector.SetMarkerSize(2.8, 3, 1.5);

    // Here we try to use RGBA just to make sure that also it works...
    //cvCvtColor(image, rgba, CV_RGB2RGBA);
    marker_detector.Detect(image, &cam, true, true);
    GlutViewer::DrawableClear();
    for (size_t i=0; i<marker_detector.markers->size(); i++) {
        if (i >= 32) break;
        
        Pose p = (*(marker_detector.markers))[i].pose;
        p.GetMatrixGL(d[i].gl_mat);

        int id = (*(marker_detector.markers))[i].GetId();
        double r = 1.0 - double(id+1)/32.0;
        double g = 1.0 - double(id*3%32+1)/32.0;
        double b = 1.0 - double(id*7%32+1)/32.0;
        d[i].SetColor(r, g, b);

        GlutViewer::DrawableAdd(&(d[i]));
    }

    if (flip_image) {
        cvFlip(image);
        image->origin = !image->origin;
    }
}

Example #2

File: SampleMarkerlessDetector.cpp Project: campbeb6/RedhawkQuad

void videocallback(IplImage *image)
{
    bool flip_image = (image->origin?true:false);
    if (flip_image) {
        cvFlip(image);
        image->origin = !image->origin;
    }

    if (init) {
        init = false;
        cout << "Loading calibration: " << calibrationFilename.str();
        if (fernEstimator.setCalibration(calibrationFilename.str(), image->width, image->height)) {
            cout << " [Ok]" << endl;
        } else {
            fernEstimator.setResolution(image->width, image->height);
            cout << " [Fail]" << endl;
        }
        double p[16];
        fernEstimator.camera().GetOpenglProjectionMatrix(p, image->width, image->height);
        GlutViewer::SetGlProjectionMatrix(p);
        d.SetScale(10);
        gray = cv::Mat(image);
    }

    if (image->nChannels == 3) {
        cv::Mat img = cvarrToMat(image);
        cv::cvtColor(img, gray, CV_RGB2GRAY);
    }
    else {
        gray = image;
    }

    vector<CvPoint2D64f> ipts;
    vector<CvPoint3D64f> mpts;

    fernDetector.findFeatures(gray, true);
    fernDetector.imagePoints(ipts);
    fernDetector.modelPoints(mpts, true);
    double test = fernDetector.inlierRatio();
    if (test > 0.15 && mpts.size() > 4) {
        fernEstimator.calculateFromPointCorrespondences(mpts, ipts);
    }

    GlutViewer::DrawableClear();
    Pose pose = fernEstimator.pose();
    pose.GetMatrixGL(d.gl_mat);
    GlutViewer::DrawableAdd(&d);

    if (flip_image) {
        cvFlip(image);
        image->origin = !image->origin;
    }
}