apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added PnP approach (now default) to compute 3D transform (requiring only single valid depth point instead of 3). Added arguments to launch files for convenience. Removed fake map/odom frames from find_object_3d.launch (so that it can be more easily included in other launch files).

Browse Source
This commit is contained in:
matlabbe 2019-08-10 15:31:12 -04:00
parent f43bc943f8
commit bde868d1cc
8 changed files with 146 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
launch/find_object_2d.launch
View File

@ -1,9 +1,14 @@
<launch> <launch>
<arg name="gui" default="true"/>
<arg name="image_topic" default="image"/>
<arg name="objects_path" default="~/objects"/>
<arg name="settings_path" default="~/.ros/find_object_2d.ini"/>
<!-- Nodes --> <!-- Nodes -->
<node name="find_object_2d" pkg="find_object_2d" type="find_object_2d" output="screen"> <node name="find_object_2d" pkg="find_object_2d" type="find_object_2d" output="screen">
<remap from="image" to="image"/> <remap from="image" to="$(arg image_topic)"/>
<param name="gui" value="false" type="bool"/> <param name="gui" value="$(arg gui)" type="bool"/>
<param name="objects_path" value="~/objects" type="str"/> <param name="objects_path" value="$(arg objects_path)" type="str"/>
<param name="settings_path" value="~/.ros/find_object_2d.ini" type="str"/> <param name="settings_path" value="$(arg settings_path)" type="str"/>
</node> </node>
</launch> </launch>

9
launch/find_object_2d_gui.launch
View File

@ -1,9 +0,0 @@
<launch>
<!-- Nodes -->
<node name="find_object_2d" pkg="find_object_2d" type="find_object_2d" output="screen">
<remap from="image" to="image"/>
<param name="gui" value="true" type="bool"/>
<param name="objects_path" value="~/objects" type="str"/>
<param name="settings_path" value="~/.ros/find_object_2d.ini" type="str"/>
</node>
</launch>

45
launch/find_object_3d.launch
View File

@ -2,29 +2,34 @@
<!-- Example finding 3D poses of the objects detected --> <!-- Example finding 3D poses of the objects detected -->
<!-- $roslaunch openni_launch openni.launch depth_registration:=true --> <!-- $roslaunch openni_launch openni.launch depth_registration:=true -->
<node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen"> <arg name="object_prefix" default="object"/>
<param name="gui" value="true" type="bool"/> <arg name="objects_path" default=""/>
<param name="settings_path" value="~/.ros/find_object_2d.ini" type="str"/> <arg name="gui" default="true"/>
<param name="subscribe_depth" value="true" type="bool"/> <arg name="approx_sync" default="true"/>
<param name="objects_path" value="" type="str"/> <arg name="pnp" default="true"/>
<param name="object_prefix" value="object" type="str"/> <arg name="tf_example" default="true"/>
<param name="approx_sync" value="true" type="bool"/> <arg name="settings_path" default="~/.ros/find_object_2d.ini"/>
<remap from="rgb/image_rect_color" to="camera/rgb/image_rect_color"/> <arg name="rgb_topic" default="camera/rgb/image_rect_color"/>
<remap from="depth_registered/image_raw" to="camera/depth_registered/image_raw"/> <arg name="depth_topic" default="camera/depth_registered/image_raw"/>
<remap from="depth_registered/camera_info" to="camera/depth_registered/camera_info"/> <arg name="camera_info_topic" default="camera/rgb/camera_info"/>
<node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen">
<param name="gui" value="$(arg gui)" type="bool"/>
<param name="settings_path" value="$(arg settings_path)" type="str"/>
<param name="subscribe_depth" value="true" type="bool"/>
<param name="objects_path" value="$(arg objects_path)" type="str"/>
<param name="object_prefix" value="$(arg object_prefix)" type="str"/>
<param name="approx_sync" value="$(arg approx_sync)" type="bool"/>
<param name="pnp" value="$(arg pnp)" type="bool"/>
<remap from="rgb/image_rect_color" to="$(arg rgb_topic)"/>
<remap from="depth_registered/image_raw" to="$(arg depth_topic)"/>
<remap from="depth_registered/camera_info" to="$(arg camera_info_topic)"/>
</node> </node>
<!-- Example of tf synchronisation with the objectsStamped message --> <!-- Example of tf synchronisation with the objectsStamped message -->
<node name="tf_example" pkg="find_object_2d" type="tf_example" output="screen"> <node if="$(arg tf_example)" name="tf_example" pkg="find_object_2d" type="tf_example" output="screen">
<param name="map_frame_id" value="/map" type="string"/> <param name="object_prefix" value="$(arg object_prefix)" type="str"/>
<param name="object_prefix" value="object" type="str"/>
</node> </node>
<!-- fake some tf frames for the example /map -> /odom -> /base_link -> /camera_link -->
<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf"
args="0.1 0.0 0.3 0.0 0.0 0.0 /base_link /camera_link 100" />
<node pkg="tf" type="static_transform_publisher" name="odom_to_base_tf"
args="1.0 0.0 0.1 1.5707 0.0 0.0 /odom /base_link 100" />
<node pkg="tf" type="static_transform_publisher" name="map_to_odom_tf"
args="0.0 0.5 0.0 0.7853 0.0 0.0 /map /odom 100" />
</launch> </launch>

