void callback( const sensor_msgs::ImageConstPtr& dep, const CameraInfoConstPtr& cam_info)
{
ros::Time begin = ros::Time::now();
// Debug info
std::cerr << "Recieved frame..." << std::endl;
std::cerr << "Cam info: fx:" << cam_info->K[0] << " fy:" << cam_info->K[4] << " cx:" << cam_info->K[2] <<" cy:" << cam_info->K[5] << std::endl;
std::cerr << "Depth image h:" << dep->height << " w:" << dep->width << " e:" << dep->encoding << " " << dep->step << endl;
std::cerr << "=========================================================" << endl;
//get image from message
cv_bridge::CvImagePtr image = cv_bridge::toCvCopy(dep);
cv::Mat depth = image->image;
double min, max;
Mat xxx = Mat(depth.size(), CV_32FC1);
minMaxLoc(depth, &min, &max);
depth.convertTo(xxx, CV_32F, 1.0/(max-min), -min);
Mat depth2;
for (int i = 0; i < depth.rows; ++i)
for (int j = 0; j < depth.cols; ++j)
{
if (depth.at<unsigned short>(i, j) > maxDepth)
depth.at<unsigned short>(i, j) = 0;
}
Regions reg;
if (typeRegions == REGIONS_DEPTH)
{
reg.watershedRegions(depth, cam_info, WatershedType::DepthDiff, 1, 2, 20);
}
if (typeRegions == REGIONS_NORMAL)
{
reg.watershedRegions(depth, cam_info, WatershedType::NormalDiff);
reg.computeStatistics(0.3);
minMaxLoc(reg.m_stddeviation, &min, &max);
reg.m_stddeviation.convertTo(depth, CV_16U, 255.0/(max-min), -min);
image->image = depth;
deviation_image.publish(image->toImageMsg());
}
if (typeRegions == REGIONS_COMBINED)
{
reg.watershedRegions(depth, cam_info, WatershedType::Combined);
reg.computeStatistics(0.3);
minMaxLoc(reg.m_stddeviation, &min, &max);
reg.m_stddeviation.convertTo(depth, CV_16U, 255.0/(max-min), -min);
minMaxLoc(reg.m_regionMatrix, &min, &max);
image->image = depth;
deviation_image.publish(image->toImageMsg());
}
if (typeRegions == REGIONS_PREDICTOR)
{
reg.watershedRegions(depth, cam_info, WatershedType::PredictorDiff);
}
if (typeRegions == REGIONS_TILE)
{
reg.independentTileRegions(depth, cam_info);
reg.computeStatistics(0.3);
minMaxLoc(reg.m_stddeviation, &min, &max);
reg.m_stddeviation.convertTo(depth, CV_16U, 255.0/(max-min), -min);
image->image = depth;
deviation_image.publish(image->toImageMsg());
}
minMaxLoc(reg.m_regionMatrix, &min, &max);
reg.m_regionMatrix.convertTo(depth, CV_16U, 255.0/(max-min), -min);
image->image = depth;
region_image.publish(image->toImageMsg());
ros::Time end = ros::Time::now();
std::cout << "Computation time: " << (end - begin).toNSec()/1000000.0 << "ms" << std::endl;
}
