Example #1
0
Mat ProcAbsPos::getPgWithSimulatedAt(ProjAcq& pAcq, const Pos& robPos) const {
    Mat pg_fov = pg.clone();
    Mat im = pAcq.getAcq()->getMat(BGR);

    Transform2D<float> tr_pg2rob = robPos.getTransform();

    for (Mat_<Vec3b>::iterator it = pg_fov.begin<Vec3b>(); it != pg_fov.end<Vec3b>(); it++) {
        Point2f p_rob = tr_pg2rob.transformLinPos(getFromPgIm(it.pos()));
        Mat p_cam = pAcq.plane2cam((Mat_<float>(3, 1) << p_rob.x, p_rob.y));

        Point2i c(int(round(p_cam.at<float>(0))), int(round(p_cam.at<float>(1))));

        if(c.x < 0 || c.y < 0 || c.x >= pAcq.getAcq()->getCam()->getSize().width || c.y >= pAcq.getAcq()->getCam()->getSize().height){
            continue;
        }

        (*it) = im.at<Vec3b>(c);
    }

    return pg_fov;
}
Example #2
0
float ProcAbsPos::getEnergy(ProjAcq& pAcq, const Pos& robPos) {
//    vector<TestPoint> posDependentTP = getPosDependentTP(robPos);

    float E = 0;

    Acq* acq = pAcq.getAcq();
    Cam const* cam = acq->getCam();
    Mat im = acq->getMat(HSV);

    Transform2D<float> tr_pg2rob(robPos.getTransform());
    Transform2D<float> tr_rob2pg = tr_pg2rob.getReverse();

    // get corners and edges of cam fov projected on playground
    Mat camCorners[4];
    camCorners[0] = tr_rob2pg.transformLinPos(pAcq.cam2plane(cam->getTopLeft()));
    camCorners[1] = tr_rob2pg.transformLinPos(pAcq.cam2plane(cam->getTopRight()));
    camCorners[2] = tr_rob2pg.transformLinPos(pAcq.cam2plane(cam->getBottomRight()));
    camCorners[3] = tr_rob2pg.transformLinPos(pAcq.cam2plane(cam->getBottomLeft()));

    float xMin = MIN(MIN(camCorners[0].at<float>(0), camCorners[1].at<float>(0)), MIN(camCorners[2].at<float>(0), camCorners[3].at<float>(0)));
    float yMin = MIN(MIN(camCorners[0].at<float>(1), camCorners[1].at<float>(1)), MIN(camCorners[2].at<float>(1), camCorners[3].at<float>(1)));
    float xMax = MAX(MAX(camCorners[0].at<float>(0), camCorners[1].at<float>(0)), MAX(camCorners[2].at<float>(0), camCorners[3].at<float>(0)));
    float yMax = MAX(MAX(camCorners[0].at<float>(1), camCorners[1].at<float>(1)), MAX(camCorners[2].at<float>(1), camCorners[3].at<float>(1)));

#ifdef COMP_PLAYGROUND
    Point2i camCornersPoints[4];
#endif /* COMP_PLAYGROUND */

    Mat camEdges[4];
    for (int i = 0; i < 4; i++) {
#ifdef COMP_PLAYGROUND
        camCornersPoints[i] = getInPGIm(camCorners[i]);
#endif /* COMP_PLAYGROUND */
        camEdges[i] = camCorners[(i + 1) % 4] - camCorners[i];
    }

#ifdef COMP_PLAYGROUND
    Mat pg_fov = pg.clone();

    // draw projected FOV
    Scalar color_fov(0, 0, 0);
    for (int i = 0; i < 4; i++) {
        line(pg_fov, camCornersPoints[i], camCornersPoints[(i + 1) % 4], color_fov, 4);
    }

    // draw robot position
    Point2i rP = getInPGIm(robPos.getLinPos());
    Point2i rP_x = getInPGIm(robPos.getLinPos() + Point2D<float>(3, 0).rotate(robPos.theta()).toCv());
    line(pg_fov, rP, rP_x, Scalar(0, 0, 255), 4);
    Point2i rP_y = getInPGIm(robPos.getLinPos() + Point2D<float>(0, 3).rotate(robPos.theta()).toCv());
    line(pg_fov, rP, rP_y, Scalar(0, 255, 0), 4);
#endif /* COMP_PLAYGROUND */

    // get Energy...
    int nb = 0;
    for (TestPoint& tp : staticTP) {
        cv::Mat tp_pos = tp.getPos();

        float tp_x = tp_pos.at<float>(0);
        float tp_y = tp_pos.at<float>(1);

        if (tp_x > xMax || tp_x < xMin || tp_y > yMax || tp_y < yMin) {
            continue;
        }

        // check if testpoint seen by camera
        int i;
        for (i = 0; i < 4; i++) {
            Mat vi = tp_pos - camCorners[i];

            double cross = vi.at<float>(0) * camEdges[i].at<float>(1) -
                    vi.at<float>(1) * camEdges[i].at<float>(0);
            if (cross < 0) {
                break;
            }
        }
        if (i < 4) {
            continue;
        }

        nb++;

#ifdef COMP_PLAYGROUND
        // testpoint is in cam field of view, draw it in playground
        pg_fov.at<Vec3b>(getInPGIm(tp_pos)) = Vec3b(255, 255, 255);
#endif /* COMP_PLAYGROUND */

        // get position of testpoint in original camera image
        cv::Mat tp_cmRob = tr_pg2rob.transformLinPos(tp_pos);
        cv::Mat tp_pxCam = pAcq.plane2cam(tp_cmRob);

        int x = int(round(tp_pxCam.at<float>(0)));
        int y = int(round(tp_pxCam.at<float>(1)));

        assert(x >= 0 && x < cam->getSize().width);
        assert(y >= 0 && y < cam->getSize().height);

        // get hue of selected pixel
        float hue = float(im.at<Vec3b>(y, x)[0]) / 255.;
        float sat = float(im.at<Vec3b>(y, x)[1]) / 255.;
        float val = float(im.at<Vec3b>(y, x)[2]) / 255.;

//        assert(hue >= 0. && hue <= 1.);
//        assert(255*hue == im.at<Vec3b>(y, x)[0]);

        // get cost of testpoint given this hue
        E += tp.getCost(hue, sat, val);
    }

//    for(const TestPoint& tp : posDependentTP){
//        Pt tp_cmRob = tr_pg2rob.transformLinPos(tp.getPos());
//        Pt tp_pxCam = pAcq->plane2Cam(tp_cmRob);
//
//        const Scalar& px = im.at<Scalar>(tp_pxCam.x, tp_pxCam.y);
//
//        E += tp.getCost(px(0));
//    }

    E /= nb;

#ifdef COMP_PLAYGROUND
#ifdef SHOW_IMAGES
    imshow("pg", pg_fov);
#endif /* SHOW_IMAGES */
#ifdef WRITE_IMAGES
    imwrite("pg.png", pg_fov);
#endif /* WRITE_IMAGES */
#endif /* COMP_PLAYGROUND */

    return E;
}
Example #3
0
Mat ProcAbsPos::getSimulatedAt(ProjAcq& pAcq, const Pos& robPos) const {
    Transform2D<float> tr_rob2pg = robPos.getTransform().getReverse();

    return getSimulatedAt(pAcq, tr_rob2pg);
}