15
launch/find_object_3d_kinect2.launch
View File

@ -3,14 +3,19 @@
<!-- $ roslaunch kinect2_bridge kinect2_bridge.launch publish_tf:=true --> <!-- $ roslaunch kinect2_bridge kinect2_bridge.launch publish_tf:=true -->
<!-- Which image resolution: sd, qhd, hd --> <!-- Which image resolution: sd, qhd, hd -->
<arg name="resolution" default="qhd" /> <arg name="resolution" default="qhd" />
<arg name="object_prefix" default="object"/>
<arg name="objects_path" default=""/>
<arg name="gui" default="true"/>
<arg name="settings_path" default="~/.ros/find_object_2d.ini"/>
<node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen"> <node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen">
<param name="gui" value="true" type="bool"/> <param name="gui" value="$(arg gui)" type="bool"/>
<param name="settings_path" value="~/.ros/find_object_2d.ini" type="str"/> <param name="settings_path" value="$(arg settings_path)" type="str"/>
<param name="subscribe_depth" value="true" type="bool"/> <param name="subscribe_depth" value="true" type="bool"/>
<param name="objects_path" value="" type="str"/> <param name="objects_path" value="$(arg objects_path)" type="str"/>
<param name="object_prefix" value="object" type="str"/> <param name="object_prefix" value="$(arg object_prefix)" type="str"/>
<param name="approx_sync" value="false" type="bool"/>
<remap from="rgb/image_rect_color" to="/kinect2/$(arg resolution)/image_color_rect"/> <remap from="rgb/image_rect_color" to="/kinect2/$(arg resolution)/image_color_rect"/>
<remap from="depth_registered/image_raw" to="/kinect2/$(arg resolution)/image_depth_rect"/> <remap from="depth_registered/image_raw" to="/kinect2/$(arg resolution)/image_depth_rect"/>

14
launch/find_object_3d_zed.launch
View File

