void Horus::update(Frame *frame)
{
vector< vector<cv::Point> > contours;
vector<Vec4i> hierarchy;
RNG rng(12345);
if (getFrameType() == DepthCamera::FRAME_XYZI_POINT_CLOUD_FRAME)
{
// Create amplitude and depth Mat
vector<float> zMap, iMap;
XYZIPointCloudFrame *frm = dynamic_cast<XYZIPointCloudFrame *>(frame);
for (int i=0; i< frm->points.size(); i++) {
zMap.push_back(frm->points[i].z);
iMap.push_back(frm->points[i].i);
}
_iMat = Mat(getDim().height, getDim().width, CV_32FC1, iMap.data());
_dMat = Mat(getDim().height, getDim().width, CV_32FC1, zMap.data());
// Apply amplitude gain
_iMat = (float)_ampGain*_iMat;
// Update background as required
if (!_setBackground) {
_dMat.copyTo(_bkgndMat);
_setBackground = true;
cout << endl << "Updated background" << endl;
}
// Find foreground by subtraction
Mat fMat = _bkgndMat-_dMat;
// Convert to binary image based on amplitude and depth thresholds
clipBackground(fMat, _iMat, (float)_depthThresh/100.0, (float)_ampThresh/100.0);
fMat.convertTo(_bMat, CV_8U, 255.0);
// Apply morphological open to clean up image
Mat morphMat = _bMat.clone();
Mat element = getStructuringElement( 0, Size(3,3), cv::Point(1,1) );
morphologyEx(_bMat, morphMat, 2, element);
// Find all contours
findContours(morphMat, contours, hierarchy, CV_RETR_TREE,
CV_CHAIN_APPROX_SIMPLE, cv::Point(0,0));
// Draw contours that meet a "person" requirement
Mat drawing = Mat::zeros(_iMat.size(), CV_8UC3);
cvtColor(_iMat, drawing, CV_GRAY2RGB);
int peopleCount = 0;
for ( int i = 0; i < contours.size(); i++ ) {
if (isPerson(contours[i], _dMat)) {
peopleCount++;
drawContours( drawing, contours, i, Scalar(0, 0, 255), 2, 8, vector<Vec4i>(), 0, cv::Point() );
}
}
putText(drawing, "Cnt="+to_string(peopleCount), cv::Point(40, 30), FONT_HERSHEY_PLAIN, 1, Scalar(255, 0, 0));
imshow("Draw", drawing);
}
}
/**
* initialize the first state of the search
*/
void initState(const vector<string> &ground, State &init)
{
// initialization
init.currentStateNum = 0;
for (int i = 0; i < ground.size(); ++i) {
for (int j = 0; j < ground[i].size(); ++j) {
if (isPerson(ground[i][j])) {
init.person = Position(i, j);
}
if (isBox(ground[i][j])) {
init.box.push_back(Position(i, j));
}
}
}
}
/**
* detecting simple deadlocks
*/
void detectDeadSquare(vector<string> &ground)
{
vector<Position> goal;
getGoalPosition(ground, goal);
vector<string> mark = ground;
for (int i = 0; i < goal.size(); ++i) {
for (int j = 0; j < 4; ++j) {
Position person(goal[i].x + direction[j][0], goal[i].y + direction[j][1]);
pullAndMark(mark, goal[i], person);
}
}
for (int i = 0; i < ground.size(); ++i) {
for (int j = 0; j < ground[i].size(); ++j) {
if (!isWall(ground[i][j]) && !isPerson(ground[i][j]) && !isBox(ground[i][j]) && mark[i][j] != 'V') {
ground[i][j] = 'X';
}
}
}
}
