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
00039 #include <ar_track_alvar/kinect_filtering.h>
00040
00041 namespace a=ar_track_alvar;
00042 namespace gm=geometry_msgs;
00043
00044 using std::cerr;
00045
00046
00047 float randFloat (float a, float b)
00048 {
00049 const float u = static_cast<float>(rand())/RAND_MAX;
00050 return a + u*(b-a);
00051 }
00052
00053
00054
00055 a::ARCloud::Ptr generateCloud(const double px, const double py, const double pz,
00056 const double vx, const double vy, const double vz,
00057 const double wx, const double wy, const double wz)
00058 {
00059 const double INC=0.1;
00060 const double NOISE=0.01;
00061
00062 a::ARCloud::Ptr cloud(boost::make_shared<a::ARCloud>());
00063 for (double u=0; u<1+INC/2; u+=INC)
00064 {
00065 for (double v=0; v<1+INC/2; v+=INC)
00066 {
00067 a::ARPoint p;
00068 p.x = px+u*vx+v*wx+randFloat(-NOISE, NOISE);
00069 p.y = py+u*vy+v*wy+randFloat(-NOISE, NOISE);
00070 p.z = pz+u*vz+v*wz+randFloat(-NOISE, NOISE);
00071 cloud->points.push_back(p);
00072 }
00073 }
00074 return cloud;
00075 }
00076
00077 int main (int argc, char** argv)
00078 {
00079 if (argc != 12)
00080 {
00081 cerr << "Usage: " << argv[0] << " PX PY PZ VX VY VZ WX WY WZ I1 I2 I3\n";
00082 return 1;
00083 }
00084
00085 const double px = atof(argv[1]);
00086 const double py = atof(argv[2]);
00087 const double pz = atof(argv[3]);
00088 const double vx = atof(argv[4]);
00089 const double vy = atof(argv[5]);
00090 const double vz = atof(argv[6]);
00091 const double wx = atof(argv[7]);
00092 const double wy = atof(argv[8]);
00093 const double wz = atof(argv[9]);
00094
00095 a::ARCloud::ConstPtr cloud =
00096 generateCloud(px, py, pz, vx, vy, vz, wx, wy, wz);
00097 const size_t n = cloud->size();
00098 ROS_INFO("Generated cloud with %zu points such as (%.4f, %.4f, %.4f)"
00099 " and (%.4f, %.4f, %.4f)", n, (*cloud)[0].x, (*cloud)[0].y,
00100 (*cloud)[0].z, (*cloud)[n-1].x, (*cloud)[n-1].y, (*cloud)[n-1].z);
00101
00102 const size_t i1 = atoi(argv[10]);
00103 const size_t i2 = atoi(argv[11]);
00104 const size_t i3 = atoi(argv[12]);
00105 a::ARPoint p1 = (*cloud)[i1];
00106 a::ARPoint p2 = (*cloud)[i2];
00107 a::ARPoint p3 = (*cloud)[i3];
00108 ROS_INFO("Points are (%.4f, %.4f, %.4f) and (%.4f, %.4f, %.4f)",
00109 p1.x, p1.y, p1.z, p2.x, p2.y, p2.z);
00110
00111 a::PlaneFitResult res = a::fitPlane(cloud);
00112 ROS_INFO("Plane equation is %.3fx + %.3fy + %.3fz + %.3f = 0",
00113 res.coeffs.values[0], res.coeffs.values[1], res.coeffs.values[2],
00114 res.coeffs.values[3]);
00115
00116 gm::Quaternion q = a::extractOrientation(res.coeffs, p1, p2, p3, p1);
00117 ROS_INFO_STREAM("Orientation is " << q);
00118
00119
00120 return 0;
00121 }