@ -2,12 +2,18 @@
<!-- Example finding 3D poses of the objects detected --> <!-- Example finding 3D poses of the objects detected -->
<!-- $ roslaunch zed_wrapper zed.launch --> <!-- $ roslaunch zed_wrapper zed.launch -->
<arg name="object_prefix" default="object"/>
<arg name="objects_path" default=""/>
<arg name="gui" default="true"/>
<arg name="settings_path" default="~/.ros/find_object_2d.ini"/>
<node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen"> <node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen">
<param name="gui" value="true" type="bool"/> <param name="gui" value="$(arg gui)" type="bool"/>
<param name="settings_path" value="~/.ros/find_object_2d.ini" type="str"/> <param name="settings_path" value="$(arg settings_path)" type="str"/>
<param name="subscribe_depth" value="true" type="bool"/> <param name="subscribe_depth" value="true" type="bool"/>
<param name="objects_path" value="" type="str"/> <param name="objects_path" value="$(arg objects_path)" type="str"/>
<param name="object_prefix" value="object" type="str"/> <param name="object_prefix" value="$(arg object_prefix)" type="str"/>
<param name="approx_sync" value="false" type="bool"/>
<remap from="rgb/image_rect_color" to="/zed/rgb/image_rect_color"/> <remap from="rgb/image_rect_color" to="/zed/rgb/image_rect_color"/>
<remap from="depth_registered/image_raw" to="/zed/depth/depth_registered"/> <remap from="depth_registered/image_raw" to="/zed/depth/depth_registered"/>

113
src/ros/FindObjectROS.cpp
View File

