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
00034
00035 #include <Eigen/Core>
00036 #include <ar_track_alvar/filter/kinect_filtering.h>
00037 #include <tf/tf.h>
00038 #include <tf/transform_datatypes.h>
00039
00040
00041 namespace ar_track_alvar
00042 {
00043
00044 namespace gm=geometry_msgs;
00045
00046 using std::vector;
00047 using std::cerr;
00048 using std::endl;
00049 using std::ostream;
00050
00051
00052
00053 const double distance_threshold_ = 0.005;
00054
00055 PlaneFitResult fitPlane (ARCloud::ConstPtr cloud)
00056 {
00057 PlaneFitResult res;
00058 pcl::PointIndices::Ptr inliers=boost::make_shared<pcl::PointIndices>();
00059
00060 pcl::SACSegmentation<ARPoint> seg;
00061 seg.setOptimizeCoefficients(true);
00062 seg.setModelType(pcl::SACMODEL_PLANE);
00063 seg.setMethodType(pcl::SAC_RANSAC);
00064 seg.setDistanceThreshold(distance_threshold_);
00065
00066 seg.setInputCloud(cloud);
00067 seg.segment(*inliers, res.coeffs);
00068
00069 pcl::ExtractIndices<ARPoint> extracter;
00070 extracter.setInputCloud(cloud);
00071 extracter.setIndices(inliers);
00072 extracter.setNegative(false);
00073 extracter.filter(*res.inliers);
00074
00075 return res;
00076 }
00077
00078 ARCloud::Ptr filterCloud (const ARCloud& cloud, const vector<cv::Point, Eigen::aligned_allocator<cv::Point> >& pixels)
00079 {
00080 ARCloud::Ptr out(new ARCloud());
00081
00082
00083 for(size_t i=0; i<pixels.size(); i++)
00084 {
00085 const cv::Point& p = pixels[i];
00086 const ARPoint& pt = cloud(p.x, p.y);
00087 if (isnan(pt.x) || isnan(pt.y) || isnan(pt.z)){
00088
00089 }
00090 else
00091 out->points.push_back(pt);
00092 }
00093 return out;
00094 }
00095
00096 gm::Point centroid (const ARCloud& points)
00097 {
00098 gm::Point sum;
00099 sum.x = 0;
00100 sum.y = 0;
00101 sum.z = 0;
00102
00103 for(size_t i=0; i<points.size(); i++)
00104 {
00105 sum.x += points[i].x;
00106 sum.y += points[i].y;
00107 sum.z += points[i].z;
00108 }
00109
00110 gm::Point center;
00111 const size_t n = points.size();
00112 center.x = sum.x/n;
00113 center.y = sum.y/n;
00114 center.z = sum.z/n;
00115 return center;
00116 }
00117
00118
00119 inline
00120 gm::Quaternion makeQuaternion (double x, double y, double z, double w)
00121 {
00122 gm::Quaternion q;
00123 q.x = x;
00124 q.y = y;
00125 q.z = z;
00126 q.w = w;
00127 return q;
00128 }
00129
00130
00131 int getCoeffs (const pcl::ModelCoefficients& coeffs, double* a, double* b,
00132 double* c, double* d)
00133 {
00134 if(coeffs.values.size() != 4)
00135 return -1;
00136 const double s = coeffs.values[0]*coeffs.values[0] +
00137 coeffs.values[1]*coeffs.values[1] + coeffs.values[2]*coeffs.values[2];
00138 if(fabs(s) < 1e-6)
00139 return -1;
00140 *a = coeffs.values[0]/s;
00141 *b = coeffs.values[1]/s;
00142 *c = coeffs.values[2]/s;
00143 *d = coeffs.values[3]/s;
00144 return 0;
00145 }
00146
00147
00148 tf::Vector3 project (const ARPoint& p, const double a, const double b,
00149 const double c, const double d)
00150 {
00151 const double t = a*p.x + b*p.y + c*p.z + d;
00152 return tf::Vector3(p.x-t*a, p.y-t*b, p.z-t*c);
00153 }
00154
00155 ostream& operator<< (ostream& str, const tf::Matrix3x3& m)
00156 {
00157 str << "[" << m[0][0] << ", " << m[0][1] << ", " << m[0][2] << "; "
00158 << m[1][0] << ", " << m[1][1] << ", " << m[1][2] << "; "
00159 << m[2][0] << ", " << m[2][1] << ", " << m[2][2] << "]";
00160 return str;
00161 }
00162
00163 ostream& operator<< (ostream& str, const tf::Quaternion& q)
00164 {
00165 str << "[(" << q.x() << ", " << q.y() << ", " << q.z() <<
00166 "), " << q.w() << "]";
00167 return str;
00168 }
00169
00170 ostream& operator<< (ostream& str, const tf::Vector3& v)
00171 {
00172 str << "(" << v.x() << ", " << v.y() << ", " << v.z() << ")";
00173 return str;
00174 }
00175
00176 int extractFrame (const pcl::ModelCoefficients& coeffs,
00177 const ARPoint& p1, const ARPoint& p2,
00178 const ARPoint& p3, const ARPoint& p4,
00179 tf::Matrix3x3 &retmat)
00180 {
00181
00182 double a=0, b=0, c=0, d=0;
00183 if(getCoeffs(coeffs, &a, &b, &c, &d) < 0)
00184 return -1;
00185
00186 const tf::Vector3 q1 = project(p1, a, b, c, d);
00187 const tf::Vector3 q2 = project(p2, a, b, c, d);
00188 const tf::Vector3 q3 = project(p3, a, b, c, d);
00189 const tf::Vector3 q4 = project(p4, a, b, c, d);
00190
00191
00192 if((q2-q1).length() < 1e-3)
00193 return -1;
00194
00195
00196 const tf::Vector3 v = (q2-q1).normalized();
00197 const tf::Vector3 n(a, b, c);
00198 const tf::Vector3 w = -v.cross(n);
00199 tf::Matrix3x3 m(v[0], v[1], v[2], w[0], w[1], w[2], n[0], n[1], n[2]);
00200
00201
00202 const tf::Vector3 diff = (q4-q3).normalized();
00203
00204 if (w.dot(diff)<0)
00205 {
00206
00207 m[1] = -m[1];
00208 m[2] = -m[2];
00209
00210 }
00211
00212
00213 retmat = m.inverse();
00214
00215 return 0;
00216 }
00217
00218
00219 int getQuaternion (const tf::Matrix3x3& m, tf::Quaternion &retQ)
00220 {
00221 if(m.determinant() <= 0)
00222 return -1;
00223
00224
00225
00226
00227
00228
00229 Eigen::Matrix3f eig_m;
00230 for(int i=0; i<3; i++){
00231 for(int j=0; j<3; j++){
00232 eig_m(i,j) = m[i][j];
00233 }
00234 }
00235 Eigen::Quaternion<float> eig_quat(eig_m);
00236
00237
00238 tfScalar ex = eig_quat.x();
00239 tfScalar ey = eig_quat.y();
00240 tfScalar ez = eig_quat.z();
00241 tfScalar ew = eig_quat.w();
00242 tf::Quaternion quat(ex,ey,ez,ew);
00243 retQ = quat.normalized();
00244
00245 return 0;
00246 }
00247
00248
00249 int extractOrientation (const pcl::ModelCoefficients& coeffs,
00250 const ARPoint& p1, const ARPoint& p2,
00251 const ARPoint& p3, const ARPoint& p4,
00252 gm::Quaternion &retQ)
00253 {
00254 tf::Matrix3x3 m;
00255 if(extractFrame(coeffs, p1, p2, p3, p4, m) < 0)
00256 return -1;
00257 tf::Quaternion q;
00258 if(getQuaternion(m,q) < 0)
00259 return -1;
00260 retQ.x = q.x();
00261 retQ.y = q.y();
00262 retQ.z = q.z();
00263 retQ.w = q.w();
00264 return 0;
00265 }
00266
00267 }