void ProcAbsPos::handleStart(ProjAcq& pAcq, AbsPos2D<float> const& pos) const {
Acq* acq = pAcq.getAcq();
Mat rgb = acq->getMat(BGR);
#ifdef WRITE_IMAGES
imwrite("rgb.png", rgb);
#endif
#ifdef COMP_HSV
Mat im_hsv = acq->getMat(HSV);
#ifdef HSV_TO_HGRAY
for (Mat_<Vec3b>::iterator it = im_hsv.begin<Vec3b>();
it != im_hsv.end<Vec3b>(); it++) {
(*it)[1] = (*it)[0];
(*it)[2] = (*it)[0];
}
#elif defined(HSV_TO_VGRAY)
for (Mat_<Vec3b>::iterator it = im_hsv.begin<Vec3b>();
it != im_hsv.end<Vec3b>(); it++) {
(*it)[0] = (*it)[2];
(*it)[1] = (*it)[2];
}
#endif
#ifdef WRITE_IMAGES
imwrite("hsv.png", im_hsv);
#endif /* WRITE_IMAGES */
#ifdef SHOW_IMAGES
imshow("hsv", im_hsv);
#endif /* SHOW_IMAGES */
#endif /* COMP_HSV */
#ifdef COMP_SIMULATED
// simulate acquisition
Mat _im3 = getSimulatedAt(pAcq, pos);
string _bn("_im3-sa");
#ifdef WRITE_IMAGES
imwrite(_bn + ".png", _im3);
#endif /* WRITE_IMAGES */
#ifdef SHOW_IMAGES
imshow(bn, _im3);
#endif /* SHOW_IMAGES */
#ifdef COMP_HSV
// show simulated acquisition in hsv
Mat im3_hsv = im3.clone();
cvtColor(im3, im3_hsv, COLOR_BGR2HSV);
#ifdef HSV_TO_HGRAY
for (Mat_<Vec3b>::iterator it = im3_hsv.begin<Vec3b>();
it != im3_hsv.end<Vec3b>(); it++) {
(*it)[1] = (*it)[0];
(*it)[2] = (*it)[0];
}
#elif defined(HSV_TO_VGRAY)
for(Mat_<Vec3b>::iterator it = im3_hsv.begin<Vec3b>(); it != im3_hsv.end<Vec3b>(); it++) {
(*it)[0] = (*it)[2];
(*it)[1] = (*it)[2];
}
#endif
#ifdef WRITE_IMAGES
imwrite(bn + "_hsv.png", im3_hsv);
#endif /* WRITE_IMAGES */
#ifdef SHOW_IMAGES
imshow(bn + "_hsv", im3_hsv);
#endif /* SHOW_IMAGES */
Mat diff_hsv = im_hsv - im3_hsv;
#ifdef WRITE_IMAGES
imwrite(bn + "_diff_hsv.png", diff_hsv);
#endif /* WRITE_IMAGES */
#ifdef SHOW_IMAGES
imshow(bn + "_diff_hsv", diff_hsv);
#endif /* SHOW_IMAGES */
#endif /* COMP_HSV */
#ifdef COMP_TESTPOINTS
Mat _im3_tp = getTestPointsAt(pAcq, pos.getTransform().getReverse());
_bn = "_im3_tp-sa";
#ifdef WRITE_IMAGES
imwrite(_bn + ".png", _im3_tp);
#endif /* WRITE_IMAGES */
#ifdef SHOW_IMAGES
imshow(bn, im3_tp);
#endif /* SHOW_IMAGES */
#endif /* COMP_TESTPOINTS */
#endif /* COMP_SIMULATED */
#ifdef COMP_TESTPOINTS
Mat _rgb_tp = rgb.clone();
addTestPointsAtTo(_rgb_tp, pAcq, pos.getTransform().getReverse());
_bn = "_im_tp-sa";
#ifdef WRITE_IMAGES
imwrite(_bn + ".png", _rgb_tp);
#endif /* WRITE_IMAGES */
#ifdef SHOW_IMAGES
imshow(bn, rgb_tp);
#endif /* SHOW_IMAGES */
#endif /* COMP_TESTPOINTS */
}
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;
}