void ProcAbsPos::addTestPointsAtTo(Mat& im, ProjAcq& pAcq, const Transform2D<float>& tr_rob2pg) const {
Transform2D<float> tr_pg2rob = tr_rob2pg.getReverse();
Cam const* cam = pAcq.getAcq()->getCam();
// 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)));
Mat camEdges[4];
for (int i = 0; i < 4; i++) {
camEdges[i] = camCorners[(i + 1) % 4] - camCorners[i];
}
for (TestPoint const& 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;
}
// 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);
// draw testpoint position in camera frame
im.at<Vec3b>(y, x) = Vec3b(255, 255, 255);
}
}
Mat ProcAbsPos::getSimulatedAt(ProjAcq& pAcq, const Transform2D<float>& tr_rob2pg) const {
// simulate acquisition
Mat im3 = pAcq.getAcq()->getMat(BGR).clone();
for (Mat_<Vec3b>::iterator it = im3.begin<Vec3b>(); it != im3.end<Vec3b>(); it++) {
Point2i pos_pg = getInPGIm(tr_rob2pg.transformLinPos(pAcq.cam2plane(it.pos())));
if (pos_pg.y < 0 || pos_pg.y >= pg.size[0] || pos_pg.x < 0 || pos_pg.x >= pg.size[1]) {
(*it) = Vec3b(0, 0, 0);
}
else {
(*it) = pg.at<Vec3b>(pos_pg);
}
}
return im3;
}