39 #include <ar_track_alvar/kinect_filtering.h> 49 const float u =
static_cast<float>(rand())/RAND_MAX;
55 a::ARCloud::Ptr
generateCloud(
const double px,
const double py,
const double pz,
56 const double vx,
const double vy,
const double vz,
57 const double wx,
const double wy,
const double wz)
60 const double NOISE=0.01;
62 a::ARCloud::Ptr cloud(boost::make_shared<a::ARCloud>());
63 for (
double u=0; u<1+INC/2; u+=INC)
65 for (
double v=0; v<1+INC/2; v+=INC)
68 p.x = px+u*vx+v*wx+
randFloat(-NOISE, NOISE);
69 p.y = py+u*vy+v*wy+
randFloat(-NOISE, NOISE);
70 p.z = pz+u*vz+v*wz+
randFloat(-NOISE, NOISE);
71 cloud->points.push_back(p);
77 int main (
int argc,
char** argv)
81 cerr <<
"Usage: " << argv[0] <<
" PX PY PZ VX VY VZ WX WY WZ I1 I2 I3\n";
85 const double px = atof(argv[1]);
86 const double py = atof(argv[2]);
87 const double pz = atof(argv[3]);
88 const double vx = atof(argv[4]);
89 const double vy = atof(argv[5]);
90 const double vz = atof(argv[6]);
91 const double wx = atof(argv[7]);
92 const double wy = atof(argv[8]);
93 const double wz = atof(argv[9]);
95 a::ARCloud::ConstPtr cloud =
97 const size_t n = cloud->size();
98 ROS_INFO(
"Generated cloud with %zu points such as (%.4f, %.4f, %.4f)" 99 " and (%.4f, %.4f, %.4f)", n, (*cloud)[0].
x, (*cloud)[0].
y,
100 (*cloud)[0].
z, (*cloud)[n-1].x, (*cloud)[n-1].y, (*cloud)[n-1].z);
102 const size_t i1 = atoi(argv[10]);
103 const size_t i2 = atoi(argv[11]);
104 const size_t i3 = atoi(argv[12]);
108 ROS_INFO(
"Points are (%.4f, %.4f, %.4f) and (%.4f, %.4f, %.4f)",
109 p1.x, p1.y, p1.z, p2.x, p2.y, p2.z);
112 ROS_INFO(
"Plane equation is %.3fx + %.3fy + %.3fz + %.3f = 0",
int main(int argc, char **argv)
int extractOrientation(const pcl::ModelCoefficients &coeffs, const ARPoint &p1, const ARPoint &p2, const ARPoint &p3, const ARPoint &p4, geometry_msgs::Quaternion &retQ)
TFSIMD_FORCE_INLINE const tfScalar & y() const
float randFloat(float a, float b)
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE const tfScalar & z() const
#define ROS_INFO_STREAM(args)
pcl::ModelCoefficients coeffs
a::ARCloud::Ptr generateCloud(const double px, const double py, const double pz, const double vx, const double vy, const double vz, const double wx, const double wy, const double wz)
PlaneFitResult fitPlane(ARCloud::ConstPtr cloud)