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>
<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 -->
<node name="find_object_2d" pkg="find_object_2d" type="find_object_2d" output="screen">
<remap from="image" to="image"/>
<param name="gui" value="false" type="bool"/>
<param name="objects_path" value="~/objects" type="str"/>
<param name="settings_path" value="~/.ros/find_object_2d.ini" type="str"/>
<remap from="image" to="$(arg image_topic)"/>
<param name="gui" value="$(arg gui)" type="bool"/>
<param name="objects_path" value="$(arg objects_path)" type="str"/>
<param name="settings_path" value="$(arg settings_path)" type="str"/>
</node>
</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 -->
<!-- $roslaunch openni_launch openni.launch depth_registration:=true -->
<node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen">
<param name="gui" value="true" type="bool"/>
<param name="settings_path" value="~/.ros/find_object_2d.ini" type="str"/>
<param name="subscribe_depth" value="true" type="bool"/>
<param name="objects_path" value="" type="str"/>
<param name="object_prefix" value="object" type="str"/>
<param name="approx_sync" value="true" type="bool"/>
<arg name="object_prefix" default="object"/>
<arg name="objects_path" default=""/>
<arg name="gui" default="true"/>
<arg name="approx_sync" default="true"/>
<arg name="pnp" default="true"/>
<arg name="tf_example" default="true"/>
<arg name="settings_path" default="~/.ros/find_object_2d.ini"/>
<remap from="rgb/image_rect_color" to="camera/rgb/image_rect_color"/>
<remap from="depth_registered/image_raw" to="camera/depth_registered/image_raw"/>
<remap from="depth_registered/camera_info" to="camera/depth_registered/camera_info"/>
<arg name="rgb_topic" default="camera/rgb/image_rect_color"/>
<arg name="depth_topic" default="camera/depth_registered/image_raw"/>
<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>
<!-- Example of tf synchronisation with the objectsStamped message -->
<node 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="object" type="str"/>
<node if="$(arg tf_example)" name="tf_example" pkg="find_object_2d" type="tf_example" output="screen">
<param name="object_prefix" value="$(arg object_prefix)" type="str"/>
</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>

13
launch/find_object_3d_kinect2.launch
View File

@ -4,13 +4,18 @@
<!-- Which image resolution: sd, qhd, hd -->
<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">
<param name="gui" value="true" type="bool"/>
<param name="settings_path" value="~/.ros/find_object_2d.ini" type="str"/>
<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="" type="str"/>
<param name="object_prefix" value="object" type="str"/>
<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="false" type="bool"/>
<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"/>

14
launch/find_object_3d_zed.launch
View File

@ -2,12 +2,18 @@
<!-- Example finding 3D poses of the objects detected -->
<!-- $ 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">
<param name="gui" value="true" type="bool"/>
<param name="settings_path" value="~/.ros/find_object_2d.ini" type="str"/>
<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="" type="str"/>
<param name="object_prefix" value="object" type="str"/>
<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="false" type="bool"/>
<remap from="rgb/image_rect_color" to="/zed/rgb/image_rect_color"/>
<remap from="depth_registered/image_raw" to="/zed/depth/depth_registered"/>

79
src/ros/FindObjectROS.cpp
View File

@ -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,36 +86,84 @@ 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,
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,
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]) &&
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]))
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::Quaternion 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]);
@ -123,15 +174,13 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info, const QStri
xAxis.x(), yAxis.x() ,zAxis.x(),
xAxis.y(), yAxis.y(), zAxis.y(),
xAxis.z(), yAxis.z(), zAxis.z());
tf::Quaternion q;
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

1
src/ros/FindObjectROS.h
View File

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

24
src/ros/tf_example_node.cpp
View File

@ -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; 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();
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_;