void BorderEstimator::estimate(
const sensor_msgs::PointCloud2::ConstPtr& msg,
const sensor_msgs::CameraInfo::ConstPtr& info)
{
if (msg->height == 1) {
JSK_NODELET_ERROR("[BorderEstimator::estimate] pointcloud must be organized");
return;
}
pcl::RangeImagePlanar range_image;
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::fromROSMsg(*msg, cloud);
Eigen::Affine3f dummytrans = Eigen::Affine3f::Identity();
float fx = info->P[0];
float cx = info->P[2];
float tx = info->P[3];
float fy = info->P[5];
float cy = info->P[6];
range_image.createFromPointCloudWithFixedSize (cloud,
msg->width,
msg->height,
cx, cy,
fx, fy,
dummytrans);
range_image.setUnseenToMaxRange();
computeBorder(range_image, msg->header);
}
void BorderEstimator::estimate(
const sensor_msgs::PointCloud2::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*msg, *cloud);
pcl::RangeImage range_image;
if (model_type_ == "sphere") {
range_image = pcl::RangeImageSpherical();
}
range_image.createFromPointCloud(
*cloud,
angular_resolution_,
max_angle_width_, max_angle_height_,
Eigen::Affine3f::Identity(),
pcl::RangeImage::CAMERA_FRAME,
noise_level_,
min_range_,
border_size_);
range_image.setUnseenToMaxRange();
computeBorder(range_image, msg->header);
}
void KisShrinkSelectionFilter::process(KisPixelSelectionSP pixelSelection, const QRect& rect)
{
if (m_xRadius <= 0 || m_yRadius <= 0) return;
/*
pretty much the same as fatten_region only different
blame all bugs in this function on [email protected]
*/
/* If edge_lock is true we assume that pixels outside the region
we are passed are identical to the edge pixels.
If edge_lock is false, we assume that pixels outside the region are 0
*/
quint8 **buf; // caches the region's pixels
quint8 **max; // caches the smallest values for each column
qint32 last_max, last_index;
max = new quint8* [rect.width() + 2 * m_xRadius];
buf = new quint8* [m_yRadius + 1];
for (qint32 i = 0; i < m_yRadius + 1; i++) {
buf[i] = new quint8[rect.width()];
}
qint32 buffer_size = (rect.width() + 2 * m_xRadius + 1) * (m_yRadius + 1);
quint8* buffer = new quint8[buffer_size];
if (m_edgeLock)
memset(buffer, 255, buffer_size);
else
memset(buffer, 0, buffer_size);
for (qint32 i = 0; i < rect.width() + 2 * m_xRadius; i++) {
if (i < m_xRadius)
if (m_edgeLock)
max[i] = buffer;
else
max[i] = &buffer[(m_yRadius + 1) * (rect.width() + m_xRadius)];
else if (i < rect.width() + m_xRadius)
max[i] = &buffer[(m_yRadius + 1) * (i - m_xRadius)];
else if (m_edgeLock)
max[i] = &buffer[(m_yRadius + 1) * (rect.width() + m_xRadius - 1)];
else
max[i] = &buffer[(m_yRadius + 1) * (rect.width() + m_xRadius)];
}
if (!m_edgeLock)
for (qint32 j = 0 ; j < m_xRadius + 1; j++) max[0][j] = 0;
// offset the max pointer by m_xRadius so the range of the array is [-m_xRadius] to [region->w + m_xRadius]
max += m_xRadius;
quint8* out = new quint8[rect.width()]; // holds the new scan line we are computing
qint32* circ = new qint32[2 * m_xRadius + 1]; // holds the y coords of the filter's mask
computeBorder(circ, m_xRadius, m_yRadius);
// offset the circ pointer by m_xRadius so the range of the array is [-m_xRadius] to [m_xRadius]
circ += m_xRadius;
for (qint32 i = 0; i < m_yRadius && i < rect.height(); i++) // load top of image
pixelSelection->readBytes(buf[i + 1], rect.x(), rect.y() + i, rect.width(), 1);
if (m_edgeLock)
memcpy(buf[0], buf[1], rect.width());
else
memset(buf[0], 0, rect.width());
for (qint32 x = 0; x < rect.width(); x++) { // set up max for top of image
max[x][0] = buf[0][x];
for (qint32 j = 1; j < m_yRadius + 1; j++)
max[x][j] = MIN(buf[j][x], max[x][j-1]);
}
for (qint32 y = 0; y < rect.height(); y++) {
rotatePointers(buf, m_yRadius + 1);
if (y < rect.height() - m_yRadius)
pixelSelection->readBytes(buf[m_yRadius], rect.x(), rect.y() + y + m_yRadius, rect.width(), 1);
else if (m_edgeLock)
memcpy(buf[m_yRadius], buf[m_yRadius - 1], rect.width());
else
memset(buf[m_yRadius], 0, rect.width());
for (qint32 x = 0 ; x < rect.width(); x++) { // update max array
for (qint32 i = m_yRadius; i > 0; i--) {
max[x][i] = MIN(MIN(max[x][i - 1], buf[i - 1][x]), buf[i][x]);
}
max[x][0] = buf[0][x];
}
last_max = max[0][circ[-1]];
last_index = 0;
for (qint32 x = 0 ; x < rect.width(); x++) { // render scan line
last_index--;
if (last_index >= 0) {
if (last_max == 0)
out[x] = 0;
else {
last_max = 255;
for (qint32 i = m_xRadius; i >= 0; i--)
if (last_max > max[x + i][circ[i]]) {
last_max = max[x + i][circ[i]];
last_index = i;
}
out[x] = last_max;
}
} else {
last_index = m_xRadius;
last_max = max[x + m_xRadius][circ[m_xRadius]];
for (qint32 i = m_xRadius - 1; i >= -m_xRadius; i--)
if (last_max > max[x + i][circ[i]]) {
last_max = max[x + i][circ[i]];
last_index = i;
}
out[x] = last_max;
}
}
pixelSelection->writeBytes(out, rect.x(), rect.y() + y, rect.width(), 1);
}
// undo the offsets to the pointers so we can free the malloced memmory
circ -= m_xRadius;
max -= m_xRadius;
delete[] circ;
delete[] buffer;
delete[] max;
for (qint32 i = 0; i < m_yRadius + 1; i++)
delete buf[i];
delete[] buf;
delete[] out;
}
void KisGrowSelectionFilter::process(KisPixelSelectionSP pixelSelection, const QRect& rect)
{
if (m_xRadius <= 0 || m_yRadius <= 0) return;
/**
* Much code resembles Shrink filter, so please fix bugs
* in both filters
*/
quint8 **buf; // caches the region's pixel data
quint8 **max; // caches the largest values for each column
max = new quint8* [rect.width() + 2 * m_xRadius];
buf = new quint8* [m_yRadius + 1];
for (qint32 i = 0; i < m_yRadius + 1; i++) {
buf[i] = new quint8[rect.width()];
}
quint8* buffer = new quint8[(rect.width() + 2 * m_xRadius) *(m_yRadius + 1)];
for (qint32 i = 0; i < rect.width() + 2 * m_xRadius; i++) {
if (i < m_xRadius)
max[i] = buffer;
else if (i < rect.width() + m_xRadius)
max[i] = &buffer[(m_yRadius + 1) * (i - m_xRadius)];
else
max[i] = &buffer[(m_yRadius + 1) * (rect.width() + m_xRadius - 1)];
for (qint32 j = 0; j < m_xRadius + 1; j++)
max[i][j] = 0;
}
/* offset the max pointer by m_xRadius so the range of the array
is [-m_xRadius] to [region->w + m_xRadius] */
max += m_xRadius;
quint8* out = new quint8[ rect.width()]; // holds the new scan line we are computing
qint32* circ = new qint32[ 2 * m_xRadius + 1 ]; // holds the y coords of the filter's mask
computeBorder(circ, m_xRadius, m_yRadius);
/* offset the circ pointer by m_xRadius so the range of the array
is [-m_xRadius] to [m_xRadius] */
circ += m_xRadius;
memset(buf[0], 0, rect.width());
for (qint32 i = 0; i < m_yRadius && i < rect.height(); i++) { // load top of image
pixelSelection->readBytes(buf[i + 1], rect.x(), rect.y() + i, rect.width(), 1);
}
for (qint32 x = 0; x < rect.width() ; x++) { // set up max for top of image
max[x][0] = 0; // buf[0][x] is always 0
max[x][1] = buf[1][x]; // MAX (buf[1][x], max[x][0]) always = buf[1][x]
for (qint32 j = 2; j < m_yRadius + 1; j++) {
max[x][j] = MAX(buf[j][x], max[x][j-1]);
}
}
for (qint32 y = 0; y < rect.height(); y++) {
rotatePointers(buf, m_yRadius + 1);
if (y < rect.height() - (m_yRadius))
pixelSelection->readBytes(buf[m_yRadius], rect.x(), rect.y() + y + m_yRadius, rect.width(), 1);
else
memset(buf[m_yRadius], 0, rect.width());
for (qint32 x = 0; x < rect.width(); x++) { /* update max array */
for (qint32 i = m_yRadius; i > 0; i--) {
max[x][i] = MAX(MAX(max[x][i - 1], buf[i - 1][x]), buf[i][x]);
}
max[x][0] = buf[0][x];
}
qint32 last_max = max[0][circ[-1]];
qint32 last_index = 1;
for (qint32 x = 0; x < rect.width(); x++) { /* render scan line */
last_index--;
if (last_index >= 0) {
if (last_max == 255)
out[x] = 255;
else {
last_max = 0;
for (qint32 i = m_xRadius; i >= 0; i--)
if (last_max < max[x + i][circ[i]]) {
last_max = max[x + i][circ[i]];
last_index = i;
}
out[x] = last_max;
}
} else {
last_index = m_xRadius;
last_max = max[x + m_xRadius][circ[m_xRadius]];
for (qint32 i = m_xRadius - 1; i >= -m_xRadius; i--)
if (last_max < max[x + i][circ[i]]) {
last_max = max[x + i][circ[i]];
last_index = i;
}
out[x] = last_max;
}
}
pixelSelection->writeBytes(out, rect.x(), rect.y() + y, rect.width(), 1);
}
/* undo the offsets to the pointers so we can free the malloced memmory */
circ -= m_xRadius;
max -= m_xRadius;
delete[] circ;
delete[] buffer;
delete[] max;
for (qint32 i = 0; i < m_yRadius + 1; i++)
delete[] buf[i];
delete[] buf;
delete[] out;
}