@ -37,11 +37,14 @@ using namespace find_object;
FindObjectROS::FindObjectROS(QObject * parent) : FindObjectROS::FindObjectROS(QObject * parent) :
FindObject(true, parent), FindObject(true, parent),
objFramePrefix_("object") objFramePrefix_("object"),
usePnP_(true)
{ {
ros::NodeHandle pnh("~"); // public ros::NodeHandle pnh("~"); // public
pnh.param("object_prefix", objFramePrefix_, objFramePrefix_); pnh.param("object_prefix", objFramePrefix_, objFramePrefix_);
pnh.param("pnp", usePnP_, usePnP_);
ROS_INFO("object_prefix = %s", objFramePrefix_.c_str()); ROS_INFO("object_prefix = %s", objFramePrefix_.c_str());
ROS_INFO("pnp = %s", usePnP_?"true":"false");
ros::NodeHandle nh; // public ros::NodeHandle nh; // public
@ -83,55 +86,101 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info, const QStri
// Find center of the object // Find center of the object
QPointF center = iter->map(QPointF(objectWidth/2, objectHeight/2)); 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, cv::Vec3f center3D = this->getDepth(depth,
center.x()+0.5f, center.y()+0.5f, center.x()+0.5f, center.y()+0.5f,
float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f, float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
1.0f/depthConstant, 1.0f/depthConstant); 1.0f/depthConstant, 1.0f/depthConstant);
cv::Vec3f axisEndX = this->getDepth(depth, cv::Vec3f axisEndX;
xAxis.x()+0.5f, xAxis.y()+0.5f, cv::Vec3f axisEndY;
float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f, if(!usePnP_)
1.0f/depthConstant, 1.0f/depthConstant); {
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, QPointF yAxis = iter->map(QPointF(objectWidth/2, 3*objectHeight/4));
yAxis.x()+0.5f, yAxis.y()+0.5f, axisEndY = this->getDepth(depth,
float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f, yAxis.x()+0.5f, yAxis.y()+0.5f,
1.0f/depthConstant, 1.0f/depthConstant); 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]) && if((std::isfinite(center3D.val[2]) && usePnP_) ||
std::isfinite(axisEndX.val[0]) && std::isfinite(axisEndX.val[1]) && std::isfinite(axisEndX.val[2]) && (std::isfinite(center3D.val[0]) && std::isfinite(center3D.val[1]) && std::isfinite(center3D.val[2]) &&
std::isfinite(axisEndY.val[0]) && std::isfinite(axisEndY.val[1]) && std::isfinite(axisEndY.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; tf::StampedTransform transform;
transform.setIdentity(); transform.setIdentity();
transform.child_frame_id_ = QString("%1_%2%3").arg(objFramePrefix_.c_str()).arg(id).arg(multiSuffix).toStdString(); transform.child_frame_id_ = QString("%1_%2%3").arg(objFramePrefix_.c_str()).arg(id).arg(multiSuffix).toStdString();
transform.frame_id_ = frameId.toStdString(); transform.frame_id_ = frameId.toStdString();
transform.stamp_.fromSec(stamp); 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; tf::Quaternion q;
rotationMatrix.getRotation(q); if(usePnP_)
{
std::vector<cv::Point3f> objectPoints(4);
std::vector<cv::Point2f> 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<double>(0,0) = 1.0f/depthConstant;
cameraMatrix.at<double>(1,1) = 1.0f/depthConstant;
cameraMatrix.at<double>(0,2) = float(depth.cols/2)-0.5f;
cameraMatrix.at<double>(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<double>(0,0), R.at<double>(0,1), R.at<double>(0,2),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2));
rotationMatrix.getRotation(q);
transform.setOrigin(tf::Vector3(
tvec.at<double>(0)*(center3D.val[2]/tvec.at<double>(2)),
tvec.at<double>(1)*(center3D.val[2]/tvec.at<double>(2)),
tvec.at<double>(2)*(center3D.val[2]/tvec.at<double>(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 // 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); q *= tf::createQuaternionFromRPY(CV_PI/2.0, CV_PI/2.0, 0);
transform.setRotation(q.normalized()); 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); transforms.push_back(transform);
} }
else else

1
src/ros/FindObjectROS.h
View File

@ -63,6 +63,7 @@ private:
ros::Publisher pubInfo_; ros::Publisher pubInfo_;
std::string objFramePrefix_; std::string objFramePrefix_;
bool usePnP_;
tf::TransformBroadcaster tfBroadcaster_; tf::TransformBroadcaster tfBroadcaster_;
}; };

24
src/ros/tf_example_node.cpp
View File

@ -34,11 +34,10 @@ class TfExample
{ {
public: public:
TfExample() : TfExample() :
mapFrameId_("/map"),
objFramePrefix_("object") objFramePrefix_("object")
{ {
ros::NodeHandle pnh("~"); ros::NodeHandle pnh("~");
pnh.param("map_frame_id", mapFrameId_, mapFrameId_); pnh.param("target_frame_id", targetFrameId_, targetFrameId_);
pnh.param("object_prefix", objFramePrefix_, objFramePrefix_); pnh.param("object_prefix", objFramePrefix_, objFramePrefix_);
ros::NodeHandle nh; ros::NodeHandle nh;
@ -51,6 +50,11 @@ public:
{ {
if(msg->objects.data.size()) if(msg->objects.data.size())
{ {
std::string targetFrameId = targetFrameId_;
if(targetFrameId.empty())
{
targetFrameId = msg->header.frame_id;
}
char multiSubId = 'b'; char multiSubId = 'b';
int previousId = -1; int previousId = -1;
for(unsigned int i=0; i<msg->objects.data.size(); i+=12) for(unsigned int i=0; i<msg->objects.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(); std::string objectFrameId = QString("%1_%2%3").arg(objFramePrefix_.c_str()).arg(id).arg(multiSuffix).toStdString();
tf::StampedTransform pose; tf::StampedTransform pose;
tf::StampedTransform poseCam;
try 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 // The timestamp matches the one sent over TF
tfListener_.lookupTransform(mapFrameId_, objectFrameId, msg->header.stamp, pose); tfListener_.lookupTransform(targetFrameId, objectFrameId, msg->header.stamp, pose);
tfListener_.lookupTransform(msg->header.frame_id, objectFrameId, msg->header.stamp, poseCam);
} }
catch(tf::TransformException & ex) catch(tf::TransformException & ex)
{ {
@ -87,21 +89,17 @@ public:
continue; 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]", 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.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z(),
pose.getRotation().x(), pose.getRotation().y(), pose.getRotation().z(), pose.getRotation().w()); 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: private:
std::string mapFrameId_; std::string targetFrameId_;
std::string objFramePrefix_; std::string objFramePrefix_;
ros::Subscriber subs_; ros::Subscriber subs_;
tf::TransformListener tfListener_; tf::TransformListener tfListener_;