diff --git a/launch/find_object_2d.launch b/launch/find_object_2d.launch
index 3d7b815b..649a821e 100644
--- a/launch/find_object_2d.launch
+++ b/launch/find_object_2d.launch
@@ -1,9 +1,14 @@
+
+
+
+
+
-
-
-
-
+
+
+
+
diff --git a/launch/find_object_2d_gui.launch b/launch/find_object_2d_gui.launch
deleted file mode 100644
index 49c3c4de..00000000
--- a/launch/find_object_2d_gui.launch
+++ /dev/null
@@ -1,9 +0,0 @@
-
-
-
-
-
-
-
-
-
diff --git a/launch/find_object_3d.launch b/launch/find_object_3d.launch
index d95da108..eb544568 100644
--- a/launch/find_object_3d.launch
+++ b/launch/find_object_3d.launch
@@ -2,29 +2,34 @@
+
+
+
+
+
+
+
+
+
+
+
+
-
-
+
+
-
-
-
+
+
+
+
-
-
-
+
+
+
-
-
-
+
+
-
-
-
-
diff --git a/launch/find_object_3d_kinect2.launch b/launch/find_object_3d_kinect2.launch
index 2cfac046..56289880 100644
--- a/launch/find_object_3d_kinect2.launch
+++ b/launch/find_object_3d_kinect2.launch
@@ -3,15 +3,20 @@
-
+
+
+
+
+
-
-
+
+
-
-
-
+
+
+
+
diff --git a/launch/find_object_3d_zed.launch b/launch/find_object_3d_zed.launch
index 3911acf7..676b8992 100644
--- a/launch/find_object_3d_zed.launch
+++ b/launch/find_object_3d_zed.launch
@@ -2,12 +2,18 @@
+
+
+
+
+
-
-
+
+
-
-
+
+
+
diff --git a/src/ros/FindObjectROS.cpp b/src/ros/FindObjectROS.cpp
index 346fa212..84e4473b 100644
--- a/src/ros/FindObjectROS.cpp
+++ b/src/ros/FindObjectROS.cpp
@@ -37,11 +37,14 @@ using namespace find_object;
FindObjectROS::FindObjectROS(QObject * parent) :
FindObject(true, parent),
- objFramePrefix_("object")
+ objFramePrefix_("object"),
+ usePnP_(true)
{
ros::NodeHandle pnh("~"); // public
pnh.param("object_prefix", objFramePrefix_, objFramePrefix_);
+ pnh.param("pnp", usePnP_, usePnP_);
ROS_INFO("object_prefix = %s", objFramePrefix_.c_str());
+ ROS_INFO("pnp = %s", usePnP_?"true":"false");
ros::NodeHandle nh; // public
@@ -83,55 +86,101 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info, const QStri
// 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 axisEndX;
+ cv::Vec3f axisEndY;
+ if(!usePnP_)
+ {
+ QPointF xAxis = iter->map(QPointF(3*objectWidth/4, objectHeight/2));
+ 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);
+ QPointF yAxis = iter->map(QPointF(objectWidth/2, 3*objectHeight/4));
+ 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]))
+ if((std::isfinite(center3D.val[2]) && usePnP_) ||
+ (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%3").arg(objFramePrefix_.c_str()).arg(id).arg(multiSuffix).toStdString();
transform.frame_id_ = frameId.toStdString();
transform.stamp_.fromSec(stamp);
- transform.setOrigin(tf::Vector3(center3D.val[0], center3D.val[1], center3D.val[2]));
- //set rotation
- 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.cross(yAxis);
- zAxis.normalize();
- 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);
+ if(usePnP_)
+ {
+ std::vector objectPoints(4);
+ std::vector imagePoints(4);
+
+ objectPoints[0] = cv::Point3f(-0.5, -(objectHeight/objectWidth)/2.0f,0);
+ objectPoints[1] = cv::Point3f(0.5,-(objectHeight/objectWidth)/2.0f,0);
+ objectPoints[2] = cv::Point3f(0.5,(objectHeight/objectWidth)/2.0f,0);
+ objectPoints[3] = cv::Point3f(-0.5,(objectHeight/objectWidth)/2.0f,0);
+
+ QPointF pt = iter->map(QPointF(0, 0));
+ imagePoints[0] = cv::Point2f(pt.x(), pt.y());
+ pt = iter->map(QPointF(objectWidth, 0));
+ imagePoints[1] = cv::Point2f(pt.x(), pt.y());
+ pt = iter->map(QPointF(objectWidth, objectHeight));
+ imagePoints[2] = cv::Point2f(pt.x(), pt.y());
+ pt = iter->map(QPointF(0, objectHeight));
+ imagePoints[3] = cv::Point2f(pt.x(), pt.y());
+
+ cv::Mat cameraMatrix = cv::Mat::eye(3,3,CV_64FC1);
+ cameraMatrix.at(0,0) = 1.0f/depthConstant;
+ cameraMatrix.at(1,1) = 1.0f/depthConstant;
+ cameraMatrix.at(0,2) = float(depth.cols/2)-0.5f;
+ cameraMatrix.at(1,2) = float(depth.rows/2)-0.5f;
+ cv::Mat distCoeffs;
+
+ cv::Mat rvec(1,3, CV_64FC1);
+ cv::Mat tvec(1,3, CV_64FC1);
+ cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
+
+ cv::Mat R;
+ cv::Rodrigues(rvec, R);
+ tf::Matrix3x3 rotationMatrix(
+ R.at(0,0), R.at(0,1), R.at(0,2),
+ R.at(1,0), R.at(1,1), R.at(1,2),
+ R.at(2,0), R.at(2,1), R.at(2,2));
+ rotationMatrix.getRotation(q);
+ transform.setOrigin(tf::Vector3(
+ tvec.at(0)*(center3D.val[2]/tvec.at(2)),
+ tvec.at(1)*(center3D.val[2]/tvec.at(2)),
+ tvec.at(2)*(center3D.val[2]/tvec.at(2))));
+ }
+ else
+ {
+ 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.cross(yAxis);
+ zAxis.normalize();
+ tf::Matrix3x3 rotationMatrix(
+ xAxis.x(), yAxis.x() ,zAxis.x(),
+ xAxis.y(), yAxis.y(), zAxis.y(),
+ xAxis.z(), yAxis.z(), zAxis.z());
+ rotationMatrix.getRotation(q);
+ transform.setOrigin(tf::Vector3(center3D.val[0], center3D.val[1], center3D.val[2]));
+ }
+
// 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());
-
- ROS_WARN("%f %f %f (%f %f %f %f)", center3D.val[0], center3D.val[1], center3D.val[2],
- transform.getRotation().x(), transform.getRotation().y(), transform.getRotation().z(), transform.getRotation().w());
-
transforms.push_back(transform);
}
else
diff --git a/src/ros/FindObjectROS.h b/src/ros/FindObjectROS.h
index 2d0c5c63..4ec785dd 100644
--- a/src/ros/FindObjectROS.h
+++ b/src/ros/FindObjectROS.h
@@ -63,6 +63,7 @@ private:
ros::Publisher pubInfo_;
std::string objFramePrefix_;
+ bool usePnP_;
tf::TransformBroadcaster tfBroadcaster_;
};
diff --git a/src/ros/tf_example_node.cpp b/src/ros/tf_example_node.cpp
index 73097a4c..c09b414f 100644
--- a/src/ros/tf_example_node.cpp
+++ b/src/ros/tf_example_node.cpp
@@ -34,11 +34,10 @@ class TfExample
{
public:
TfExample() :
- mapFrameId_("/map"),
objFramePrefix_("object")
{
ros::NodeHandle pnh("~");
- pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
+ pnh.param("target_frame_id", targetFrameId_, targetFrameId_);
pnh.param("object_prefix", objFramePrefix_, objFramePrefix_);
ros::NodeHandle nh;
@@ -51,6 +50,11 @@ public:
{
if(msg->objects.data.size())
{
+ std::string targetFrameId = targetFrameId_;
+ if(targetFrameId.empty())
+ {
+ targetFrameId = msg->header.frame_id;
+ }
char multiSubId = 'b';
int previousId = -1;
for(unsigned int i=0; iobjects.data.size(); i+=12)
@@ -73,13 +77,11 @@ public:
std::string objectFrameId = QString("%1_%2%3").arg(objFramePrefix_.c_str()).arg(id).arg(multiSuffix).toStdString();
tf::StampedTransform pose;
- tf::StampedTransform poseCam;
try
{
- // Get transformation from "object_#" frame to target frame "map"
+ // Get transformation from "object_#" frame to target frame
// The timestamp matches the one sent over TF
- tfListener_.lookupTransform(mapFrameId_, objectFrameId, msg->header.stamp, pose);
- tfListener_.lookupTransform(msg->header.frame_id, objectFrameId, msg->header.stamp, poseCam);
+ tfListener_.lookupTransform(targetFrameId, objectFrameId, msg->header.stamp, pose);
}
catch(tf::TransformException & ex)
{
@@ -87,21 +89,17 @@ public:
continue;
}
- // Here "pose" is the position of the object "id" in "/map" frame.
+ // Here "pose" is the position of the object "id" in target frame.
ROS_INFO("%s [x,y,z] [x,y,z,w] in \"%s\" frame: [%f,%f,%f] [%f,%f,%f,%f]",
- objectFrameId.c_str(), mapFrameId_.c_str(),
+ objectFrameId.c_str(), targetFrameId.c_str(),
pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z(),
pose.getRotation().x(), pose.getRotation().y(), pose.getRotation().z(), pose.getRotation().w());
- ROS_INFO("%s [x,y,z] [x,y,z,w] in \"%s\" frame: [%f,%f,%f] [%f,%f,%f,%f]",
- objectFrameId.c_str(), msg->header.frame_id.c_str(),
- poseCam.getOrigin().x(), poseCam.getOrigin().y(), poseCam.getOrigin().z(),
- poseCam.getRotation().x(), poseCam.getRotation().y(), poseCam.getRotation().z(), poseCam.getRotation().w());
}
}
}
private:
- std::string mapFrameId_;
+ std::string targetFrameId_;
std::string objFramePrefix_;
ros::Subscriber subs_;
tf::TransformListener tfListener_;