void FindObjectROS::publish(const find_object::DetectionInfo & info)
{
// send tf before the message
if(info.objDetected_.size() && !depth_.empty() && depthConstant_ != 0.0f)
{
std::vector<tf::StampedTransform> transforms;
QMultiMap<int, QSize>::const_iterator iterSizes=info.objDetectedSizes_.constBegin();
for(QMultiMap<int, QTransform>::const_iterator iter=info.objDetected_.constBegin();
iter!=info.objDetected_.constEnd();
++iter, ++iterSizes)
{
// get data
int id = iter.key();
float objectWidth = iterSizes->width();
float objectHeight = iterSizes->height();
// Find center of the object
QPointF center = iter->map(QPointF(objectWidth/2, objectHeight/2));
QPointF xAxis = iter->map(QPointF(3*objectWidth/4, objectHeight/2));
QPointF yAxis = iter->map(QPointF(objectWidth/2, 3*objectHeight/4));
cv::Vec3f center3D = this->getDepth(depth_,
center.x()+0.5f, center.y()+0.5f,
float(depth_.cols/2)-0.5f, float(depth_.rows/2)-0.5f,
1.0f/depthConstant_, 1.0f/depthConstant_);
cv::Vec3f axisEndX = this->getDepth(depth_,
xAxis.x()+0.5f, xAxis.y()+0.5f,
float(depth_.cols/2)-0.5f, float(depth_.rows/2)-0.5f,
1.0f/depthConstant_, 1.0f/depthConstant_);
cv::Vec3f axisEndY = this->getDepth(depth_,
yAxis.x()+0.5f, yAxis.y()+0.5f,
float(depth_.cols/2)-0.5f, float(depth_.rows/2)-0.5f,
1.0f/depthConstant_, 1.0f/depthConstant_);
if(std::isfinite(center3D.val[0]) && std::isfinite(center3D.val[1]) && std::isfinite(center3D.val[2]) &&
std::isfinite(axisEndX.val[0]) && std::isfinite(axisEndX.val[1]) && std::isfinite(axisEndX.val[2]) &&
std::isfinite(axisEndY.val[0]) && std::isfinite(axisEndY.val[1]) && std::isfinite(axisEndY.val[2]))
{
tf::StampedTransform transform;
transform.setIdentity();
transform.child_frame_id_ = QString("%1_%2").arg(objFramePrefix_.c_str()).arg(id).toStdString();
transform.frame_id_ = frameId_;
transform.stamp_ = stamp_;
transform.setOrigin(tf::Vector3(center3D.val[0], center3D.val[1], center3D.val[2]));
//set rotation (y inverted)
tf::Vector3 xAxis(axisEndX.val[0] - center3D.val[0], axisEndX.val[1] - center3D.val[1], axisEndX.val[2] - center3D.val[2]);
xAxis.normalize();
tf::Vector3 yAxis(axisEndY.val[0] - center3D.val[0], axisEndY.val[1] - center3D.val[1], axisEndY.val[2] - center3D.val[2]);
yAxis.normalize();
tf::Vector3 zAxis = xAxis*yAxis;
tf::Matrix3x3 rotationMatrix(
xAxis.x(), yAxis.x() ,zAxis.x(),
xAxis.y(), yAxis.y(), zAxis.y(),
xAxis.z(), yAxis.z(), zAxis.z());
tf::Quaternion q;
rotationMatrix.getRotation(q);
// set x axis going front of the object, with z up and z left
q *= tf::createQuaternionFromRPY(CV_PI/2.0, CV_PI/2.0, 0);
transform.setRotation(q.normalized());
transforms.push_back(transform);
}
else
{
ROS_WARN("Object %d detected, center 2D at (%f,%f), but invalid depth, cannot set frame \"%s\"! "
"(maybe object is too near of the camera or bad depth image)\n",
id,
center.x(), center.y(),
QString("%1_%2").arg(objFramePrefix_.c_str()).arg(id).toStdString().c_str());
}
}
if(transforms.size())
{
tfBroadcaster_.sendTransform(transforms);
}
}
if(pub_.getNumSubscribers() || pubStamped_.getNumSubscribers())
{
std_msgs::Float32MultiArray msg;
find_object_2d::ObjectsStamped msgStamped;
msg.data = std::vector<float>(info.objDetected_.size()*12);
msgStamped.objects.data = std::vector<float>(info.objDetected_.size()*12);
int i=0;
QMultiMap<int, QSize>::const_iterator iterSizes=info.objDetectedSizes_.constBegin();
for(QMultiMap<int, QTransform>::const_iterator iter=info.objDetected_.constBegin();
iter!=info.objDetected_.constEnd();
++iter, ++iterSizes)
{
msg.data[i] = msgStamped.objects.data[i] = iter.key(); ++i;
msg.data[i] = msgStamped.objects.data[i] = iterSizes->width(); ++i;
msg.data[i] = msgStamped.objects.data[i] = iterSizes->height(); ++i;
msg.data[i] = msgStamped.objects.data[i] = iter->m11(); ++i;
msg.data[i] = msgStamped.objects.data[i] = iter->m12(); ++i;
msg.data[i] = msgStamped.objects.data[i] = iter->m13(); ++i;
msg.data[i] = msgStamped.objects.data[i] = iter->m21(); ++i;
msg.data[i] = msgStamped.objects.data[i] = iter->m22(); ++i;
msg.data[i] = msgStamped.objects.data[i] = iter->m23(); ++i;
msg.data[i] = msgStamped.objects.data[i] = iter->m31(); ++i;// dx
msg.data[i] = msgStamped.objects.data[i] = iter->m32(); ++i;// dy
msg.data[i] = msgStamped.objects.data[i] = iter->m33(); ++i;
}
if(pub_.getNumSubscribers())
{
pub_.publish(msg);
}
if(pubStamped_.getNumSubscribers())
{
// use same header as the input image (for synchronization and frame reference)
msgStamped.header.frame_id = frameId_;
msgStamped.header.stamp = stamp_;
pubStamped_.publish(msgStamped);
}
}
}
