00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033 #include "fast_plane_detector/plane_transform.h"
00034 #include <geometry_msgs/Pose.h>
00035
00036 namespace fast_plane_detection {
00037
00038 PlaneTransform::PlaneTransform(const sensor_msgs::CameraInfo &camera_info,
00039 float baseline, float up_direction)
00040 : camera_info_(camera_info)
00041 , baseline_(baseline)
00042 , up_direction_(up_direction)
00043 {}
00044
00045 PlaneTransform::~PlaneTransform()
00046 {}
00047
00048 void PlaneTransform::setParameters( float alpha,
00049 float beta,
00050 float d,
00051 int min_x, int max_x,
00052 int min_y, int max_y,
00053 std_msgs::Header header)
00054 {
00055 alpha_ = alpha;
00056 beta_ = beta;
00057 d_ = d;
00058
00059 min_x_ = min_x;
00060 max_x_ = max_x;
00061
00062 min_y_ = min_y;
00063 max_y_ = max_y;
00064
00065 header_ = header;
00066 }
00067
00068 bool PlaneTransform::getPlane( Plane &plane ){
00069
00070
00071 std::vector<double> plane_coefficients
00072 = convertTo3DPlane( camera_info_, alpha_, beta_, d_);
00073
00074 tf::Transform plane_trans
00075 = getPlaneTransform (plane_coefficients, up_direction_);
00076
00077 sensor_msgs::PointCloud plane_limits;
00078 plane_limits.header.frame_id = header_.frame_id;
00079 plane_limits.header.stamp = ros::Time::now();
00080 plane_limits.points.resize(4);
00081
00082 get3DTableLimits(plane_limits);
00083
00084 if(!transformPlanePoints(plane_trans, plane_limits))
00085 return false;
00086
00087 plane = constructPlaneMsg(plane_limits, plane_coefficients, plane_trans);
00088
00089 return true;
00090 }
00091
00092
00093 std::vector<double> PlaneTransform::convertTo3DPlane(const sensor_msgs::CameraInfo &camera_info,
00094 float alpha,
00095 float beta,
00096 float d )
00097 {
00098 float fx = camera_info.P[0*4+0];
00099 float cx = camera_info.P[0*4+2];
00100 float cy = camera_info.P[1*4+2];
00101
00102 float A = alpha/baseline_;
00103 float B = beta/baseline_;
00104 float C = d/(baseline_ * fx) + (A*cx + B*cy)/fx;
00105
00106 ROS_DEBUG("Camera Parameters:");
00107 ROS_DEBUG("Focal length: %f", fx);
00108 ROS_DEBUG("Principal point: %f %f", cx, cy);
00109 ROS_DEBUG("Baseline %f", baseline_);
00110 ROS_DEBUG("alpha, beta and d %f %f %f", alpha, beta, d);
00111
00112
00113 float disp = alpha*cx + beta*cy + d;
00114
00115 float Z = baseline_*fx/disp;
00116 float D = C * Z;
00117 ROS_DEBUG("Apart from rounding error D should be 1 and is %f", D);
00118
00119
00120 float NMod = sqrt(A*A+B*B+C*C);
00121
00122 A/=NMod;
00123 B/=NMod;
00124 C/=NMod;
00125 D/=NMod;
00126
00127 ROS_DEBUG("3D Table plane parameters: %f %f %f %f", A, B, C, D);
00128
00129
00130 std::vector<double> table_coefficients;
00131 table_coefficients.push_back(A);
00132 table_coefficients.push_back(B);
00133 table_coefficients.push_back(C);
00134 table_coefficients.push_back(D);
00135
00136 return table_coefficients;
00137 }
00138
00139 tf::Transform
00140 PlaneTransform::getPlaneTransform ( const std::vector<double> &coeffs,
00141
00142 double up_direction)
00143 {
00144 ROS_ASSERT(coeffs.size() > 3);
00145 double a = coeffs[0], b = coeffs[1],
00146 c = coeffs[2], d = coeffs[3];
00147
00148 btVector3 position(a*d, b*d, c*d);
00149 btVector3 z(a, b, c);
00150
00151 ROS_DEBUG("z.dot: %0.3f", z.dot(btVector3(0,0,1)));
00152 ROS_DEBUG("in getPlaneTransform, z: %0.3f, %0.3f, %0.3f",
00153 z[0], z[1], z[2]);
00154 if ( z.dot( btVector3(0, 0, up_direction) ) < 0)
00155 {
00156 z = -1.0 * z;
00157 ROS_DEBUG("flipped z");
00158 }
00159 ROS_DEBUG("in getPlaneTransform, z: %0.3f, %0.3f, %0.3f",
00160 z[0], z[1], z[2]);
00161
00162
00163
00164 btVector3 x(1, 0, 0);
00165 if ( fabs(z.dot(x)) > 1.0 - 1.0e-4) x = btVector3(0, 1, 0);
00166 btVector3 y = z.cross(x).normalized();
00167 x = y.cross(z).normalized();
00168
00169 btMatrix3x3 rotation;
00170 rotation[0] = x;
00171 rotation[1] = y;
00172 rotation[2] = z;
00173 rotation = rotation.transpose();
00174 btQuaternion orientation;
00175 rotation.getRotation(orientation);
00176 return tf::Transform(orientation, position);
00177 }
00178
00179 bool PlaneTransform::transformPlanePoints ( const tf::Transform& plane_trans,
00180 sensor_msgs::PointCloud &plane_limits )
00181 {
00182
00183
00184
00185 tf::StampedTransform plane_pose_frame(plane_trans,
00186 plane_limits.header.stamp,
00187 plane_limits.header.frame_id,
00188 "plane_frame");
00189 listener.setTransform(plane_pose_frame);
00190 std::string error_msg;
00191
00192 if (!listener.canTransform("plane_frame", plane_limits.header.frame_id,
00193 plane_limits.header.stamp, &error_msg))
00194 {
00195 ROS_ERROR("Cannot transform point cloud from frame %s to plane frame; error %s",
00196 plane_limits.header.frame_id.c_str(), error_msg.c_str());
00197 return false;
00198 }
00199 try
00200 {
00201 listener.transformPointCloud("plane_frame", plane_limits, plane_limits);
00202 }
00203 catch (tf::TransformException ex)
00204 {
00205 ROS_ERROR("Failed to transform point cloud from frame %s into plane_frame; error %s",
00206 plane_limits.header.frame_id.c_str(), ex.what());
00207 return false;
00208 }
00209 plane_limits.header.frame_id = "plane_frame";
00210 return true;
00211 }
00212
00213
00214 Plane
00215 PlaneTransform::constructPlaneMsg(const sensor_msgs::PointCloud &plane_limits,
00216 const std::vector<double> &coeffs,
00217
00218 const tf::Transform &plane_trans)
00219 {
00220
00221 Plane plane;
00222
00223 ROS_ASSERT(coeffs.size() > 3);
00224 double a = coeffs[0], b = coeffs[1],
00225 c = coeffs[2];
00226
00227 plane.normal.header = plane_limits.header;
00228 plane.normal.vector.x = a;
00229 plane.normal.vector.y = b;
00230 plane.normal.vector.z = c;
00231
00232
00233 geometry_msgs::Pose plane_pose;
00234 tf::poseTFToMsg(plane_trans, plane_pose);
00235
00236 plane.top_left = plane_limits.points[0];
00237 plane.top_right = plane_limits.points[1];
00238 plane.bottom_left = plane_limits.points[3];
00239 plane.bottom_right = plane_limits.points[2];
00240
00241
00242 ROS_DEBUG("Plane Pose: [%f %f %f] [%f %f %f %f] ",
00243 plane_pose.position.x, plane_pose.position.y,
00244 plane_pose.position.z,
00245 plane_pose.orientation.x, plane_pose.orientation.y,
00246 plane_pose.orientation.z,
00247 plane_pose.orientation.w);
00248
00249 plane.pose.pose = plane_pose;
00250 plane.pose.header = plane_limits.header;
00251
00252 return plane;
00253 }
00254
00255 void PlaneTransform::get3DTableLimits(sensor_msgs::PointCloud &plane_limits)
00256 {
00257 Point p;
00258 get3DPlanePoint(min_x_, min_y_, p);
00259 plane_limits.points[0] = p;
00260
00261 get3DPlanePoint(max_x_, min_y_, p);
00262 plane_limits.points[1] = p;
00263
00264 get3DPlanePoint(max_x_, max_y_, p);
00265 plane_limits.points[2] = p;
00266
00267 get3DPlanePoint(min_x_, max_y_, p);
00268 plane_limits.points[3] = p;
00269 }
00270
00271 void PlaneTransform::get3DPlanePoint( int u, int v,
00272 Point &p)
00273 {
00274 float disp = alpha_*u + beta_*v + d_;
00275
00276 float fx = camera_info_.P[0*4+0];
00277 float fy = camera_info_.P[1*4+1];
00278 float cx = camera_info_.P[0*4+2];
00279 float cy = camera_info_.P[1*4+2];
00280
00281 p.z = (fx * baseline_) / disp;
00282 p.x = (u - cx) * p.z / fx;
00283 p.y = (v - cy) * p.z / fy;
00284 }
00285
00286 }
00287
