ias_drawer_executive: ias_drawer_executive: AverageTF.cpp Source File

00001 /*
00002  * Copyright (c) 2010, Thomas Ruehr <ruehr@cs.tum.edu>
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 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 
00031 #include <ias_drawer_executive/AverageTF.h>
00032 
00033 tf::Stamped<tf::Pose> AverageTF::getMarkerTransform(const char tf_frame[], double numSamples)
00034 {
00035   tf::Stamped<tf::Pose> marker;
00036   tf::TransformListener listener;
00037   ros::NodeHandle node;
00038   ros::Rate rate(100.0);
00039   int count = 0;
00040 
00041   double lastTime = 0;
00042   tf::Stamped<tf::Pose> transSum;
00043   int i = 0;
00044   geometry_msgs::Quaternion rot[500];
00045   geometry_msgs::Vector3 trans[500];
00046 
00047   int numh = 0;
00048   int numv = 0;
00049 
00050 
00051   while (count < numSamples  && ros::ok()){
00052     tf::StampedTransform transform;
00053     try{
00054       listener.lookupTransform("/base_link", tf_frame,
00055                                ros::Time(0), transform);
00056     }
00057     catch (tf::TransformException ex){
00058       //ROS_ERROR("%s",ex.what());
00059     }
00060 
00061     double actTime = transform.stamp_.toSec();
00062 
00063     //ROS_INFO("I see a ARToolkit Marker. %f", transform.stamp_.toSec());
00064 
00065     if (actTime!=lastTime) {
00066       lastTime=actTime;
00067      // ROS_INFO("NEW MARKER POSITION %i" , count);
00068       marker.frame_id_ = transform.frame_id_;
00069       marker.setOrigin(transform.getOrigin());
00070       marker.setRotation(transform.getRotation());
00071       marker.stamp_ = transform.stamp_;
00072 
00073       transform.setRotation(transform.getRotation().normalize());
00074       trans[count].x = transform.getOrigin().x();
00075       trans[count].y = transform.getOrigin().y();
00076       trans[count].z = transform.getOrigin().z();
00077       rot[count].x = transform.getRotation().x();
00078       rot[count].y = transform.getRotation().y();
00079       rot[count].z = transform.getRotation().z();
00080       rot[count].w = transform.getRotation().w();
00081       count++;
00082 
00083       double meanZ = 0;
00084       for (i = 0; i < count; i++)
00085       meanZ += trans[i].z;
00086       meanZ /= (double)count;
00087 
00088       double deviation=0;
00089       for (i = 0; i < count; i++)
00090          deviation+= (trans[i].z - meanZ) * (trans[i].z - meanZ);
00091       deviation/= (double)count;
00092 
00093       printf("\a");
00094 
00095       ROS_INFO("new marker %i STD DEVIATION IN Z %f", count, sqrtf(deviation));
00096 
00097       ROS_INFO("CURRENT MARKER %f %f %f ",transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z());
00098       ROS_INFO("CURRENT MARKER OR %f %f %f %f", transform.getRotation().x(), transform.getRotation().y(), transform.getRotation().z(), transform.getRotation().w());
00099 
00100 
00101       tf::Stamped<tf::Pose> decid;
00102       decid.frame_id_ = "none";
00103       decid.stamp_ = ros::Time();
00104       decid.setOrigin(btVector3(1,0,0));
00105       decid.setRotation(btQuaternion(0,0,0,1));
00106       btTransform classify = transform * decid;
00107 
00108       tf::Stamped<tf::Pose> decid2;
00109       decid2.frame_id_ = "none";
00110       decid2.stamp_ = ros::Time();
00111       decid2.setOrigin(btVector3(0,1,0));
00112       decid2.setRotation(btQuaternion(0,0,0,1));
00113 
00114       btTransform classify2 = transform * decid2;
00115 
00116       ROS_INFO("DECID :c  %f c1 %f m %f", classify.getOrigin().z(), classify2.getOrigin().z(), transform.getOrigin().z());
00117 
00118       if (classify.getOrigin().z() < classify2.getOrigin().z()) {
00119           numh++;
00120           ROS_INFO("HORIZONTAL %i", numh);
00121       } else {
00122           numv++;
00123           ROS_INFO("VERTICAL %i", numv);
00124       }
00125 
00126       //tf::Stamped<tf::Pose> rot;
00127       //rot.setRotation(btQuaternion(0,0,-M_PI/2));
00128 
00129       //btQuaternion myq(transform.getRotation().x(),transform.getRotation().y(),transform.getRotation().z(),transform.getRotation().w());
00130       //btQuaternion qy = myq * btQuaternion(0,M_PI / 2,0);;
00131       //ROS_INFO("ROT Y %f %f %f %f", qy.x(),qy.y(),qy.z(),qy.w());
00132 
00133      //btVector3 euler;
00134      //QuaternionToEuler(btQuaternion(rot[count].x,rot[count].y,rot[count].z,rot[count].w).normalize(), euler);
00135      //QuaternionToEuler(transform.getRotation().normalize(), euler);
00136 
00137      //ROS_INFO("EULER %f %f %f", euler.x(),euler.y(),euler.z());
00138     }
00139 
00140     rate.sleep();
00141   }
00142 
00143   //remove outliers
00144   double meanZ = 0;
00145   for (i = 0; i < numSamples; i++)
00146           meanZ += trans[i].z;
00147   meanZ /= numSamples;
00148 
00149   double deviation=0;
00150   for (i = 0; i < numSamples; i++)
00151           deviation+= (trans[i].z - meanZ) * (trans[i].z - meanZ);
00152   deviation /= numSamples;
00153 
00154   ROS_INFO("STD DEVIATION IN Z %f", sqrtf(deviation));
00155 
00156   geometry_msgs::Quaternion rotMean;
00157   geometry_msgs::Vector3 transMean;
00158 
00159   transMean.x = 0;
00160   transMean.y = 0;
00161   transMean.z = 0;
00162   rotMean.x = 0;
00163   rotMean.y = 0;
00164   rotMean.z = 0;
00165   rotMean.w = 0; // would otherwise be 1
00166 
00167   double numGoodSamples = 0;
00168   for (i = 0; i < numSamples; i++)
00169      if ((trans[i].z - meanZ) * (trans[i].z - meanZ) < deviation) {
00170                  transMean.x += trans[i].x;
00171                  transMean.y += trans[i].y;
00172                  transMean.z += trans[i].z;
00173                  rotMean.x += rot[i].x;
00174                  rotMean.y += rot[i].y;
00175                  rotMean.z += rot[i].z;
00176                  rotMean.w += rot[i].w;
00177                  numGoodSamples+=1;
00178           }
00179 
00180   ROS_INFO("Number of accepted samples %f", numGoodSamples);
00181 
00182   transMean.x /= numGoodSamples;
00183   transMean.y /= numGoodSamples;
00184   transMean.z /= numGoodSamples;
00185   rotMean.x /= numGoodSamples;
00186   rotMean.y /= numGoodSamples;
00187   rotMean.z /= numGoodSamples;
00188   rotMean.w /= numGoodSamples;
00189 
00190 
00191   if (numh > numv) {
00192      rotMean.x = -.711;
00193      rotMean.y = -.008;
00194      rotMean.z = .005;
00195      rotMean.w = .703;
00196   } else {
00197      rotMean.x = 0;
00198      rotMean.y = 0;
00199      rotMean.z = 0;
00200      rotMean.w = 1;
00201   }
00202 
00203   marker.setOrigin(btVector3(transMean.x,transMean.y,transMean.z));
00204   marker.setRotation(btQuaternion(rotMean.x,rotMean.y,rotMean.z,rotMean.w).normalize());
00205   return marker;
00206 }