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).
This commit is contained in:
matlabbe
+81
-32
@@ -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<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
|
||||
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
|
||||
|
||||
@@ -63,6 +63,7 @@ private:
|
||||
ros::Publisher pubInfo_;
|
||||
|
||||
std::string objFramePrefix_;
|
||||
bool usePnP_;
|
||||
tf::TransformBroadcaster tfBroadcaster_;
|
||||
|
||||
};
|
||||
|
||||
+11
-13
@@ -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_;
|
||||
|
||||
Reference in New Issue
Block a user