Revision: 8859
http://playerstage.svn.sourceforge.net/playerstage/?rev=8859&view=rev
Author: jpgr87
Date: 2010-08-29 00:39:27 +0000 (Sun, 29 Aug 2010)
Log Message:
-----------
Applied patch #3046846: Corrects localize interface to full covariance matrix
Modified Paths:
--------------
code/player/trunk/client_libs/libplayerc/dev_localize.c
code/player/trunk/client_libs/libplayerc/playerc.h
code/player/trunk/client_libs/libplayerc++/localizeproxy.cc
code/player/trunk/libplayerinterface/interfaces/025_localize.def
code/player/trunk/server/drivers/localization/amcl/amcl.cc
code/player/trunk/server/drivers/shell/readlog.cc
code/player/trunk/server/drivers/shell/writelog.cc
Modified: code/player/trunk/client_libs/libplayerc/dev_localize.c
===================================================================
--- code/player/trunk/client_libs/libplayerc/dev_localize.c 2010-08-26
22:43:10 UTC (rev 8858)
+++ code/player/trunk/client_libs/libplayerc/dev_localize.c 2010-08-29
00:39:27 UTC (rev 8859)
@@ -119,7 +119,7 @@
// Set the robot pose (mean and covariance)
-int playerc_localize_set_pose(playerc_localize_t *device, double pose[3],
double cov[3])
+int playerc_localize_set_pose(playerc_localize_t *device, double pose[3],
double cov[6])
{
player_localize_set_pose_t req;
@@ -130,6 +130,9 @@
req.cov[0] = cov[0];
req.cov[1] = cov[1];
req.cov[2] = cov[2];
+ req.cov[3] = cov[3];
+ req.cov[4] = cov[4];
+ req.cov[5] = cov[5];
if(playerc_client_request(device->info.client,
&device->info,
Modified: code/player/trunk/client_libs/libplayerc/playerc.h
===================================================================
--- code/player/trunk/client_libs/libplayerc/playerc.h 2010-08-26 22:43:10 UTC
(rev 8858)
+++ code/player/trunk/client_libs/libplayerc/playerc.h 2010-08-29 00:39:27 UTC
(rev 8859)
@@ -2331,7 +2331,7 @@
PLAYERC_EXPORT int playerc_localize_unsubscribe(playerc_localize_t *device);
/** @brief Set the the robot pose (mean and covariance). */
-PLAYERC_EXPORT int playerc_localize_set_pose(playerc_localize_t *device,
double pose[3], double cov[3]);
+PLAYERC_EXPORT int playerc_localize_set_pose(playerc_localize_t *device,
double pose[3], double cov[6]);
/** @brief Request the particle set. Caller must supply sufficient storage for
the result. */
Modified: code/player/trunk/client_libs/libplayerc++/localizeproxy.cc
===================================================================
--- code/player/trunk/client_libs/libplayerc++/localizeproxy.cc 2010-08-26
22:43:10 UTC (rev 8858)
+++ code/player/trunk/client_libs/libplayerc++/localizeproxy.cc 2010-08-29
00:39:27 UTC (rev 8859)
@@ -85,12 +85,19 @@
std::ostream&
std::operator << (std::ostream &os, const PlayerCc::LocalizeProxy &c)
{
+ double* cov;
os << "#Localize (" << c.GetInterface() << ":" << c.GetIndex() << ")" <<
std::endl;
os << "Hypothesis Count: " << c.GetNumHypoths() << std::endl;
for (unsigned int i = 0; i < c.GetNumHypoths(); ++i)
{
+ cov = c.GetHypoth(i).cov;
os << i << " (weight " << c.GetHypoth(i).alpha << "): ";
- os << "[" << c.GetHypoth(i).mean << "]" << std::endl;
+ os << "[" << c.GetHypoth(i).mean << "]";
+ os << " [cov:";
+ os << " (" << cov[0] << "," << cov[3] << "," << cov[5] << ")";
+ os << " (" << cov[3] << "," << cov[1] << "," << cov[4] << ")";
+ os << " (" << cov[5] << "," << cov[4] << "," << cov[2] << ")";
+ os << "]" << std::endl;
}
return os;
}
Modified: code/player/trunk/libplayerinterface/interfaces/025_localize.def
===================================================================
--- code/player/trunk/libplayerinterface/interfaces/025_localize.def
2010-08-26 22:43:10 UTC (rev 8858)
+++ code/player/trunk/libplayerinterface/interfaces/025_localize.def
2010-08-29 00:39:27 UTC (rev 8859)
@@ -27,8 +27,10 @@
{
/** The mean value of the pose estimate (m, m, rad). */
player_pose2d_t mean;
- /** The covariance matrix pose estimate (m$^2$, rad$^2$). */
- double cov[3];
+ /** The covariance matrix pose estimate (lower half, symmetric matrix)
+ (cov(xx) in m$^2$, cov(yy) in $^2$, cov(aa) in rad$^2$,
+ cov(xy), cov(ya), cov(xa) ). */
+ double cov[6];
/** The weight coefficient for linear combination (alpha) */
double alpha;
} player_localize_hypoth_t;
@@ -57,9 +59,11 @@
{
/** The mean value of the pose estimate (m, m, rad). */
player_pose2d_t mean;
- /** The diagonal elements of the covariance matrix pose estimate
- (m$^2$, rad$^2$). */
- double cov[3];
+ /** The diagonal elements of the covariance matrix pose estimate (variance)
and the
+ (symmetric) covariances between the pose elements .
+ (cov(xx) in m$^2$, cov(yy) in $^2$, cov(aa) in rad$^2$,
+ cov(xy), cov(ya), cov(xa) ). */
+ double cov[6];
} player_localize_set_pose_t;
/** @brief A particle */
Modified: code/player/trunk/server/drivers/localization/amcl/amcl.cc
===================================================================
--- code/player/trunk/server/drivers/localization/amcl/amcl.cc 2010-08-26
22:43:10 UTC (rev 8858)
+++ code/player/trunk/server/drivers/localization/amcl/amcl.cc 2010-08-29
00:39:27 UTC (rev 8859)
@@ -1037,6 +1037,9 @@
data.hypoths[i].cov[0] = pose_cov.m[0][0];
data.hypoths[i].cov[1] = pose_cov.m[1][1];
data.hypoths[i].cov[2] = pose_cov.m[2][2];
+ data.hypoths[i].cov[3] = pose_cov.m[0][1];
+ data.hypoths[i].cov[4] = pose_cov.m[2][1];
+ data.hypoths[i].cov[5] = pose_cov.m[2][0];
}
// sort according to weight
@@ -1104,7 +1107,13 @@
cov = pf_matrix_zero();
cov.m[0][0] = setposereq->cov[0];
+ cov.m[0][1] = setposereq->cov[3];
+ cov.m[0][2] = setposereq->cov[5];
+ cov.m[1][0] = setposereq->cov[3];
cov.m[1][1] = setposereq->cov[1];
+ cov.m[1][2] = setposereq->cov[4];
+ cov.m[2][0] = setposereq->cov[5];
+ cov.m[2][1] = setposereq->cov[4];
cov.m[2][2] = setposereq->cov[2];
// Re-initialize the filter
Modified: code/player/trunk/server/drivers/shell/readlog.cc
===================================================================
--- code/player/trunk/server/drivers/shell/readlog.cc 2010-08-26 22:43:10 UTC
(rev 8858)
+++ code/player/trunk/server/drivers/shell/readlog.cc 2010-08-29 00:39:27 UTC
(rev 8859)
@@ -2554,7 +2554,7 @@
hypoths.hypoths = new
player_localize_hypoth_t[hypoths.hypoths_count];
count = 0;
- for (i = 10; i < token_count; i += 7)
+ for (i = 10; i < token_count; i += 10)
{
hypoths.hypoths[count].mean.px = atof(tokens[i + 0]);
hypoths.hypoths[count].mean.py = atof(tokens[i + 1]);
@@ -2562,7 +2562,10 @@
hypoths.hypoths[count].cov[0] = atof(tokens[i + 3]);
hypoths.hypoths[count].cov[1] = atof(tokens[i + 4]);
hypoths.hypoths[count].cov[2] = atof(tokens[i + 5]);
- hypoths.hypoths[count].alpha = atof(tokens[i + 6]);
+ hypoths.hypoths[count].cov[3] = atof(tokens[i + 6]);
+ hypoths.hypoths[count].cov[4] = atof(tokens[i + 7]);
+ hypoths.hypoths[count].cov[5] = atof(tokens[i + 8]);
+ hypoths.hypoths[count].alpha = atof(tokens[i + 9]);
count += 1;
}
Modified: code/player/trunk/server/drivers/shell/writelog.cc
===================================================================
--- code/player/trunk/server/drivers/shell/writelog.cc 2010-08-26 22:43:10 UTC
(rev 8858)
+++ code/player/trunk/server/drivers/shell/writelog.cc 2010-08-29 00:39:27 UTC
(rev 8859)
@@ -1483,9 +1483,12 @@
- px (float): X coordinate of the mean value of the pose estimate (in m)
- py (float): Y coordinate of the mean value of the pose estimate (in m)
- pa (float): yaw coordinate of the mean value of the pose estimate (in
rad)
- - cov[0] (float): X coordinate of the covariance matrix pose estimate
- - cov[1] (float): Y coordinate of the covariance matrix pose estimate
- - cov[2] (float): yaw coordinate of the covariance matrix pose estimate
+ - cov[0] (float): X Variance of the covariance matrix to the pose estimate
+ - cov[1] (float): Y Variance of the covariance matrix to the pose estimate
+ - cov[2] (float): yaw Variance of the covariance matrix to the pose
estimate
+ - cov[3] (float): X,Y covariance of the pose estimate
+ - cov[4] (float): Y,yaw covariance of the pose estimate
+ - cov[5] (float): X,yaw covariance of the pose estimate
- alpha (float): weight coefficient for linear combination
- 4:2 (PLAYER_LOCALIZE_REQ_GET_PARTICLES) - Current particle set. The format
is:
@@ -1534,6 +1537,9 @@
hypoths->hypoths[i].cov[0],
hypoths->hypoths[i].cov[1],
hypoths->hypoths[i].cov[2],
+ hypoths->hypoths[i].cov[3],
+ hypoths->hypoths[i].cov[4],
+ hypoths->hypoths[i].cov[5],
hypoths->hypoths[i].alpha);
if (write_particles)
// every time we receive localize data also write localize particles
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Sell apps to millions through the Intel(R) Atom(Tm) Developer Program
Be part of this innovative community and reach millions of netbook users
worldwide. Take advantage of special opportunities to increase revenue and
speed time-to-market. Join now, and jumpstart your future.
http://p.sf.net/sfu/intel-atom-d2d
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
