cv::Mat NFringeStructuredLight::WrapPhase( vector<cv::Mat> fringeImages, cv::Ptr<cv::FilterEngine> filter )
{
Utils::AssertOrThrowIfFalse(fringeImages.size() == m_numberOfFringes,
"Invalid number of fringes passed into phase wrapper");
// Should be the same size as our fringe images
// and floating point precision for decimal phase values
cv::Mat sine(fringeImages[0].size(), CV_32F, 0.0f);
cv::Mat cosine(fringeImages[0].size(), CV_32F, 0.0f);
cv::Mat phase(fringeImages[0].size(), CV_32F, 0.0f);
for(int row = 0; row < phase.rows; ++row)
{
for(int col = 0; col < phase.cols; ++col)
{
for(int fringe = 0; fringe < m_numberOfFringes; ++fringe)
{
sine.at<float>(row, col) += ( float( fringeImages[fringe].at<uchar>(row, col) ) / 255.0 ) * sin(2.0 * M_PI * float(fringe) / float(m_numberOfFringes));
cosine.at<float>(row, col) += ( float( fringeImages[fringe].at<uchar>(row, col) ) / 255.0 ) * cos(2.0 * M_PI * float(fringe) / float(m_numberOfFringes));
}
}
}
// Filter out noise in the sine and cosine components
if( !filter.empty( ) )
{
filter->apply( sine, sine );
filter->apply( cosine, cosine );
}
// Now perform phase wrapping
for(int row = 0; row < phase.rows; ++row)
{
for(int col = 0; col < phase.cols; ++col)
{
// This is negative so that are phase gradient increases from 0 -> rows or 0 -> cols
phase.at<float>(row, col) = -atan2( sine.at<float>( row, col ), cosine.at<float>( row, col ) );
}
}
return phase;
}
void BackgroundSubtraction::imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImageConstPtr cv_ptr;
try
{
// TBD why converting to BGR8
cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::RGB8);
//, "mono8"); // sensor_msgs::image_encodings::MONO8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::Mat live_frame = cv_ptr->image.clone();
mog2_->apply(live_frame, fg_mask_mog2_);
if (config_.capture_background)
{
bg_image_ = live_frame;
}
// else
// if (config_.subtract_background)
// if (!bg_image_.isEmpty())
{
cv::Mat image;
#if 0
cv::cvtColor(fg_mask_mog2_, fg_mask_mog2_rgb_, CV_GRAY2RGB);
if (fg_mask_mog2_rgb_.size() == live_frame.size())
image = live_frame & fg_mask_mog2_rgb_;
else
image = live_frame;
#endif
cv::Mat diff;
cv::absdiff(bg_image_, live_frame, diff);
cv::Mat diff_gray;
cv::cvtColor(diff, diff_gray, CV_RGB2GRAY);
cv::Mat mask = diff_gray > 20; // TODO(lucasw) make this a config_ dr param
cv::Mat mask_rgb;
cv::cvtColor(mask, mask_rgb, CV_GRAY2RGB);
image = mask_rgb & live_frame;
cv_bridge::CvImage cv_image;
cv_image.image = image;
cv_image.encoding = "rgb8";
cv_image.header.stamp = msg->header.stamp;
cv_image.header.frame_id = msg->header.frame_id;
image_pub_.publish(cv_image.toImageMsg());
}
}
void convertIr(const cv::Mat &ir, cv::Mat &grey)
{
const float factor = 255.0f / (maxIr - minIr);
grey.create(ir.rows, ir.cols, CV_8U);
#pragma omp parallel for
for(size_t r = 0; r < (size_t)ir.rows; ++r)
{
const uint16_t *itI = ir.ptr<uint16_t>(r);
uint8_t *itO = grey.ptr<uint8_t>(r);
for(size_t c = 0; c < (size_t)ir.cols; ++c, ++itI, ++itO)
{
*itO = std::min(std::max(*itI - minIr, 0) * factor, 255.0f);
}
}
clahe->apply(grey, grey);
}
void processImage(cv::Mat& image) {
if (image.empty())
return;
#ifdef _OPENCV3
pMOG->apply(image, fgMaskMOG, 0.05);
#else
pMOG->operator()(image, fgMaskMOG, 0.05);
#endif
cv::dilate(fgMaskMOG,fgMaskMOG,cv::getStructuringElement(cv::MORPH_ELLIPSE,cv::Size(15,15)));
bin = new IplImage(fgMaskMOG);
frame = new IplImage(image);
labelImg = cvCreateImage(cvSize(image.cols,image.rows),IPL_DEPTH_LABEL,1);
unsigned int result = cvLabel(bin, labelImg, blobs);
cvRenderBlobs(labelImg, blobs, frame, frame, CV_BLOB_RENDER_BOUNDING_BOX|CV_BLOB_RENDER_CENTROID|CV_BLOB_RENDER_ANGLE);
cvFilterByArea(blobs, 1500, 40000);
cvUpdateTracks(blobs, tracks, 200., 5);
cvRenderTracks(tracks, frame, frame, CV_TRACK_RENDER_ID);
for (std::map<CvID, CvTrack*>::iterator track_it = tracks.begin(); track_it!=tracks.end(); track_it++) {
CvID id = (*track_it).first;
CvTrack* track = (*track_it).second;
cur_pos = track->centroid;
if (track->inactive == 0) {
if (last_poses.count(id)) {
std::map<CvID, CvPoint2D64f>::iterator pose_it = last_poses.find(id);
last_pos = pose_it -> second;
last_poses.erase(pose_it);
}
last_poses.insert(std::pair<CvID, CvPoint2D64f>(id, cur_pos));
if (line_pos+25>cur_pos.y && cur_pos.y>line_pos && line_pos-25<last_pos.y && last_pos.y<line_pos) {
count++;
countUD++;
}
if (line_pos-25<cur_pos.y && cur_pos.y<line_pos && line_pos+25>last_pos.y && last_pos.y>line_pos) {
count++;
countDU++;
}
if ( cur_pos.y<line_pos+50 && cur_pos.y>line_pos-50) {
avg_vel += abs(cur_pos.y-last_pos.y);
count_active++;
}
//update heatmapfg
heat_mapfg = cv::Mat::zeros(FR_H, FR_W, CV_8UC3);
count_arr[lmindex] = count;
avg_vel_arr[lmindex] = avg_vel/count_active ;
for (int i=0; i<landmarks.size(); i++) {
cv::circle(heat_mapfg, cv::Point((landmarks[i].y + 50)*2.4, (landmarks[i].x + 50)*2.4), count_arr[i]*3, cv::Scalar(0, 16*avg_vel_arr[i], 255 - 16*avg_vel_arr[i]), -1);
}
cv::GaussianBlur(heat_mapfg, heat_mapfg, cv::Size(15, 15), 5);
} else {
if (last_poses.count(id)) {
last_poses.erase(last_poses.find(id));
}
}
}
cv::line(image, cv::Point(0, line_pos), cv::Point(FR_W, line_pos), cv::Scalar(0,255,0),2);
cv::putText(image, "COUNT: "+to_string(count), cv::Point(10, 15), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(255,255,255));
cv::putText(image, "UP->DOWN: "+to_string(countUD), cv::Point(10, 30), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(255,255,255));
cv::putText(image, "DOWN->UP: "+to_string(countDU), cv::Point(10, 45), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(255,255,255));
cv::imshow("BLOBS", image);
cv::imshow("HEATMAP", heat_map + heat_mapfg);
cv::waitKey(33);
}
void backgroundSubstractionDetection(cv::Mat sequence, std::vector<cv::Rect> &detectedPedestrianFiltered, cv::Ptr<cv::BackgroundSubtractor> &pMOG2, trackingOption &tracking)
{
int threshold = 150;
cv::Mat mask;
cv::Mat sequenceGrayDiff;
std::vector<std::vector<cv::Point> > contours;
std::vector<cv::Vec4i> hierarchy;
std::vector<std::vector<cv::Point> > contours_poly;
std::vector<cv::Rect> detectedPedestrian;
pMOG2->apply(sequence,sequenceGrayDiff);
cv::threshold(sequenceGrayDiff, mask, threshold, 255, cv::THRESH_BINARY);
cv::erode(mask, mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(6,6)));
cv::dilate(mask, mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(25,55)));
cv::erode(mask, mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3,6)));
/*
cv::Mat dist;
cv::distanceTransform(mask, dist, CV_DIST_L2, 3);
cv::normalize(dist, dist, 0, 1., cv::NORM_MINMAX);
cv::threshold(dist, dist, .4, 1., CV_THRESH_BINARY);
cv::imshow("temp", dist);
*/
cv::findContours(mask, contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE, cv::Point(0,0));
contours_poly.resize(contours.size());
detectedPedestrian.resize(contours.size());
for(size_t j=0;j<contours.size();j++)
{
cv::approxPolyDP(cv::Mat(contours[j]), contours_poly[j], 3, true);
detectedPedestrian[j] = cv::boundingRect(cv::Mat(contours_poly[j]));
//test
/*
double pix = 30;
if(detectedPedestrian[j].x >= pix)
detectedPedestrian[j].x -= pix;
else
detectedPedestrian[j].x = 0;
if((detectedPedestrian[j].x+detectedPedestrian[j].width) <= (sequence.cols-pix))
detectedPedestrian[j].width += pix;
else
detectedPedestrian[j].width = sequence.cols - detectedPedestrian[j].x;
if(detectedPedestrian[j].y >= pix)
detectedPedestrian[j].y -= pix;
else
detectedPedestrian[j].y = 0;
if((detectedPedestrian[j].y+detectedPedestrian[j].height) <= (sequence.rows-pix))
detectedPedestrian[j].height += pix;
else
detectedPedestrian[j].height = sequence.rows - detectedPedestrian[j].y;
*/
}
if(detectedPedestrian.size() != 0)
{
tracking = GOOD_FEATURES_TO_TRACK;
detectedPedestrianFiltered.clear();
detectedPedestrianFiltered.resize(detectedPedestrian.size());
detectedPedestrianFiltered = detectedPedestrian;
}
else
tracking = NOTHING_TO_TRACK;
}