Revision: 8966
http://playerstage.svn.sourceforge.net/playerstage/?rev=8966&view=rev
Author: jpgr87
Date: 2010-11-12 04:44:03 +0000 (Fri, 12 Nov 2010)
Log Message:
-----------
Forgot to add new source files from patch #3055958
Added Paths:
-----------
code/player/trunk/libplayerutil/
code/player/trunk/libplayerutil/CMakeLists.txt
code/player/trunk/libplayerutil/localization.c
code/player/trunk/libplayerutil/localization.h
Added: code/player/trunk/libplayerutil/CMakeLists.txt
===================================================================
--- code/player/trunk/libplayerutil/CMakeLists.txt
(rev 0)
+++ code/player/trunk/libplayerutil/CMakeLists.txt 2010-11-12 04:44:03 UTC
(rev 8966)
@@ -0,0 +1,10 @@
+SET (playerutilSrcs localization.c)
+INCLUDE_DIRECTORIES (${PROJECT_SOURCE_DIR}/client_libs)
+
+PLAYER_ADD_LIBRARY (playerutil ${playerutilSrcs})
+PLAYER_MAKE_PKGCONFIG ("playerutil" "Player utility functions library - part
of the Player Project"
+ "" "" "" "")
+ADD_DEPENDENCIES (playerutil playerc)
+
+PLAYER_INSTALL_HEADERS (playerutil localization.h)
+
Added: code/player/trunk/libplayerutil/localization.c
===================================================================
--- code/player/trunk/libplayerutil/localization.c
(rev 0)
+++ code/player/trunk/libplayerutil/localization.c 2010-11-12 04:44:03 UTC
(rev 8966)
@@ -0,0 +1,54 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2000
+ * Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ ********************************************************************/
+
+/***************************************************************************
+ * Desc: Localization helpers
+ * Author: Thimo Langbehn <[email protected]>
+ * Date: 05 July 2010
+ **************************************************************************/
+
+#include <libplayerutil/localization.h>
+
+void
+derive_uncertainty_ellipsis2d(player_pose2d_t* ellipse_pose,
+ double* radius_x, double* radius_y,
+ player_localize_hypoth_t* hypothesis,
+ double probability_coverage)
+{
+ double xx, yy, rxy;
+ double kk, t;
+
+ /** The covariance matrix pose estimate (lower half)
+ (cov(xx) in m$^2$, cov(yy) in $^2$, cov(aa) in rad$^2$,
+ cov(xy), cov(ya), cov(xa) ). */
+ xx = hypothesis->cov[0];
+ yy = hypothesis->cov[1];
+ rxy = hypothesis->cov[3];
+
+ ellipse_pose->px = hypothesis->mean.px;
+ ellipse_pose->py = hypothesis->mean.py;
+ ellipse_pose->pa = 0.5*atan2(2.0*rxy, xx-yy);
+
+ kk = -1 * log(1-probability_coverage);
+ t = sqrt( (xx-yy)*(xx-yy) + 4*rxy*rxy );
+ *radius_x = sqrt(kk*(xx+yy+t));
+ *radius_y = sqrt(kk*(xx+yy-t));
+}
Added: code/player/trunk/libplayerutil/localization.h
===================================================================
--- code/player/trunk/libplayerutil/localization.h
(rev 0)
+++ code/player/trunk/libplayerutil/localization.h 2010-11-12 04:44:03 UTC
(rev 8966)
@@ -0,0 +1,64 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2000
+ * Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ ********************************************************************/
+
+/***************************************************************************
+ * Desc: Localization helpers
+ * Author: Thimo Langbehn <[email protected]>
+ * Date: 05 July 2010
+ **************************************************************************/
+
+#ifndef LOCALIZATION_HH
+#define LOCALIZATION_HH
+
+#include <libplayerinterface/player.h>
+
+/** Calculates the 2d uncertainty ellipsis parameters for a given hypothesis
+ *
+ * This function can be used to calculate the parameters for an uncertainty
+ * ellipsis to a given, two dimensional gaussian distribution. The function
+ * only requires the values of a two dimensional covariance matrix, however
+ * to it makes sense (semantically) to use the player_localize_hypoth_t
+ * structure as parameter instead of a plain array.
+ *
+ * The unccertainty ellipsis derived from the hypothesis is a projection
+ * of the 3-dimensional (x,y,angle) ellipsoid into a 2-dimensional ellipsis
+ * (x,y), ignoring variance and covariances related to the angle.
+ *
+ * The probability that is to be covered by the ellipsis (i.e. the scale)
+ * can be controlled by the parameter probability_coverage. Pay attention to
+ * the semantics, this is not the probability of the 2d-Gaussian on the
+ * border of the ellipsis, in fact it is the integral of the 2d-gaussian
+ * enclosed in the ellipsis.
+ *
+ * \param ellipse_pose (out) will store the ellipsis position and angle
+ * \param radius_x (out) will store the first ellipsis radius
+ * \param radius_y (out) will store the second ellipsis radius
+ * \param hypothesis (in) contains the covariance matrix used
+ * \param probability_coverage (in) contains a value in the range [0,1]
+ * that represents the probability covered by the ellipsis.
+ */
+void
+derive_uncertainty_ellipsis2d(player_pose2d_t* ellipse_pose,
+ double* radius_x, double* radius_y,
+ player_localize_hypoth_t* hypothesis,
+ double probability_coverage);
+
+#endif //LOCALIZATION_HH
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Centralized Desktop Delivery: Dell and VMware Reference Architecture
Simplifying enterprise desktop deployment and management using
Dell EqualLogic storage and VMware View: A highly scalable, end-to-end
client virtualization framework. Read more!
http://p.sf.net/sfu/dell-eql-dev2dev
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
