test_points.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  */
00030 
00039 #include <ar_track_alvar/kinect_filtering.h>
00040 #include <fstream>
00041 
00042 namespace a=ar_track_alvar;
00043 namespace gm=geometry_msgs;
00044 
00045 using std::cerr;
00046 using std::ifstream;
00047 
00048 // Random float between a and b
00049 float randFloat (float a, float b)
00050 {
00051   const float u = static_cast<float>(rand())/RAND_MAX;
00052   return a + u*(b-a);
00053 }
00054 
00055 // Generate points in a square in space of form p+av+bw where 
00056 // a and b range from 0 to 1
00057 a::ARCloud::Ptr generateCloud(const double px, const double py, const double pz,
00058                               const double vx, const double vy, const double vz,
00059                               const double wx, const double wy, const double wz)
00060 {
00061   const double INC=0.1;
00062   const double NOISE=0.01;
00063 
00064   a::ARCloud::Ptr cloud(boost::make_shared<a::ARCloud>());
00065   for (double u=0; u<1+INC/2; u+=INC)
00066   {
00067     for (double v=0; v<1+INC/2; v+=INC)
00068     {
00069       a::ARPoint p;
00070       p.x = px+u*vx+v*wx+randFloat(-NOISE, NOISE);
00071       p.y = py+u*vy+v*wy+randFloat(-NOISE, NOISE);
00072       p.z = pz+u*vz+v*wz+randFloat(-NOISE, NOISE);
00073       cloud->points.push_back(p);
00074     }
00075   }
00076   return cloud;
00077 }
00078 
00079 int main (int argc, char** argv)
00080 {
00081   ros::init(argc, argv, "test_points");
00082   ifstream f("points");
00083   a::ARCloud::Ptr cloud(new a::ARCloud());
00084   while (!f.eof())
00085   {
00086     a::ARPoint pt;
00087     f >> pt.x >> pt.y >> pt.z;
00088     cloud->points.push_back(pt);
00089   }
00090   ROS_INFO("Cloud has %zu points such as (%.2f, %.2f, %.2f)",
00091            cloud->points.size(), cloud->points[0].x, cloud->points[0].y,
00092            cloud->points[0].z);
00093   a::ARPoint p1, p2, p3;
00094   p1.x = 0.1888;
00095   p1.y = 0.1240;
00096   p1.z = 0.8620;
00097   p2.x = 0.0372;
00098   p2.y = 0.1181;
00099   p2.z = 0.8670;
00100   p3.x = 42;
00101   p3.y = 24;
00102   p3.z = 88;
00103 
00104   a::PlaneFitResult res = a::fitPlane(cloud);
00105   ROS_INFO("Plane equation is %.3fx + %.3fy + %.3fz + %.3f = 0",
00106            res.coeffs.values[0], res.coeffs.values[1], res.coeffs.values[2],
00107            res.coeffs.values[3]);
00108   
00109   gm::Quaternion q = a::extractOrientation(res.coeffs, p1, p2, p3, p1);
00110   ROS_INFO_STREAM("Orientation is " << q);
00111   return 0;
00112 }


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Sun Oct 5 2014 22:16:26