void image_cb(const sensor_msgs::Image::ConstPtr& image)
{
cv_bridge::CvImageConstPtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvShare(image, enc::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv_bridge::CvImagePtr seg_ptr (new cv_bridge::CvImage);
seg_ptr->header = cv_ptr->header;
seg_ptr->encoding = cv_ptr->encoding;
if(half_)
{
// shrink & expand
cv::Size half_size = cv::Size(cv_ptr->image.cols/2, cv_ptr->image.rows/2);
cv::Mat half_img = cv::Mat(half_size, CV_8UC3);
cv::Mat half_seg = cv::Mat(half_size, CV_8UC3);
cv::resize(cv_ptr->image, half_img, half_size);
segment(half_img, half_seg, rects_);
cv::Size full_size = cv::Size(cv_ptr->image.cols, cv_ptr->image.rows);
cv::resize(half_seg, seg_ptr->image, full_size);
// expand `rects`
for(int i=0; i<rects_.size(); i++)
{
rects_[i].x *= 2;
rects_[i].y *= 2;
rects_[i].w *= 2;
rects_[i].h *= 2;
}
}
else
segment(cv_ptr->image, seg_ptr->image, rects_);
pub_image_.publish(seg_ptr->toImageMsg());
rects_msg_.rects = rects_;
pub_rects_.publish(rects_msg_);
segments_msg_.segimage = *seg_ptr->toImageMsg();
segments_msg_.rects = rects_;
pub_segments_.publish(segments_msg_);
}
inline std::pair<volume_ptr<ID>, ID>
simple_watershed( const affinity_graph_ptr<F>& aff_ptr,
const L& lowv,
const H& highv,
std::vector<std::size_t>& counts )
{
typedef F float_type;
typedef watershed_traits<ID> traits;
float_type low = static_cast<float_type>(lowv);
float_type high = static_cast<float_type>(highv);
std::ptrdiff_t xdim = aff_ptr->shape()[0];
std::ptrdiff_t ydim = aff_ptr->shape()[1];
std::ptrdiff_t zdim = aff_ptr->shape()[2];
std::ptrdiff_t size = xdim * ydim * zdim;
volume_ptr<ID> seg_ptr( new volume<ID>(boost::extents[xdim][ydim][zdim],
boost::fortran_storage_order()));
affinity_graph<F>& aff = *aff_ptr;
volume<ID>& seg = *seg_ptr;
ID* seg_raw = seg_ptr->data();
for ( std::ptrdiff_t z = 0; z < zdim; ++z )
for ( std::ptrdiff_t y = 0; y < ydim; ++y )
for ( std::ptrdiff_t x = 0; x < xdim; ++x )
{
ID& id = seg[x][y][z] = 0;
F negx = (x>0) ? aff[x][y][z][0] : low;
F negy = (y>0) ? aff[x][y][z][1] : low;
F negz = (z>0) ? aff[x][y][z][2] : low;
F posx = (x<(xdim-1)) ? aff[x+1][y][z][0] : low;
F posy = (y<(ydim-1)) ? aff[x][y+1][z][1] : low;
F posz = (z<(zdim-1)) ? aff[x][y][z+1][2] : low;
F m = std::max({negx,negy,negz,posx,posy,posz});
if ( m > low )
{
if ( negx == m || negx >= high ) { id |= 0x01; }
if ( negy == m || negy >= high ) { id |= 0x02; }
if ( negz == m || negz >= high ) { id |= 0x04; }
if ( posx == m || posx >= high ) { id |= 0x08; }
if ( posy == m || posy >= high ) { id |= 0x10; }
if ( posz == m || posz >= high ) { id |= 0x20; }
}
}
const std::ptrdiff_t dir[6] = { -1, -xdim, -xdim*ydim, 1, xdim, xdim*ydim };
const ID dirmask[6] = { 0x01, 0x02, 0x04, 0x08, 0x10, 0x20 };
const ID idirmask[6] = { 0x08, 0x10, 0x20, 0x01, 0x02, 0x04 };
// get plato escapers
std::vector<std::ptrdiff_t> bfs;
for ( std::ptrdiff_t idx = 0; idx < size; ++idx )
{
for ( std::ptrdiff_t d = 0; d < 6; ++d )
{
if ( seg_raw[idx] & dirmask[d] )
{
if ( !(seg_raw[idx+dir[d]] & idirmask[d]) )
{
seg_raw[idx] |= 0x40;
bfs.push_back(idx);
d = 6; // break;
}
}
}
}
std::size_t bfs_index = 0;
while ( bfs_index < bfs.size() )
{
std::ptrdiff_t idx = bfs[bfs_index];
ID to_set = 0;
for ( std::ptrdiff_t d = 0; d < 6; ++d )
{
if ( seg_raw[idx] & dirmask[d] )
{
if ( seg_raw[idx+dir[d]] & idirmask[d] )
{
if ( !( seg_raw[idx+dir[d]] & 0x40 ) )
{
bfs.push_back(idx+dir[d]);
seg_raw[idx+dir[d]] |= 0x40;
}
}
else
{
to_set = dirmask[d];
}
}
}
seg_raw[idx] = to_set;
++bfs_index;
}
bfs.clear();
//std::vector<std::size_t> counts({0});
counts.resize(1);
counts[0] = 0;
ID next_id = static_cast<ID>(1);
for ( std::ptrdiff_t idx = 0; idx < size; ++idx )
{
if ( seg_raw[idx] == 0 )
{
seg_raw[idx] |= traits::high_bit;
++counts[0];
}
if ( !( seg_raw[idx] & traits::high_bit ) && seg_raw[idx] )
{
bfs.push_back(idx);
bfs_index = 0;
seg_raw[idx] |= 0x40;
while ( bfs_index < bfs.size() )
{
std::ptrdiff_t me = bfs[bfs_index];
for ( std::ptrdiff_t d = 0; d < 6; ++d )
{
if ( seg_raw[me] & dirmask[d] )
{
std::ptrdiff_t him = me + dir[d];
if ( seg_raw[him] & traits::high_bit )
{
counts[ seg_raw[him] & ~traits::high_bit ]
+= bfs.size();
for ( auto& it: bfs )
{
seg_raw[it] = seg_raw[him];
}
bfs.clear();
d = 6; // break
}
else if ( !( seg_raw[him] & 0x40 ) )
{
seg_raw[him] |= 0x40;
bfs.push_back( him );
}
}
}
++bfs_index;
}
if ( bfs.size() )
{
counts.push_back( bfs.size() );
for ( auto& it: bfs )
{
seg_raw[it] = traits::high_bit | next_id;
}
++next_id;
bfs.clear();
}
}
}
std::cout << "found: " << (next_id-1) << " components\n";
for ( std::ptrdiff_t idx = 0; idx < size; ++idx )
{
seg_raw[idx] &= traits::mask;
}
return std::make_pair(seg_ptr, next_id-1);
}
