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; }