Email Archive: playerstage-commit (read-only)
|
[Playerstage-commit] SF.net SVN: playerstage:[9007]
code/player/trunk/server/drivers/ localization
From: <jpgr87@us...> - 2010-12-12 05:49
|
Revision: 9007
http://playerstage.svn.sourceforge.net/playerstage/?rev=9007&view=rev
Author: jpgr87
Date: 2010-12-12 05:49:13 +0000 (Sun, 12 Dec 2010)
Log Message:
-----------
Applied patch #2948635: Extended Kalman Filter localization with vector map
Modified Paths:
--------------
code/player/trunk/server/drivers/localization/CMakeLists.txt
Added Paths:
-----------
code/player/trunk/server/drivers/localization/ekfvloc/
code/player/trunk/server/drivers/localization/ekfvloc/CMakeLists.txt
code/player/trunk/server/drivers/localization/ekfvloc/ekfvloc_driver.cc
code/player/trunk/server/drivers/localization/ekfvloc/feature.cc
code/player/trunk/server/drivers/localization/ekfvloc/feature.hh
code/player/trunk/server/drivers/localization/ekfvloc/hregions.cc
code/player/trunk/server/drivers/localization/ekfvloc/hregions.hh
code/player/trunk/server/drivers/localization/ekfvloc/localize.cc
code/player/trunk/server/drivers/localization/ekfvloc/localize.hh
code/player/trunk/server/drivers/localization/ekfvloc/matrix.h
code/player/trunk/server/drivers/localization/ekfvloc/params.cc
code/player/trunk/server/drivers/localization/ekfvloc/params.hh
code/player/trunk/server/drivers/localization/ekfvloc/robot_location.cc
code/player/trunk/server/drivers/localization/ekfvloc/robot_location.hh
code/player/trunk/server/drivers/localization/ekfvloc/scan.cc
code/player/trunk/server/drivers/localization/ekfvloc/scan.hh
code/player/trunk/server/drivers/localization/ekfvloc/segment_map.cc
code/player/trunk/server/drivers/localization/ekfvloc/segment_map.hh
code/player/trunk/server/drivers/localization/ekfvloc/sub_opt.cc
code/player/trunk/server/drivers/localization/ekfvloc/sub_opt.hh
code/player/trunk/server/drivers/localization/ekfvloc/transf.cc
code/player/trunk/server/drivers/localization/ekfvloc/transf.hh
code/player/trunk/server/drivers/localization/ekfvloc/types.hh
code/player/trunk/server/drivers/localization/ekfvloc/uloc.cc
code/player/trunk/server/drivers/localization/ekfvloc/uloc.hh
Modified: code/player/trunk/server/drivers/localization/CMakeLists.txt
===================================================================
--- code/player/trunk/server/drivers/localization/CMakeLists.txt 2010-12-11 21:59:16 UTC (rev 9006)
+++ code/player/trunk/server/drivers/localization/CMakeLists.txt 2010-12-12 05:49:13 UTC (rev 9007)
@@ -1,4 +1,5 @@
ADD_SUBDIRECTORY (amcl)
+ADD_SUBDIRECTORY (ekfvloc)
PLAYERDRIVER_OPTION (fakelocalize build_fakelocalize ON)
PLAYERDRIVER_ADD_DRIVER (fakelocalize build_fakelocalize SOURCES fakelocalize.cc)
Added: code/player/trunk/server/drivers/localization/ekfvloc/CMakeLists.txt
===================================================================
--- code/player/trunk/server/drivers/localization/ekfvloc/CMakeLists.txt (rev 0)
+++ code/player/trunk/server/drivers/localization/ekfvloc/CMakeLists.txt 2010-12-12 05:49:13 UTC (rev 9007)
@@ -0,0 +1,30 @@
+PLAYERDRIVER_OPTION(ekfvloc build_ekfvloc ON)
+PLAYERDRIVER_REQUIRE_PKG (ekfvloc build_ekfvloc gsl
+ ekfvloc_includeDirs ekfvloc_libDirs ekfvloc_linkLibs
+ ekfvloc_linkFlags ekfvloc_cFlags)
+
+
+IF (NOT HAVE_STL)
+ PLAYERDRIVER_OPTION (ekfvloc build_ekfvloc OFF "STL not found")
+ELSE (NOT HAVE_STL)
+ PLAYERDRIVER_OPTION (ekfvloc build_ekfvloc ON)
+ENDIF (NOT HAVE_STL)
+
+PLAYERDRIVER_REQUIRE_OS(ekfvloc build_ekfvloc UNIX)
+PLAYERDRIVER_ADD_DRIVER (ekfvloc build_ekfvloc
+ INCLUDEDIRS ${ekfvloc_includeDirs} LIBDIRS ${ekfvloc_libDirs}
+ LINKLIBS ${ekfvloc_linkLibs} LINKFLAGS ${ekfvloc_linkFlags}
+ CFLAGS ${ekfvloc_cFlags}
+ SOURCES
+ ekfvloc_driver.cc
+ feature.cc
+ hregions.cc
+ localize.cc
+ params.cc
+ robot_location.cc
+ scan.cc
+ segment_map.cc
+ sub_opt.cc
+ transf.cc
+ uloc.cc
+)
Added: code/player/trunk/server/drivers/localization/ekfvloc/ekfvloc_driver.cc
===================================================================
--- code/player/trunk/server/drivers/localization/ekfvloc/ekfvloc_driver.cc (rev 0)
+++ code/player/trunk/server/drivers/localization/ekfvloc/ekfvloc_driver.cc 2010-12-12 05:49:13 UTC (rev 9007)
@@ -0,0 +1,1148 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2010
+ * Alejandro R. Mosteo
+ *
+ *
+ * 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
+ */
+
+/** @ingroup drivers */
+/** @{ */
+/** @defgroup driver_ekfvmap ekfvmap
+ * @brief ScanMatching
+
+This driver implements segment-based EKF localization:
+
+"Mobile Robot Localization and Map Building: A Multisensor Fusion Approach"
+J.A. Castellanos and J.D. Tardós
+Kluwer Academic Publishers, Boston, 1999
+ISBN 0-7923-7789-3
+
+@par Compile-time dependencies
+
+- none
+
+@par Provides
+
+- @ref interface_position2d
+ - The pose estimation
+- @ref interface_localize
+ - The full data about the estimation
+- @ref interface_opaque
+ - Publishes a 9-tuple of doubles which is the covariance matrix.
+ - Under the key "covariance"
+
+@par Requires
+
+- @ref interface_position2d : odometry source of pose and velocity information
+- @ref interface_laser : Pose-stamped laser scans (subtype PLAYER_LASER_DATA_SCANPOSE)
+- @ref interface_map : Vector map with segments modelling the environment
+- @ref interface_graphics2d : optional. If provided, it will be used to display debug data
+
+@par Configuration requests
+
+- Position requests are forwarded to the odometry position2 interface.
+ - They are expected to be in the ekfvloc ref. frame, and will be automatically
+ converted to odometry reference.
+- Velocity requests are forwarded to the odometry position2 interface
+
+@par Configuration file options
+
+- max_laser_range (float)
+ - Default: 7.9 m
+ - Maximum laser range.
+
+- laser_noise (tuple (doubles): [m rad])
+ - Default: [0.004 0.045]
+ - Noise in laser range/bearing readings; default is for a SICK LMS200
+
+- odom_noise (tuple (doubles): [m m rad])
+ - Default: [0.4 0.2 0.2]
+ - Noise in motion model
+ - This is, in terms of covariance, 2*sigma
+
+- robot_pose (tuple (doubles): [m m rad])
+ - Default: [0.0 0.0 0.0]
+ - Initial pose estimation
+
+- robot_pose_initial_error (tuple (doubles): [m m rad])
+ - Default: [1.0 1.0 0.2]
+ - Initial error the initial robot pose. It EX = 1.0, robot is at X +/- (E/2)
+
+- mapfile (string)
+ - Default: none
+ - Parameter that allows the loading of a mapfile instead of using
+ a map interface. Mapfile formap is #segments [x0 y0 x1 y1]...
+ - When *not* using this but a requires, use alwayson 1 in the vmapfile driver
+ - if you get errors initializing this driver
+
+- truth_model (string)
+ - Default : none
+ - Named model in stage for ground truth. Used for debugging.
+ - When using this, also use a "simulation" requires
+
+- FINE TUNING OPTIONS (units (default))
+ - Note that player unit conversions are applied, so these units are only informative for the defaults
+
+- max_region_empty_angle (rad (0.035 ~ 2deg))
+ - Angular distance to split regions
+
+- max_region_empty_distance (m (0.1))
+ - Euclidean distance to split regions
+
+- min_region_length (m (0.2))
+ - Minimum distance between region endpoints
+
+- min_points_in_region (int (4))
+ - Minimum laser returns in a region
+
+- split_confidence (percent (95.0))
+ - chi-2 confidence test for segment splitting
+
+- check_residual (boolean_int (0))
+ - perform residual testing for segment splitting
+
+- max_ang_ebe (rad (0.0))
+ - ???
+
+- min_split_segment_distance (m (0.0))
+ - minimum dist between new segment endpoints for segment splitting
+
+- min_odom_distance_delta (m (0.0))
+- min_odom_angle_delta (rad (0.0))
+ - minimum change in odometry before performing update
+
+- backoff_period (s (0.05))
+ - minimum time between updates; if lasers arrive faster they'll be dropped
+
+- DEBUG OPTIONS
+ - send_debug (int (0))
+ - port for custom socket connection to an external debugging GUI
+ - If != 0 it will be used.
+
+@par Example
+
+@verbatim
+# Using degrees
+driver
+(
+ name "ekfvmap"
+ provides ["position2d:1" "localize:0" "covariance:::0"]
+ requires ["position2d:0" "laser:0" "map:0"]
+
+ max_laser_range 31.9
+ laser_noise [0.004 0.5]
+ odom_noise [0.4 0.2 10]
+ robot_pose [-49 -39 0]
+ robot_pose_initial_error [1.0 1.0 20]
+ # mapfile "upc.vec"
+ # Use either "requires" or "mapfile"
+
+ # truth_model "robot"
+)
+
+@endverbatim
+
+@author J.A. Castellanos, J.D. Tardós (underlying algorithm), Mayte Lázaro (code), A. Mosteo (player integration)
+*/
+/** @} */
+
+#include <arpa/inet.h>
+#include <errno.h>
+#include <math.h>
+#include <netinet/in.h>
+#include <string>
+#include <sys/socket.h>
+#include <sys/time.h>
+#include <sys/types.h>
+
+#include <libplayercore/device.h>
+#include <libplayercore/driver.h>
+#include <libplayercore/playercore.h>
+
+#include "localize.hh"
+#include "params.hh"
+#include "types.hh"
+
+const bool kThreaded = true;
+
+GuiData GUI_DATA;
+
+class Ekfvloc : public ThreadedDriver
+{
+
+public:
+
+ Ekfvloc(ConfigFile* cf, int section);
+ virtual ~Ekfvloc();
+
+ virtual int MainSetup();
+ virtual void MainQuit();
+
+ virtual int ProcessMessage(QueuePointer &resp_queue,
+ player_msghdr * hdr,
+ void * data);
+private:
+ Localize localize_;
+ string mapfile_;
+
+ player_devaddr_t odom_addr_;
+ player_devaddr_t laser_addr_;
+ player_devaddr_t map_addr_;
+ player_devaddr_t sim_addr_;
+ player_devaddr_t p2d_addr_; // Position address
+ player_devaddr_t loc_addr_; // Localization address
+ player_devaddr_t cov_opaque_addr_;
+ player_devaddr_t g2d_addr_;
+
+ Device *odom_;
+ Device *laser_;
+ Device *map_;
+ Device *sim_;
+ Device *g2d_;
+
+ player_position2d_data position_; // Odometry pose
+ bool have_pose_;
+
+ Pose global_initial_pose_; // Given initial pose
+
+// Transf odom_base_ref_;
+// Transf global_base_ref_;
+
+ int scan_count_;
+
+ struct timeval prev_scan_timestamp_;
+
+ std::string sim_model_;
+ Pose sim_pose_; // Ground truth if simulating
+
+ int debug_sock_;
+
+ bool publish_cov_;
+ bool use_g2d_;
+
+ std::vector<double> cov_pub_; // published covariance via opaque
+
+ Transf laser_pose_;
+
+ double laser_gap_; // angular gap in laser scans
+
+ // Main function for device thread.
+ virtual void Main();
+
+ // Say if we should process another scan, to avoid excessive CPU usage
+ bool CheckElapsed(void);
+
+ Pose GroundTruth(void);
+
+ void PublishInterfaces(double timestamp); // Publish all output interfaces
+
+ bool PrepareDebug(int port); // False on failure
+ void SendDebug(const GuiData &gui_data) const;
+ void DrawDebug(const GuiData &gui_data);
+ void DrawClear(void);
+ void DrawLine(player_color_t color, player_point_2d_t ini, player_point_2d_t fin);
+ void DrawLaser(player_color_t color,
+ double rho0, double phi0,
+ double rho1, double phi1); // Take laser coords and draw in robot frame
+ void DrawEllipse(void);
+};
+
+////////////////////////////////////////////////////////////////////////////////
+Driver* ekfvloc_Init(ConfigFile* cf, int section)
+{
+ return static_cast<Driver*>(new Ekfvloc(cf, section));
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+void ekfvloc_Register(DriverTable* table)
+{
+ table->AddDriver("ekfvloc", ekfvloc_Init);
+}
+
+bool Ekfvloc::CheckElapsed(void)
+{
+
+ struct timeval now;
+
+ if (gettimeofday(&now, NULL))
+ {
+ PLAYER_ERROR1("Ekfvloc:CheckElapsed: %s", strerror(errno));
+ return true;
+ }
+
+ const uint64_t elapsed =
+ (now.tv_sec - prev_scan_timestamp_.tv_sec) * 1000 +
+ (now.tv_usec - prev_scan_timestamp_.tv_usec) / 1000;
+
+ PLAYER_MSG1(5, "Ekfvloc::CheckElapsed: %u elapsed", elapsed);
+
+ prev_scan_timestamp_ = now;
+
+ return elapsed >= static_cast<uint64_t>(kMinMillisBetweenScans);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+int Ekfvloc::MainSetup()
+{
+ if (mapfile_.length() > 0)
+ {
+ try
+ {
+ localize_.LoadMap(mapfile_.c_str());
+ printf("Map loaded from %s\n", mapfile_.c_str());
+ }
+ catch (exception &e)
+ {
+ PLAYER_ERROR1("File could not be loaded: %s", e.what());
+ }
+ }
+ else
+ {
+ if (!(map_ = deviceTable->GetDevice(map_addr_)))
+ {
+ PLAYER_ERROR("Unable to locate map device at given address");
+ return -1;
+ }
+
+ if (map_->Subscribe(InQueue) != 0)
+ {
+ PLAYER_ERROR("Unable to subscribe to map interface");
+ return -1;
+ }
+
+ Message *msg = map_->Request(InQueue, PLAYER_MSGTYPE_REQ, PLAYER_MAP_REQ_GET_VECTOR, NULL, 0, NULL, kThreaded);
+
+ if (msg == NULL)
+ {
+ PLAYER_ERROR("Unable to request map");
+ return -1;
+ }
+ else
+ {
+ const player_map_data_vector_t &map = *static_cast<player_map_data_vector_t *>(msg->GetPayload());
+ SegmentsVector segments;
+
+ for (uint32_t i = 0; i < map.segments_count; i++)
+ {
+ segments.push_back(Segment(
+ map.segments[i].x0,
+ map.segments[i].y0,
+ map.segments[i].x1,
+ map.segments[i].y1));
+ }
+
+ localize_.SetMap(segments);
+
+ delete msg;
+ }
+ }
+
+ if (!(odom_ = deviceTable->GetDevice(odom_addr_)))
+ {
+ PLAYER_ERROR("Unable to locate position device at given address");
+ return -1;
+ }
+
+ if (odom_->Subscribe(InQueue) != 0)
+ {
+ PLAYER_ERROR("Unable to subscribe to position device");
+ return -1;
+ }
+
+ if (!(laser_ = deviceTable->GetDevice(laser_addr_)))
+ {
+ PLAYER_ERROR("Unable to locate laser device at given address");
+ return -1;
+ }
+
+ if (laser_->Subscribe(InQueue) != 0)
+ {
+ PLAYER_ERROR("Unable to subscribe to laser device");
+ return -1;
+ }
+
+ if (use_g2d_)
+ {
+ if (!(g2d_ = deviceTable->GetDevice(g2d_addr_)))
+ {
+ PLAYER_ERROR("Unable to locate graphics2d device");
+ return -1;
+ }
+ if (g2d_->Subscribe(InQueue) != 0)
+ {
+ PLAYER_ERROR("Unable to subscribe to graphics2d device");
+ return -1;
+ }
+ }
+
+ Message *msg = laser_->Request(InQueue, PLAYER_MSGTYPE_REQ, PLAYER_LASER_REQ_GET_GEOM, NULL, 0, NULL);
+
+ if (msg)
+ {
+ const player_laser_geom_t &laser_geom = *static_cast<player_laser_geom_t *>(msg->GetPayload());
+
+ PLAYER_MSG3(0, "Ekfvloc: Reported laser pose: %8.3f %8.3f %8.3f",
+ laser_geom.pose.px, laser_geom.pose.py, laser_geom.pose.pyaw);
+
+ localize_.SetLaserPose(
+ laser_geom.pose.px,
+ laser_geom.pose.py,
+ laser_geom.pose.pyaw);
+
+ laser_pose_ = Transf(laser_geom.pose.px, laser_geom.pose.py, laser_geom.pose.pyaw);
+ }
+ else
+ {
+ PLAYER_WARN("Laser didn't provide its pose!");
+ laser_pose_ = Transf(0, 0, 0);
+ }
+
+ delete msg;
+
+ return 0;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void Ekfvloc::MainQuit()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+Ekfvloc::Ekfvloc(ConfigFile* cf, int section)
+ : ThreadedDriver(cf, section),
+ localize_(cf->ReadLength(section, "max_laser_range", 7.9),
+ 0.0, 0.0, 0.0, // Laser pose is corrected via GEOM request
+ cf->ReadTupleLength(section, "laser_noise", 0, 0.045),
+ cf->ReadTupleAngle(section, "laser_noise", 1, 0.004),
+ cf->ReadTupleLength(section, "odom_noise", 0, 0.4),
+ cf->ReadTupleLength(section, "odom_noise", 1, 0.2),
+ cf->ReadTupleAngle(section, "odom_noise", 2, 0.2)),
+ mapfile_(cf->ReadString(section, "mapfile", "")),
+ have_pose_(false),
+ global_initial_pose_(0, 0, 0),
+// global_base_ref_(cf->ReadTupleLength(section, "robot_pose", 0, 0.0),
+// cf->ReadTupleLength(section, "robot_pose", 1, 0.0),
+// cf->ReadTupleAngle(section, "robot_pose", 2, 0.0)),
+ scan_count_(0),
+ sim_pose_(0.0, 0.0, 0.0),
+ debug_sock_(-1),
+ publish_cov_(false),
+ use_g2d_(false)
+{
+ odom_ = laser_ = NULL;
+ prev_scan_timestamp_.tv_sec = 0;
+
+ global_initial_pose_ = Pose (cf->ReadTupleLength(section, "robot_pose", 0, 0.0),
+ cf->ReadTupleLength(section, "robot_pose", 1, 0.0),
+ cf->ReadTupleAngle(section, "robot_pose", 2, 0.0));
+
+// This was very wrong for odometries not starting at zero
+// localize_.SetRobotPose(cf->ReadTupleLength(section, "robot_pose", 0, 0.0),
+// cf->ReadTupleLength(section, "robot_pose", 1, 0.0),
+// cf->ReadTupleAngle(section, "robot_pose", 2, 0.0));
+
+ localize_.SetRobotPoseError(
+ cf->ReadTupleLength(section, "robot_pose_initial_error", 0, 1.0),
+ cf->ReadTupleLength(section, "robot_pose_initial_error", 1, 1.0),
+ cf->ReadTupleAngle(section, "robot_pose_initial_error", 2, 0.2));
+
+ // Device basic checking
+
+ if (cf->ReadDeviceAddr(&odom_addr_, section, "requires",
+ PLAYER_POSITION2D_CODE, -1, NULL))
+ {
+ PLAYER_ERROR("Missing address of position required interface");
+ SetError(-1);
+ return;
+ }
+
+ if (cf->ReadDeviceAddr(&laser_addr_, section, "requires",
+ PLAYER_LASER_CODE, -1, NULL))
+ {
+ PLAYER_ERROR("Missing address of laser required interface");
+ SetError(-1);
+ return;
+ }
+
+ if (mapfile_.empty())
+ if (cf->ReadDeviceAddr(&map_addr_, section, "requires",
+ PLAYER_MAP_CODE, -1, NULL))
+ {
+ PLAYER_ERROR("Missing address of map required interface");
+ SetError(-1);
+ return;
+ }
+
+ if (cf->ReadDeviceAddr(&p2d_addr_, section, "provides",
+ PLAYER_POSITION2D_CODE, -1, NULL))
+ {
+ PLAYER_ERROR("Missing address of position provided interface");
+ SetError(-1);
+ return;
+ }
+ else
+ {
+ if (AddInterface(p2d_addr_) != 0)
+ {
+ PLAYER_ERROR("Cannot add position2d interface");
+ SetError(-1);
+ return;
+ }
+ }
+
+ if (cf->ReadDeviceAddr(&loc_addr_, section, "provides",
+ PLAYER_LOCALIZE_CODE, -1, NULL))
+ {
+ PLAYER_ERROR("Missing address of localize provided interface");
+ SetError(-1);
+ return;
+ }
+ else
+ {
+ if (AddInterface(loc_addr_) != 0)
+ {
+ PLAYER_ERROR("Cannot add localize interface");
+ SetError(-1);
+ return;
+ }
+ }
+
+ if (cf->ReadDeviceAddr(&cov_opaque_addr_, section, "provides",
+ PLAYER_OPAQUE_CODE, -1, "covariance") == 0)
+ {
+ if (AddInterface(cov_opaque_addr_) != 0)
+ {
+ PLAYER_ERROR("Cannot add cov opaque interface");
+ SetError(-1);
+ return;
+ }
+ else
+ printf("Ekfvloc: using opaque interface for covariance\n");
+ }
+ else
+ {
+ publish_cov_ = false;
+ }
+
+ if (!(cf->ReadDeviceAddr(&sim_addr_, section, "requires", PLAYER_SIMULATION_CODE, -1, NULL)))
+ {
+ if ((sim_ = deviceTable->GetDevice(sim_addr_)))
+ {
+ sim_model_ = std::string(cf->ReadString(section, "truth_model", "missing!"));
+ printf("Ekfvloc: using simulation model [%s] for ground truth\n", sim_model_.c_str());
+ }
+ }
+ else
+ sim_ = NULL;
+
+ if (cf->ReadDeviceAddr(&g2d_addr_, section, "requires",
+ PLAYER_GRAPHICS2D_CODE, -1, NULL) == 0)
+ {
+ printf("Ekfvloc: using graphics2d interface for display\n");
+ use_g2d_ = true;
+ }
+
+ if (cf->ReadInt(section, "send_debug", 0) != 0)
+ if (!PrepareDebug(cf->ReadInt(section, "send_debug", 0)))
+ {
+ PLAYER_ERROR("Cannot connect to remote debugger");
+ SetError(-1);
+ return;
+ }
+
+ // Rest of initialization at MainSetup, when first client connects
+
+ // PARAMETERS FINE TUNING
+ kMaxEmptyAngle = cf->ReadAngle(section, "max_region_empty_angle", kMaxEmptyAngle);
+ kMaxEmptyDistance = cf->ReadLength(section, "max_region_empty_distance", kMaxEmptyDistance);
+ kMinRegionLength = cf->ReadLength(section, "min_region_length", kMinRegionLength);
+ kMinPointsInRegion = cf->ReadInt(section, "min_points_in_region", kMinPointsInRegion);
+ kMinPointsInSegment = cf->ReadInt(section, "min_points_in_segment", kMinPointsInSegment);
+ kConfidence = cf->ReadFloat(section, "split_confidence", kConfidence);
+ kCheckResidual = (cf->ReadInt(section, "check_residual", kCheckResidual ? 1 : 0) != 0);
+ kMaxAngEBE = cf->ReadAngle(section, "max_ang_ebe", kMaxAngEBE);
+ kMinDistBetweenEndpoints = cf->ReadLength(section, "min_split_segment_distance", kMinDistBetweenEndpoints);
+ kMinOdomDistChange = cf->ReadLength(section, "min_odom_distance_delta", kMinOdomDistChange);
+ kMinOdomAngChange = cf->ReadAngle(section, "min_odom_angle_delta", kMinOdomAngChange);
+ kMinMillisBetweenScans = static_cast<long>(cf->ReadFloat(section, "backoff_period", kMinMillisBetweenScans / 1000.0) * 1000.0);
+
+ PLAYER_MSG2(1, "Ekfvloc: %30s: %8.3f", "max_region_empty_angle", kMaxEmptyAngle);
+ PLAYER_MSG2(1, "Ekfvloc: %30s: %8.3f", "max_region_empty_distance", kMaxEmptyDistance);
+ PLAYER_MSG2(1, "Ekfvloc: %30s: %8.3f", "min_region_length", kMinRegionLength);
+ PLAYER_MSG2(1, "Ekfvloc: %30s: %8d", "min_points_in_region", kMinPointsInRegion);
+ PLAYER_MSG2(1, "Ekfvloc: %30s: %8d", "min_points_in_segment", kMinPointsInSegment);
+ PLAYER_MSG2(1, "Ekfvloc: %30s: %8.3f", "split_confidence", kConfidence);
+ PLAYER_MSG2(1, "Ekfvloc: %30s: %s", "check_residual", kCheckResidual ? "true" : "false");
+ PLAYER_MSG2(1, "Ekfvloc: %30s: %8.3f", "max_ang_ebe", kMaxAngEBE);
+ PLAYER_MSG2(1, "Ekfvloc: %30s: %8.3f", "min_split_segment_distance", kMinDistBetweenEndpoints);
+ PLAYER_MSG2(1, "Ekfvloc: %30s: %8.3f", "min_odom_distance_delta", kMinOdomDistChange);
+ PLAYER_MSG2(1, "Ekfvloc: %30s: %8.3f", "min_odom_angle_delta", kMinOdomAngChange);
+ PLAYER_MSG2(1, "Ekfvloc: %30s: %8.3f", "backoff_period(ms)", kMinMillisBetweenScans);
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+Ekfvloc::~Ekfvloc()
+{
+}
+
+bool Ekfvloc::PrepareDebug(int port)
+{
+ if ((debug_sock_ = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP)) == -1)
+ {
+ perror("Can't create gui socket");
+ return false;
+ }
+
+ struct sockaddr_in localhost;
+ memset (&localhost, 0, sizeof(localhost));
+ localhost.sin_family = AF_INET;
+ localhost.sin_port = htons(port);
+ if (inet_aton("127.0.0.1", &localhost.sin_addr) == 0)
+ {
+ perror("Can't set localhost address");
+ return false;
+ }
+
+ PLAYER_MSG2(1, "Ekfvloc: Connecting to %s:%d...\n", inet_ntoa(localhost.sin_addr), ntohs(localhost.sin_port));
+ if (connect(debug_sock_, (sockaddr*)&localhost, sizeof(localhost)) != 0)
+ {
+ perror("Can't connect to gui listener");
+ return false;
+ }
+ PLAYER_MSG0(1, "Ekfvloc: Connected.");
+
+ return true;
+}
+
+void Ekfvloc::SendDebug(const GuiData &gui_data) const
+{
+ int i, j;
+
+ {
+ const int nregs = gui_data.regions.size();
+ std::vector<double> regs;
+
+ for (j = 0, i = 0; i < nregs; i++)
+ {
+ regs.push_back(gui_data.regions.at(i).rho0());
+ regs.push_back(gui_data.regions.at(i).phi0());
+ regs.push_back(gui_data.regions.at(i).rho1());
+ regs.push_back(gui_data.regions.at(i).phi1());
+ }
+ // regions
+ if (write(debug_sock_, (void*)&nregs, sizeof(int)) == -1)
+ perror("Ekfvloc error sending gui region count");
+ if (write(debug_sock_, (void*)®s[0], nregs * 4 * sizeof(double)) == -1)
+ perror("Ekfvloc error sending gui regions");
+ }
+
+ {
+ const int nsplt = gui_data.splits.size();
+ std::vector<double> splt;
+
+ for (j = 0, i = 0; i < nsplt; i++)
+ {
+ splt.push_back(gui_data.splits.at(i).rho0());
+ splt.push_back(gui_data.splits.at(i).phi0());
+ splt.push_back(gui_data.splits.at(i).rho1());
+ splt.push_back(gui_data.splits.at(i).phi1());
+ }
+ // splits
+ if (write(debug_sock_, (void*)&nsplt, sizeof(int)) == -1)
+ perror("Ekfvloc error sending gui split count");
+ if (write(debug_sock_, (void*)&splt[0], nsplt * 4 * sizeof(double)) == -1)
+ perror("Ekfvloc error sending gui splits");
+ }
+
+ {
+ const int nmtch = gui_data.matches.size();
+ std::vector<double> mtch;
+
+ for (j = 0, i = 0; i < nmtch; i++)
+ {
+ mtch.push_back(gui_data.matches.at(i).rho0());
+ mtch.push_back(gui_data.matches.at(i).phi0());
+ mtch.push_back(gui_data.matches.at(i).rho1());
+ mtch.push_back(gui_data.matches.at(i).phi1());
+ }
+ // matches
+ if (write(debug_sock_, (void*)&nmtch, sizeof(int)) == -1)
+ perror("Ekfvloc error sending gui match count");
+ if (write(debug_sock_, (void*)&mtch[0], nmtch * 4 * sizeof(double)) == -1)
+ perror("Ekfvloc error sending gui matches");
+ }
+
+ // Covs
+ for (i = 0; i < 3; i++)
+ for (j = 0; j < 3; j++)
+ {
+ double x = localize_.GetCovariance()(i, j);
+ if (write(debug_sock_, (void*)&x, sizeof(x)) == -1)
+ {
+ perror("Error sending covariance");
+ printf("Ekfvloc error sending gui Cov(%d, %d)", i, j);
+ }
+ }
+}
+
+void Ekfvloc::DrawClear(void)
+{
+ g2d_->PutMsg(InQueue, PLAYER_MSGTYPE_CMD, PLAYER_GRAPHICS2D_CMD_CLEAR,
+ static_cast<void*>(NULL), 0.0, NULL);
+}
+
+void Ekfvloc::DrawLine(player_color_t color, player_point_2d_t ini, player_point_2d_t fin)
+{
+ player_point_2d_t points[2] = {ini, fin};
+
+ player_graphics2d_cmd_polyline_t line = { 2, points, color };
+
+ // We use a request to ensure that &points remains valid until return
+ // Message *msg =
+ g2d_->PutMsg(InQueue, PLAYER_MSGTYPE_CMD, PLAYER_GRAPHICS2D_CMD_POLYLINE,
+ static_cast<void*>(&line), 0.0, NULL);
+}
+
+void Ekfvloc::DrawLaser(player_color_t color,
+ double rho0, double phi0,
+ double rho1, double phi1)
+{
+ const Transf tpini(rho0 * cos(phi0), rho0 * sin(phi0), 0.0);
+ const Transf tpfin(rho1 * cos(phi1), rho1 * sin(phi1), 0.0);
+ const Transf tini(Compose(laser_pose_, tpini));
+ const Transf tfin(Compose(laser_pose_, tpfin));
+ const player_point_2d_t ini = {tini.tX(), tini.tY()};
+ const player_point_2d_t fin = {tfin.tX(), tfin.tY()};
+
+ DrawLine(color, ini, fin);
+}
+
+void Ekfvloc::DrawEllipse(void)
+{
+ const double k = 5.99; // 95% confidence.
+ const int points = 20; // segments in ellipse
+ const player_color_t black = {0, 0, 0, 0};
+ const player_color_t blue = {0, 0, 0, 255};
+
+ const Matrix cov = localize_.GetCovariance();
+ Matrix sigma(2, 2);
+
+ for (int r = 0; r < 2; r++)
+ for (int c = 0; c < 2; c++)
+ sigma(r, c) = cov(r, c);
+
+ sigma *= k; // Matrix * scalar
+
+ Matrix D, V; // eigenvalues & eigenvectors
+ Eigenv(sigma, &V, &D);
+
+ Matrix Dsqrt = D; // Keep sqrt(D) for reuse
+ for (int r = 0; r < 2; r++)
+ for (int c = 0; c < 2; c++)
+ Dsqrt(r, c) = sqrt(Dsqrt(r, c));
+
+ Matrix U(2, points); // 2 x points, to approximate the ellipse
+ for (int c = 0; c < points; c++)
+ {
+ U(0, c) = cos(2.0 * M_PI / points * c);
+ U(1, c) = sin(2.0 * M_PI / points * c);
+ }
+
+ const Matrix W = (V * Dsqrt) * U; // ((2x2)*(2x2))*(2xn) = (2xn)
+ // This does the ellipse approximation somehow
+
+ // Obtain relative position if we have ground truth:
+ const Pose truth = GroundTruth();
+ const Transf ttruth = Transf(truth.x, truth.y, truth.th);
+ const Pose local = localize_.pose();
+ const Transf tlocal = Transf(local.x, local.y, local.th);
+
+ const Transf nu = (sim_ ? TRel(ttruth, tlocal) : Transf(0, 0, 0));
+
+ for (int c1 = 0; c1 < points; c1++)
+ {
+ const int c2 = (c1 + 1 == points ? 0 : c1 + 1);
+
+ const player_point_2d_t p1 = { W(0, c1) + nu.tX(), W(1, c1) + nu.tY()};
+ const player_point_2d_t p2 = { W(0, c2) + nu.tX(), W(1, c2) + nu.tY()};
+ DrawLine(black, p1, p2);
+ }
+
+ // 2 points for angular incertitude
+ {
+ Matrix Z(2, 2);
+ Z(0, 0) = Z(0, 1) = cos(3.84 * 2.0 * sqrt(cov(2, 2)));
+ Z(1, 0) = sin(3.84 * 2.0 * sqrt(cov(2, 2)));
+ Z(1, 1) = -Z(1, 0);
+ const Matrix WZ = V * Z; // Apply orientation somehow, but don't scale to ellipse
+ // This does the ellipse approximation somehow
+ const player_point_2d_t ow = { nu.tX(), nu.tY() };
+ const player_point_2d_t p1 = { WZ(0, 0) + nu.tX(), WZ(1, 0) + nu.tY()};
+ const player_point_2d_t p2 = { WZ(0, 1) + nu.tX(), WZ(1, 1) + nu.tY()};
+ DrawLine(blue, ow, p1);
+ DrawLine(blue, ow, p2);
+ }
+
+ // Axes
+ {
+ const player_point_2d_t a1 = { nu.tX() - Dsqrt(0, 0) * V(0, 0), nu.tY() - Dsqrt(0, 0) * V(1, 0) };
+ const player_point_2d_t a2 = { nu.tX() + Dsqrt(0, 0) * V(0, 0), nu.tY() + Dsqrt(0, 0) * V(1, 0) };
+ const player_point_2d_t a3 = { nu.tX() - Dsqrt(1, 1) * V(0, 1), nu.tY() - Dsqrt(1, 1) * V(1, 1) };
+ const player_point_2d_t a4 = { nu.tX() + Dsqrt(1, 1) * V(0, 1), nu.tY() + Dsqrt(1, 1) * V(1, 1) };
+ DrawLine(black, a1, a2);
+ DrawLine(black, a3, a4);
+ }
+}
+
+void Ekfvloc::DrawDebug(const GuiData &gui_data)
+{
+ const GuiData &gd = gui_data;
+
+ const player_color_t blue = { 0, 0, 0, 255 };
+
+ DrawClear();
+
+ // Draw laser scans
+ {
+ const double semigap = laser_gap_ / 2.0;
+ for (size_t i = 1; i < gd.laser_rho.size(); i++)
+ {
+ const double r = gd.laser_rho[i];
+ const double p = gd.laser_phi[i];
+ DrawLaser(blue, r, p - semigap, r, p + semigap);
+ }
+ }
+
+ {
+ for (size_t i = 0; i < gui_data.regions.size(); i++)
+ {
+ player_color_t color = {0, 168, 168, 168};
+ DrawLaser(color, 0.0, 0.0, gd.regions[i].rho0(), gd.regions[i].phi0());
+ DrawLaser(color, 0.0, 0.0, gd.regions[i].rho1(), gd.regions[i].phi1());
+ }
+ }
+
+ {
+ for (size_t i = 0; i < gui_data.splits.size(); i++)
+ {
+ player_color_t color = {0, 0, 200, 0};
+ DrawLaser(color,
+ 0.5 * gd.splits[i].rho0(), gd.splits[i].phi0(),
+ gd.splits[i].rho0(), gd.splits[i].phi0());
+ DrawLaser(color,
+ 0.5 * gd.splits[i].rho1(), gd.splits[i].phi1(),
+ gd.splits[i].rho1(), gd.splits[i].phi1());
+// DrawLaser(color,
+// gd.splits[i].rho0(), gd.splits[i].phi0(),
+// gd.splits[i].rho1(), gd.splits[i].phi1());
+ }
+ }
+
+ {
+ // Use as reference for the feat either our estimation or ground truth if available
+ const Pose estim = localize_.pose();
+ const Transf pose(estim.x, estim.y, estim.th);
+ const Pose truth = GroundTruth();
+ const Transf ttruth = Transf(truth.x, truth.y, truth.th);
+ const Transf base = (sim_ ? ttruth : pose);
+
+ for (size_t i = 0; i < gui_data.matches.size(); i++)
+ {
+ player_color_t color_laser = {0, 255, 0, 0};
+ const GuiSplit &m = gd.matches[i]; // current match
+ DrawLaser(color_laser,
+ m.rho0(), m.phi0(),
+ m.rho1(), m.phi1());
+
+ const double err = gd.mahala[i];
+ const Transf cross(0, 0.5 * err, 0), crossn(0, -0.5 * err, 0);
+ const Transf tpini(m.rho0() * cos(m.phi0()), m.rho0() * sin(m.phi0()), 0.0);
+ const Transf tpfin(m.rho1() * cos(m.phi1()), m.rho1() * sin(m.phi1()), 0.0);
+ const Transf tini(Compose(laser_pose_, tpini));
+ const Transf tfin(Compose(laser_pose_, tpfin));
+ const Transf tobs((tini.x() + tfin.x()) / 2,
+ (tini.y() + tfin.y()) / 2,
+ atan2(tfin.y() - tini.y(), tfin.x() - tini.x()));
+
+ // Draw error
+ const Transf c1 = Compose(tobs, cross);
+ const Transf c2 = Compose(tobs, crossn);
+ const player_point_2d_t p1 = {c1.tX(), c1.tY()};
+ const player_point_2d_t p2 = {c2.tX(), c2.tY()};
+ DrawLine(color_laser, p1, p2);
+ }
+ }
+
+ DrawEllipse();
+
+ GUI_DATA.Clear();
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void Ekfvloc::Main()
+{
+ while (true)
+ {
+ // Wait till we get new data
+ Wait();
+
+ // Test if we are supposed to cancel this thread.
+ pthread_testcancel();
+
+ // Process any pending requests.
+ ProcessMessages(0);
+ }
+}
+
+Pose Ekfvloc::GroundTruth(void)
+{
+ // For some **** reason the simulation driver doesn't track subscriptions
+ // and the player request setup doesn't work. So we have to do it the
+ // ugly way ourselves.
+ // That is, we wait for the reply in the ProcessMessage side of things.
+
+ if (!sim_)
+ return Pose(0.0, 0.0, 0.0);
+
+ player_simulation_pose2d_req_t req =
+ { sim_model_.size() + 1, const_cast<char*>(sim_model_.c_str())};
+
+ sim_->PutMsg(InQueue, PLAYER_MSGTYPE_REQ, PLAYER_SIMULATION_REQ_GET_POSE2D,
+ static_cast<void*>(&req), -1, NULL);
+
+ return sim_pose_;
+}
+
+void Ekfvloc::PublishInterfaces(double timestamp)
+{
+ // Publish updated pose
+ const Pose new_pose = localize_.pose();
+ player_position2d_data_t publish_pose =
+ { { new_pose.x, new_pose.y, new_pose.th },
+ position_.vel,
+ position_.stall
+ };
+
+ Publish(p2d_addr_, PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE,
+ static_cast<void*>(&publish_pose));
+ PLAYER_MSG4(3, "Ekfvloc loclz pose: %8.3f %8.3f %8.3f (%d)", new_pose.x, new_pose.y, new_pose.th, scan_count_);
+
+ // Publish localize data
+ const Matrix cov = localize_.GetCovariance();
+ player_localize_hypoth_t hyp[1] =
+ {{{new_pose.x, new_pose.y, new_pose.th}, {cov(0, 0), cov(1, 1), cov(2, 2)}, 1.0}};
+ // {{{new_pose.x, new_pose.y, new_pose.th}, {cov(0, 0), cov(1, 1), cov(2, 2),
+ // cov(0, 1), cov(1, 2), cov(0,2) }, 1.0}};
+ // This was contributed as a patch for the extended localization interface,
+ // but I though this had dissapeared at some point (2.x -> 3.x ?)
+ // So here it is, but commented.
+
+ player_localize_data_t loc_data = { 0, timestamp, 1, hyp};
+ Publish(loc_addr_, PLAYER_MSGTYPE_DATA, PLAYER_LOCALIZE_DATA_HYPOTHS,
+ static_cast<void*>(&loc_data));
+
+ // Publish full covariance via opaque
+ if (publish_cov_)
+ {
+ cov_pub_.clear();
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ cov_pub_.push_back(cov(i, j));
+
+ player_opaque_data_t data = { 9 * sizeof(double),
+ reinterpret_cast<uint8_t*>(&cov_pub_[0]) };
+
+
+ Publish(cov_opaque_addr_, PLAYER_MSGTYPE_DATA, PLAYER_OPAQUE_DATA_STATE,
+ static_cast<void*>(&data));
+ }
+
+ // Check error if simulating
+ if (sim_)
+ {
+ const Pose truth(GroundTruth());
+ const Transf error(TRel(Transf(truth.x, truth.y, truth.th),
+ Transf(new_pose.x, new_pose.y, new_pose.th)));
+ PLAYER_MSG3(2, "Ekfvloc: Error: %8.3f %8.3f %8.3f", error.tX(), error.tY(), error.tPhi());
+
+ if (error.Distance(Transf(0.0, 0.0, 0.0)) >= kTruthWarnDistance)
+ PLAYER_WARN3("Ekfvloc: Error: %8.3f %8.3f %8.3f", error.tX(), error.tY(), error.tPhi());
+ }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+int Ekfvloc::ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data)
+{
+ if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE, odom_addr_))
+ {
+ const player_position2d_data_t &p2d = *static_cast<player_position2d_data_t *>(data);
+ position_ = p2d;
+ PLAYER_MSG3(3, "Ekfvloc odomz pose: %8.3f %8.3f %8.3f", p2d.pos.px, p2d.pos.py, p2d.pos.pa);
+
+ if (!have_pose_)
+ {
+ have_pose_ = true;
+ localize_.SetPoses (p2d.pos.px, p2d.pos.py, p2d.pos.pa,
+ global_initial_pose_.x, global_initial_pose_.y, global_initial_pose_.th);
+ }
+
+ const Pose sim_pose = GroundTruth();
+ if ((sim_pose.x != 0) || (sim_pose.y != 0) || (sim_pose.th != 0))
+ PLAYER_MSG3(3, "Ekfvloc truth pose: %8.3f %8.3f %8.3f", sim_pose.x, sim_pose.y, sim_pose.th);
+
+ return 0;
+ }
+
+ if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA, PLAYER_LASER_DATA_SCAN, laser_addr_))
+ {
+ const player_laser_data_t &scan = *static_cast<player_laser_data_t *>(data);
+
+ if (!CheckElapsed())
+ {
+ PLAYER_WARN("Ekfvloc: Laserscans arriving too fast");
+ }
+ else if (have_pose_)
+ {
+ DoublesVector ranges_, bearings_;
+
+ scan_count_++;
+
+ // Process the data
+ double angle = scan.min_angle;
+ const double gap = (scan.max_angle - scan.min_angle) /
+ (scan.ranges_count - 1);
+ laser_gap_ = gap;
+
+ GUI_DATA.laser_rho.clear();
+ GUI_DATA.laser_phi.clear();
+
+ for (int i = 0; i < static_cast<int>(scan.ranges_count); i++)
+ {
+ ranges_.push_back(scan.ranges[i]);
+ bearings_.push_back(angle);
+ GUI_DATA.laser_rho.push_back(scan.ranges[i]);
+ GUI_DATA.laser_phi.push_back(angle);
+ angle += gap;
+ }
+
+ if (localize_.Update(position_.pos.px, position_.pos.py, position_.pos.pa, ranges_, bearings_))
+ {
+ if (debug_sock_ != -1)
+ SendDebug(GUI_DATA);
+ if (use_g2d_)
+ DrawDebug(GUI_DATA);
+ }
+
+ PublishInterfaces(hdr->timestamp);
+ }
+ else
+ {
+ PLAYER_WARN("Received scan but pose is unknown yet");
+ }
+
+ return 0;
+ }
+
+ if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL, p2d_addr_))
+ {
+ // make a copy of the header and change the address
+ player_msghdr_t newhdr = *hdr;
+ newhdr.addr = odom_addr_;
+ odom_->PutMsg(InQueue, &newhdr, (void*)data);
+
+ return 0;
+ }
+
+ if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_POS, p2d_addr_))
+ {
+ // make a copy of the header and change the address
+ player_msghdr_t newhdr = *hdr;
+ newhdr.addr = odom_addr_;
+
+ // Change the target pose to odom reference frame
+ const player_position2d_cmd_pos_t &glob_target =
+ *static_cast<player_position2d_cmd_pos_t*>(data);
+
+ // Transformation from localized reference to odometry reference
+ const Transf odom_pose_ref(position_.pos.px, position_.pos.py, position_.pos.pa);
+ const Transf glob_pose_ref(localize_.pose().x, localize_.pose().y, localize_.pose().th);
+ const Transf glob_trgt_ref(glob_target.pos.px, glob_target.pos.py, glob_target.pos.pa);
+ const Transf odom_trgt_ref = Compose(odom_pose_ref,
+ TRel(glob_pose_ref, glob_trgt_ref));
+
+ // Marshalling
+ player_position2d_cmd_pos_t odom_target = glob_target;
+ odom_target.pos.px = odom_trgt_ref.tX();
+ odom_target.pos.py = odom_trgt_ref.tY();
+ odom_target.pos.pa = odom_trgt_ref.tPhi();
+
+ odom_->PutMsg(InQueue, &newhdr, static_cast<void*>(&odom_target));
+
+ //printf("TARGET (%8.3f %8.3f %8.3f)-->(%8.3f %8.3f %8.3f)\n",
+ // glob_target.pos.px, glob_target.pos.py, glob_target.pos.pa,
+ // odom_target.pos.px, odom_target.pos.py, odom_target.pos.pa);
+
+ return 0;
+ }
+
+ if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, -1, p2d_addr_))
+ {
+ // Pass the request on to the underlying position device and wait for the reply.
+ Message* msg;
+
+ if (!(msg = odom_->Request(
+ InQueue,
+ hdr->type,
+ hdr->subtype,
+ (void*)data,
+ hdr->size,
+ &hdr->timestamp)))
+ {
+ PLAYER_WARN1("failed to forward config request with subtype: %d\n",
+ hdr->subtype);
+ return -1;
+ }
+
+ player_msghdr_t* rephdr = msg->GetHeader();
+
+ void* repdata = msg->GetPayload();
+ // Copy in our address and forward the response
+ rephdr->addr = p2d_addr_;
+ this->Publish(resp_queue, rephdr, repdata);
+ delete msg;
+ return 0;
+ }
+
+ if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_RESP_ACK, PLAYER_SIMULATION_REQ_GET_POSE2D, sim_addr_))
+ {
+ // The ground truth pose is back
+ const player_simulation_pose2d_req_t &reply =
+ *static_cast<const player_simulation_pose2d_req_t *>(data);
+
+ sim_pose_ = Pose(reply.pose.px, reply.pose.py, reply.pose.pa);
+
+ return 0;
+ }
+
+ // Dismiss clear messages
+
+
+ return -1;
+}
Added: code/player/trunk/server/drivers/localization/ekfvloc/feature.cc
===================================================================
--- code/player/trunk/server/drivers/localization/ekfvloc/feature.cc (rev 0)
+++ code/player/trunk/server/drivers/localization/ekfvloc/feature.cc 2010-12-12 05:49:13 UTC (rev 9007)
@@ -0,0 +1,230 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2010
+ * Mayte Lázaro, Alejandro R. Mosteo
+ *
+ *
+ * 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
+ */
+
+#include "feature.hh"
+#include "hregions.hh"
+#include "params.hh"
+#include "sub_opt.hh"
+
+Feature::Feature(GeometricEntityKinds entity_kind) :
+ uloc_(entity_kind),
+ split_(0, 0, 0, 0)
+{
+ dimension_ = 0;
+ codimension_ = 0;
+}
+
+Feature::~Feature()
+{
+}
+
+double Feature::dimension() const
+{
+ return dimension_;
+}
+
+double Feature::codimension() const
+{
+ return codimension_;
+}
+
+const Transf& Feature::Loc() const
+{
+ return uloc_.kX();
+}
+
+const Matrix& Feature::Cov() const
+{
+ return uloc_.kCov();
+}
+
+double Feature::Cov(int i, int j) const
+{
+ return uloc_.kCov()(i, j);
+}
+
+ObservedFeatures::ObservedFeatures()
+{
+}
+
+ObservedFeatures::~ObservedFeatures()
+{
+}
+
+void ObservedFeatures::AddObservedFeature(Feature f)
+{
+ features_.push_back(f);
+ is_paired_.push_back(false);
+}
+
+int ObservedFeatures::Count() const
+{
+ return features_.size();
+}
+
+/*
+void ObservedFeatures::SetPaired(int i, bool b)
+{
+ is_paired_[i] = b;
+}
+*/
+
+const Feature & ObservedFeatures::features(int f) const
+{
+ return features_[f];
+}
+
+void IntegrateScanPoint(Uloc segment, Uloc point, Matrix &Fk, Matrix &Nk)
+{
+ Matrix hk = Matrix(0, 0);
+ Matrix Hk = Matrix(0, 0);
+ Matrix Gk = Matrix(0, 0);
+ Matrix Sk = Matrix(0, 0);
+
+ const Transf xep = Compose(Inv(segment.kX()), point.kX());
+
+ Matrix bep = Matrix(1, 3);
+ bep(0, 1) = 1;
+
+ const Matrix hp = bep * xep;
+ hk.Include(hp, 0, 0);
+
+ const Matrix Hp = -bep * J1zero(xep) * ~segment.kBind();
+ Hk.Include(Hp, 0, 0);
+
+ const Matrix Gp = bep * J2zero(xep) * ~point.kBind();
+ Gk.Include(Gp, 0, 0);
+
+ Sk.Include(point.kCov(), 0, 0);
+
+ EIFnn(Hk, Gk, hk, Sk, Fk, Nk);
+}
+
+void IntegrateScanPoints(Uloc *seg, Scan sTbl, int pFrom, int pEnd, int step)
+{
+ Matrix FkTotal = Matrix(2, 2);
+ Matrix NkTotal = Matrix(2, 1);
+
+ Matrix Nk, Fk;
+
+ for (int pk = pFrom; pk < pEnd; pk += step)
+ {
+ IntegrateScanPoint(*seg, sTbl.uloc(pk), Fk, Nk);
+ FkTotal = FkTotal + Fk;
+ NkTotal = NkTotal + Nk;
+ }
+
+ IntegrateScanPoint(*seg, sTbl.uloc(pEnd), Fk, Nk);
+ FkTotal = FkTotal + Fk;
+ NkTotal = NkTotal + Nk;
+
+ Matrix Mean(FkTotal.RowNo(), 1);
+ CalculateEstimationEIFnn(FkTotal, NkTotal, Mean, seg->Cov());
+
+ seg->Pert()(0, 0) = Mean(0, 0);
+ seg->Pert()(1, 0) = Mean(1, 0);
+
+ seg->CenterUloc();
+}
+
+void Feature::GeometricRelationsObservationPointToPoint(Uloc Lsp1, Uloc Lsp2)
+{
+
+ Transf xp1p2 = TRel(Lsp1.kX(), Lsp2.kX());
+
+ double phi1 = atan2(xp1p2.tY(), xp1p2.tX());
+ double phi2 = phi1 - xp1p2.tPhi();
+
+ dimension_ = sqrt(pow(Lsp2.kX().tX() - Lsp1.kX().tX(), 2) +
+ pow(Lsp2.kX().tY() - Lsp1.kX().tY(), 2));
+
+ codimension_ =
+ Lsp1.kCov()(0, 0) * pow(cos(phi1), 2) +
+ Lsp1.kCov()(1, 1) * pow(sin(phi1), 2) +
+ Lsp2.kCov()(0, 0) * pow(cos(phi2), 2) +
+ Lsp2.kCov()(1, 1) * pow(sin(phi2), 2);
+}
+
+void Feature::ComputeSegmentLength(Uloc pnt1, Uloc pnt2)
+{
+ GeometricRelationsObservationPointToPoint(pnt1, pnt2);
+}
+
+void ComputeSegments(Scan sTbl, const RegionsVector &rTbl, ObservedFeatures *mTbl)
+{
+ Feature seg = Feature(EDGE);
+ int pFrom, pTo;
+
+ for (int hr = 0; hr < (int)rTbl.size(); hr++)
+ {
+ for (int ep = 0; ep < rTbl.at(hr).NumEps() - 1; ep++)
+ {
+ pFrom = rTbl[hr].Ep(ep).idx(); // RG_EP(rTbl, hr, ep);
+ pTo = rTbl[hr].Ep(ep + 1).idx(); // RG_EP(rTbl, hr, ep+1);
+ // ALEX: the +1,-1 wasn't originally there. This way we take a
+ // conservative shorter observed segment, but the former way,
+ // induced errors with the misaligned splitting extra scan point.
+ // Since I'm not sure which of the two endpoints will be mis-
+ // aligned, better safer than sorry.
+
+ if (pTo - pFrom < kMinPointsInSegment) // ALEX ADDED
+ continue;
+
+ // Calculate segment pFrom -- pTo
+ seg.set_uloc(CalculateAnalyticalEdge(sTbl.uloc(pFrom).kX(),
+ sTbl.uloc(pTo).kX()));
+
+ {
+ Uloc u = seg.uloc();
+ IntegrateScanPoints(&u, sTbl, pFrom, pTo, 1);
+ seg.set_uloc(u);
+ }
+
+ // This is critical: it adjusts segment co-variance realistically.
+ // Without this, Kalman filter won't work properly.
+ {
+ Uloc u = seg.uloc();
+ IntegrateScanPoints(&u, sTbl, pFrom, pTo, pTo - pFrom);
+ seg.SetCov(u.kCov());
+ }
+
+ seg.ComputeSegmentLength(sTbl.uloc(pFrom), sTbl.uloc(pTo));
+
+ seg.SetScan(GuiSplit(sTbl.rho(pFrom), sTbl.phi(pFrom),
+ sTbl.rho(pTo), sTbl.phi(pTo)));
+
+ //Add feature to Obs_Features
+ mTbl->AddObservedFeature(seg);
+ }
+ }
+}
+
+void ScanDataSegmentation(const Scan &laser_raw_data, ObservedFeatures *feat_table)
+{
+ RegionsVector HomRegionsTable;
+
+ FindHomogeneousRegions(laser_raw_data, &HomRegionsTable);
+ IterativeLineFitting(laser_raw_data, &HomRegionsTable);
+
+ feat_table->Clear();
+ ComputeSegments(laser_raw_data, HomRegionsTable, feat_table);
+}
+
Added: code/player/trunk/server/drivers/localization/ekfvloc/feature.hh
===================================================================
--- code/player/trunk/server/drivers/localization/ekfvloc/feature.hh (rev 0)
+++ code/player/trunk/server/drivers/localization/ekfvloc/feature.hh 2010-12-12 05:49:13 UTC (rev 9007)
@@ -0,0 +1,87 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2010
+ * Mayte Lázaro, Alejandro R. Mosteo
+ *
+ *
+ * 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
+ */
+
+#ifndef FEATURE_H_
+#define FEATURE_H_
+
+
+#define MAX_OBS_FEATURES 100
+
+#include <vector>
+#include "scan.hh"
+#include "types.hh"
+#include "uloc.hh"
+
+class Feature
+{
+public:
+ /// Defaults to EDGE
+ Feature(GeometricEntityKinds entity_kind);
+ virtual ~Feature();
+
+ double dimension() const;
+ double codimension() const;
+ const Transf& Loc() const;
+
+ const Matrix& Cov() const;
+ double Cov(int i, int j) const;
+ void SetCov(const Matrix& c) { uloc_.SetCov(c); };
+
+ const Uloc& uloc(void) const { return uloc_; };
+ void set_uloc(const Uloc& u) { uloc_ = u; };
+
+ void ComputeSegmentLength(Uloc p1, Uloc p2);
+
+ void SetScan(const GuiSplit &split) { split_ = split; };
+ // Keep the original raw data for debug
+ const GuiSplit &GetScan(void) const { return split_; };
+private:
+ void GeometricRelationsObservationPointToPoint(Uloc Lsp1, Uloc Lsp2);
+
+ double dimension_;
+ double codimension_;
+ Uloc uloc_;
+
+ GuiSplit split_;
+};
+
+
+class ObservedFeatures
+{
+public:
+ ObservedFeatures();
+ virtual ~ObservedFeatures();
+
+ void AddObservedFeature(Feature f);
+ int Count() const;
+ //void SetPaired(int i, bool b);
+ const Feature & features(int f) const;
+
+ void Clear(void) { features_.clear(); }
+
+private:
+ vector<Feature> features_;
+ vector<bool> is_paired_;
+};
+
+void ScanDataSegmentation(const Scan &laser_raw_data, ObservedFeatures *feat_table);
+
+#endif /* FEATURE_H_ */
Added: code/player/trunk/server/drivers/localization/ekfvloc/hregions.cc
===================================================================
--- code/player/trunk/server/drivers/localization/ekfvloc/hregions.cc (rev 0)
+++ code/player/trunk/server/drivers/localization/ekfvloc/hregions.cc 2010-12-12 05:49:13 UTC (rev 9007)
@@ -0,0 +1,291 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2010
+ * Mayte Lázaro, Alejandro R. Mosteo
+ *
+ *
+ * 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
+ */
+
+#include <algorithm>
+#include <libplayercore/playercore.h>
+#include "hregions.hh"
+#include "params.hh"
+
+/*
+HRParameters::HRParameters()
+{
+ maxEmptyAng = D_MAX_LOST_PNT * RADIANS(D_LASER_ANG_RES);
+ alphaReg = 50; //(1-HRP_ALPHA_HREG/1000)*100 es el nivel de confianza
+ minPntReg = 4;
+ minLenReg = 0.2;
+
+ alphaILF = 50;//5; (1-HRP_ALPHA_ILF/1000)*100 es el nivel de confianza
+ checkResidual = 0;
+ minDistEP = 0;
+ maxAngEBE = 0;
+}
+*/
+
+/////////////////////////////////////////////////////////////////////////////////
+double randomNormal(double mean, double stdDev)
+{
+ // Returns a random value from a Normal Distribution with the given mean and standard deviation
+
+ double sum = 0.0;
+ const int n = 20;
+ const double half_sum = n * 0.5;
+ const double dt_sum = sqrt(n / 12);
+
+ for (int i = 0; i < n; i++)
+ sum += rand() / RAND_MAX;
+
+ return (((sum - half_sum) / dt_sum * stdDev) + mean);
+}
+
+double computeLengthHRegion(Scan s, int from, int to)
+// Computes the distance between the extremes of the HRegion
+{
+ const Transf x12 = TRel(s.uloc(from).kX(), s.uloc(to).kX());
+
+ return sqrt(pow(x12.tX(), 2) + pow(x12.tY(), 2));
+}
+
+HRegion::HRegion(const Scan &s, int idxFrom, int idxTo) : scan_(&s)
+{
+ endpoints_.push_back(Endpoint(s, idxFrom));
+ endpoints_.push_back(Endpoint(s, idxTo));
+}
+
+void FindHomogeneousRegions(const Scan &s, RegionsVector *r)
+{
+
+ int from, to;
+
+ r->clear(); // RG_LEN (*r) = 0;
+ // RG_NUM_EP(*r, RG_LEN(*r)) = 0;
+
+ for (int k = 0; k < s.ScanCount(); /* below */)
+ {
+ from = k;
+ while (k < (s.ScanCount() - 1))
+ {
+ if (fabs(s.phi(k) - s.phi(k + 1)) > kMaxEmptyAngle) // HRP_MAX_EMPTY_ANGLE(HRP(*r)))
+ break;
+ else if (fabs(s.rho(k + 1) - s.rho(k)) > kMaxEmptyDistance) // in meters!
+ break;
+ else
+ k++;
+ }
+ to = k;
+ const double len = computeLengthHRegion(s, from, to);
+ if ((len > kMinRegionLength) && (to - from > kMinPointsInRegion))
+ {
+ r->push_back(HRegion(s, from, to));
+ // RG_NUM_EP(*r, RG_LEN(*r)) = 2;
+ // RG_FROM (*r, RG_LEN(*r)) = from;
+ // RG_TO (*r, RG_LEN(*r)) = to;
+ // RG_LEN (*r)++;
+ }
+ k++;
+ }
+}
+
+int farthestPointToEdge(Scan s,
+ int from,
+ int to,
+ double *maxd2,
+ double *residual)
+/* Calculates the point with the greater disparity with respect to an edge */
+{
+ if ((from < 0) || (from >= s.ScanCount()) || (to < 0) || (to >= s.ScanCount()))
+ {
+ PLAYER_ERROR3("Wrong uloc access in farthestPointToEdge: (%d)--(%d) (max:%d)", from, to, s.ScanCount());
+ }
+
+ const Uloc Lse(
+ integrateEndpointsInEdge(
+ s.uloc(from),
+ s.uloc(to)));
+
+ int bp = -1;
+ double d2;
+
+ *maxd2 = 0.0;
+ *residual = 0.0;
+
+ for (int k = from + 1; k < to; k++)
+ {
+ d2 = mahalanobis_distance_edge_point(Lse, s.uloc(k));
+ *residual += d2;
+ if (d2 > *maxd2)
+ {
+ *maxd2 = d2;
+ bp = k;
+ }
+ }
+
+ return bp;
+}
+
+double calculateResidual(Scan s, int from, int to)
+/* Calculates the residual*/
+{
+ Uloc Lse = integrateEndpointsInEdge(s.uloc(from), s.uloc(to));
+ double r = 0;
+
+ for (int k = from + 1; k < to; k++)
+ r += mahalanobis_distance_edge_point(Lse, s.uloc(k));
+
+ return r;
+}
+
+bool verifyResidualConditions(Scan s, int from, int to, int bp, double r)
+{
+ /* Verifies whether the new edge improves the representation */
+
+ double r1, r2;
+
+ r1 = calculateResidual(s, from, bp);
+ r2 = calculateResidual(s, bp, to);
+
+ return (r >= (r1 + r2));
+}
+
+bool verifyEndPointsAlignment(Scan s, int from, int to, int bp, double maxAngle)
+/* Verifies whether the detected endpoint is aligned with the endpoints */
+{
+ double x1, y1, xb, yb, x2, y2, phi;
+
+ x1 = s.uloc(from).kX().tX();
+ y1 = s.uloc(from).kX().tY();
+
+ xb = s.uloc(bp).kX().tX();
+ yb = s.uloc(bp).kX().tY();
+
+ x2 = s.uloc(to).kX().tX();
+ y2 = s.uloc(to).kX().tY();
+
+ phi = spAtan2(y2 - yb, x2 - xb) - spAtan2(yb - y1, xb - x1);
+
+ return (fabs(phi) >= maxAngle);
+}
+
+double computeDistanceEndPoints(Scan s, int from, int to)
+// Computes the distance between two endpoints
+{
+ Transf x12 = TRel(s.uloc(from).kX(), s.uloc(to).kX());
+
+ return sqrt(pow(x12.tX(), 2) + pow(x12.tY(), 2));
+}
+
+void HRegion::IterativeLineSplit(int fromIdx, int toIdx)
+/* Performs one iteration of the iterative line fitting algorithm */
+{
+ double m;
+ double residual;
+
+ const int b = farthestPointToEdge(*scan_, fromIdx, toIdx, &m, &residual);
+
+ PLAYER_MSG1(9, " Farthest idx: %d", b);
+
+ if ((b >= fromIdx) &&
+ (m > CHISQUARE[0][COLUMN(kAlphaILF())]) &&
+ (kCheckResidual ? verifyResidualConditions(*scan_, fromIdx, toIdx, b, residual) : true) &&
+ verifyEndPointsAlignment(*scan_, fromIdx, toIdx, b, kMaxAngEBE))
+ {
+ const double dfb = computeDistanceEndPoints(*scan_, fromIdx, b);
+ const double dbt = computeDistanceEndPoints(*scan_, b, toIdx);
+
+ if ((dfb > kMinDistBetweenEndpoints) && (dbt > kMinDistBetweenEndpoints))
+ {
+ PLAYER_MSG3(9, " Splitting: %3d -- %3d -- %3d", fromIdx, b, toIdx);
+ PLAYER_MSG2(9, " because: [m > CHI] [%5.3f > %5.3f]", m, CHISQUARE[0][COLUMN(kAlphaILF())]);
+ endpoints_.push_back(Endpoint(*scan_, b));
+ // Endpoints are not sorted here, so they will have to be at the end
+
+ IterativeLineSplit(fromIdx, b);
+ IterativeLineSplit(b, toIdx);
+ }
+ else
+ PLAYER_MSG0(9, "Split rejected because dist between endpoints");
+ }
+ else
+ PLAYER_MSG0(9, "Split rejected outright");
+}
+
+void HRegion::IterativeLineSplit(void)
+{
+ if (endpoints_.size() != 2)
+ PLAYER_ERROR1("Initial endpoints %d != 2", endpoints_.size());
+
+ PLAYER_MSG2(8, " SPLITTING INDEXES %3d -- %3d",
+ endpoints_[0].idx(), endpoints_[1].idx());
+
+ IterativeLineSplit(endpoints_[0].idx(), endpoints_[1].idx());
+ sort(endpoints_.begin(), endpoints_.end());
+}
+
+void IterativeLineFitting(const Scan &s, RegionsVector *r)
+/*****************************************************************************/
+/* Performs the iterative line fitting algorithm in the set of detected */
+/* Homogeneous Regions */
+/*****************************************************************************/
+{
+ for (int i = 0; i < (int) r->size(); i++)
+ {
+ PLAYER_MSG1(8, "SPLITTING REGION %d", i);
+ r->at(i).IterativeLineSplit();
+ }
+
+ PLAYER_MSG2(5, "SCAN %d -- %d", 0, s.ScanCount());
+
+ for (int i = 0; i < (int) r->size(); i++)
+ {
+ r->at(i).PushGuiData(&GUI_DATA);
+
+ PLAYER_MSG1(6, "REGION %02d", i);
+ for (int j = 0; j < (int) r->at(i).NumEps(); j++)
+ {
+ PLAYER_MSG2(7, " EP %02d: %d", j, r->at(i).Ep(j).idx());
+ }
+ }
+}
+
+void HRegion::PushGuiData(GuiData *gui_data) const
+{
+ gui_data->regions.push_back(GetGuiRegion());
+
+ for (size_t i = 0; i < endpoints_.size() - 1; i++)
+ gui_data->splits.push_back(GetGuiSplit(i));
+}
+
+GuiRegion HRegion::GetGuiRegion(void) const
+{
+ const int ini = endpoints_.front().idx();
+ const int fin = endpoints_.back().idx();
+
+ return GuiRegion(scan_->rho(ini), scan_->phi(ini),
+ scan_->rho(fin), scan_->phi(fin));
+}
+
+GuiSplit HRegion::GetGuiSplit(int i) const
+{
+ const int ini = endpoints_.at(i).idx();
+ const int fin = endpoints_.at(i + 1).idx();
+
+ return GuiSplit(scan_->rho(ini), scan_->phi(ini),
+ scan_->rho(fin), scan_->phi(fin));
+}
\ No newline at end of file
Added: code/player/trunk/server/drivers/localization/ekfvloc/hregions.hh
===================================================================
--- code/player/trunk/server/drivers/localization/ekfvloc/hregions.hh (rev 0)
+++ code/player/trunk/server/drivers/localization/ekfvloc/hregions.hh 2010-12-12 05:49:13 UTC (rev 9007)
@@ -0,0 +1,127 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2010
+ * Mayte Lázaro, Alejandro R. Mosteo
+ *
+ *
+ * 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
+ */
+
+#ifndef HREGIONS_H_
+#define HREGIONS_H_
+
+#include <vector>
+#include "scan.hh"
+#include "types.hh"
+
+/////////////////////////////////////////////////////////
+#define DF (20)
+#define NALPHAS (6)
+
+const double
+CHISQUARE[DF][NALPHAS] =
+{ { 7.88, 6.63, 5.02, 3.84, 2.71, 1.32 },
+ { 10.6, 9.21, 7.38, 5.99, 4.61, 2.77 },
+ { 12.8, 11.3, 9.35, 7.81, 6.25, 4.11 },
+ { 14.9, 13.3, 11.1, 9.49, 7.78, 5.39 },
+ { 16.7, 15.1, 12.8, 11.1, 9.24, 6.63 },
+ { 18.5, 16.8, 14.4, 12.6, 10.6, 7.84 },
+ { 20.3, 18.5, 16.0, 14.1, 12.0, 9.04 },
+ { 22.0, 20.1, 17.5, 15.5, 13.4, 10.2 },
+ { 23.6, 21.7, 19.0, 16.9, 14.7, 11.4 },
+ { 25.2, 23.2, 20.5, 18.3, 16.0, 12.5 },
+ { 26.8, 24.7, 21.9, 19.7, 17.3, 13.7 },
+ { 28.3, 26.2, 23.3, 21.0, 18.5, 14.8 },
+ { 29.8, 27.7, 24.7, 22.4, 19.8, 16.0 },
+ { 31.3, 29.1, 26.1, 23.7, 21.1, 17.1 },
+ { 32.8, 30.6, 27.5, 25.0, 22.3, 18.2 },
+ { 34.3, 32.0, 28.8, 26.3, 23.5, 19.4 },
+ { 35.7, 33.4, 30.2, 27.6, 24.8, 20.5 },
+ { 37.2, 34.8, 31.5, 28.9, 26.0, 21.6 },
+ { 38.6, 36.2, 32.9, 30.1, 27.2, 22.7 },
+ { 40.0, 37.6, 34.2, 31.4, 28.4, 23.8 }
+};
+
+#define COLUMN(a) ((a) <= 5 ? 0 : ((a) <= 10 ? 1 : ((a) <= 20 ? 2 : ((a) <= 50 ? 3 : ((a) <= 100 ? 4 : 5)))))
+#define RADIANS(d) ((d)*M_PI/180.0)
+
+// see params.hh
+// const int D_MAX_LOST_PNT = 4;
+// const double D_LASER_ANG_RES = 0.5;
+
+class Endpoint
+{
+public:
+ Endpoint(const Scan &s, int idx) : idx_(idx) {}
+
+ const int idx(void) const { return idx_; }
+
+ bool operator < (const Endpoint &rhs) const { return idx_ < rhs.idx_; }
+
+private:
+ int idx_;
+};
+
+typedef std::vector<Endpoint> EndpointsVector;
+
+class HRegion
+{
+public:
+ HRegion(const Scan &s, int idxFrom, int idxTo);
+ virtual ~HRegion() {}
+
+ const int NumEps(void) const { return endpoints_.size(); }
+ const Endpoint &Ep(int i) const { return endpoints_.at(i); }
+
+ void IterativeLineSplit(int fromIdx, int toIdx);
+ void IterativeLineSplit(void); // Initiator of recursion
+
+ void PushGuiData(GuiData *gui_data) const;
+private:
+ GuiRegion GetGuiRegion(void) const;
+ GuiSplit GetGuiSplit(int i) const;
+
+ const Scan *scan_;
+ EndpointsVector endpoints_;
+};
+
+typedef std::vector<HRegion> RegionsVector;
+
+/*
+#define RG_LEN(r) ((r).len)
+#define RG_EP2(r, i) ((r).ep[i])
+#define RG_NUM_EP(r, i) ((r).ep[i].len)
+#define RG_EP(r, i, j) ((r).ep[i].idx[j])
+#define RG_FROM(r, i) ((r).ep[i].idx[0])
+#define RG_TO(r, i) ((r).ep[i].idx[(r).ep[i].len-1])
+*/
+
+/*
+#define HRP(p) ((p).par)
+#define HRP_MAX_EMPTY_ANGLE(p) ((p).maxEmptyAng)
+#define HRP_ALPHA_HREG(p) ((p).alphaReg)
+#define HRP_MIN_PNT_REG(p) ((p).minPntReg)
+#define HRP_MIN_LEN_REG(p) ((p).minLenReg)
+
+#define HRP_ALPHA_ILF(p) ((p).alphaILF)
+#define HRP_CHECK_RESIDUAL(p) ((p).checkResidual)
+#define HRP_MIN_DIST_EP(p) ((p).minDistEP)
+#define HRP_MAX_EBE_ANG(p) ((p).maxAngEBE)
+*/
+
+void FindHomogeneousRegions(const Scan &s, RegionsVector *r);
+void IterativeLineFitting(const Scan &s, RegionsVector *r);
+
+#endif /* HREGIONS_H_ */
Added: code/player/trunk/server/drivers/localization/ekfvloc/localize.cc
===================================================================
--- code/player/trunk/server/drivers/localization/ekfvloc/localize.cc (rev 0)
+++ code/player/trunk/server/drivers/localization/ekfvloc/localize.cc 2010-12-12 05:49:13 UTC (rev 9007)
@@ -0,0 +1,110 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2010
+ * Alejandro R. Mosteo
+ *
+ *
+ * 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
+ */
+
+#include <cassert>
+#include "localize.hh"
+#include <map>
+
+Localize::Localize(double laser_max_range,
+ double laser_pose_x,
+ double laser_pose_y,
+ double laser_pose_angle,
+ double laser_noise_range, // uncertainty in distance measured
+ double laser_noise_bearing, // uncertainty in sample bearing
+ double odom_noise_x,
+ double odom_noise_y,
+ double odom_noise_angle // uncertainty for odometry
+) :
+ scan_(laser_max_range,
+ laser_pose_x, laser_pose_y, laser_pose_angle,
+ laser_noise_range, laser_noise_bearing),
+ robot_(odom_noise_x, odom_noise_y, odom_noise_angle),
+ has_pose_(false)
+{
+}
+
+void Localize::SetLaserPose(double x, double y, double a)
+{
+ scan_.SetLaserPose(x, y, a);
+}
+
+void Localize::SetRobotPose(double x, double y, double angle)
+{
+ SetPoses(0, 0, 0, x, y, angle);
+}
+
+void Localize::SetPoses(double ox, double oy, double oth,
+ double gx, double gy, double gth)
+{
+ robot_.SetPoses(ox, oy, oth, gx, gy, gth);
+ has_pose_ = true;
+}
+
+void Localize::SetRobotPoseError(double ex, double ey, double eth)
+{
+ robot_.SetCurrentError(ex, ey, eth);
+}
+
+void Localize::LoadMap(const char* filename)
+{
+ robot_.map() = SegmentMap(filename);
+}
+
+void Localize::SetMap(const SegmentsVector& map)
+{
+ robot_.map() = SegmentMap();
+
+ for (int i = 0; i < (int) map.size(); i++)
+ {
+ robot_.map().AddSegment(map[i].x1, map[i].y1,
+ map[i].x2, map[i].y2);
+ }
+}
+
+const SegmentMap &Localize::Map(void) const
+{
+ return robot_.map();
+}
+
+void Localize::AddSegment(const Segment& segment) {
+ robot_.map().AddSegment(segment.x1, segment.y1,
+ segment.x2, segment.y2);
+}
+
+/// Compute actualization from robot accumulated odometry and laser reading
+bool Localize::Update(double robot_x,
+ double robot_y,
+ double robot_angle,
+ DoublesVector ranges,
+ DoublesVector bearings)
+{
+ assert(has_pose_);
+ assert(!robot_.map().IsEmpty());
+
+ scan_.SetLastScan(ranges, bearings);
+
+ return robot_.Locate(Transf(robot_x, robot_y, robot_angle), scan_);
+}
+
+Pose Localize::pose(void) const
+{
+ return robot_.EstimatedPose();
+}
Added: code/player/trunk/server/drivers/localization/ekfvloc/localize.hh
===================================================================
--- code/player/trunk/server/drivers/localization/ekfvloc/localize.hh (rev 0)
+++ code/player/trunk/server/drivers/localization/ekfvloc/localize.hh 2010-12-12 05:49:13 UTC (rev 9007)
@@ -0,0 +1,74 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2010
+ * Alejandro R. Mosteo
+ *
+ *
+ * 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
+ */
+
+#ifndef LOCALIZE_HH_
+#define LOCALIZE_HH_
+
+#include "robot_location.hh"
+#include "scan.hh"
+#include "segment_map.hh"
+
+class Localize
+{
+public:
+ /// Initialize parameters
+ Localize(double laser_max_range,
+ double laser_pose_x,
+ double laser_pose_y,
+ double laser_pose_angle,
+ double laser_noise_range, // uncertainty in distance measured (2sigma)
+ double laser_noise_bearing, // uncertainty in sample bearing (2sigma)
+ double odom_noise_x,
+ double odom_noise_y,
+ double odom_noise_angle // uncertainty for odometry model
+ );
+
+ /// Initialize robot map pose, and reset odometry to zero
+ void SetRobotPose(double x, double y, double angle);
+ /// Initialize both odometry (if it is not zero) and global (map) pose
+ void SetPoses(double ox, double oy, double oth,
+ double gx, double gy, double gth);
+ /// Initial pose error
+ void SetRobotPoseError(double ex, double ey, double eth);
+
+ /// Update laser pose (limited to x, y ,a for now, though)
+ /// m m rad
+ void SetLaserPose(double x, double y, double a);
+
+ void LoadMap(const char* filename);
+ void SetMap(const SegmentsVector& map);
+ void AddSegment(const Segment& segment);
+ const SegmentMap &Map(void) const;
+
+ /// Compute actualization from robot accumulated odometry and laser reading
+ /// Returns true if the update was performed (minimum delta in odometry)
+ bool Update(double robot_x, double robot_y, double robot_angle,
+ DoublesVector ranges, DoublesVector bearings);
+
+ Pose pose(void) const;
+ Matrix GetCovariance(void) const { return robot_.Covariance(); };
+private:
+ Scan scan_;
+ RobotLocation robot_;
+ bool has_pose_;
+};
+
+#endif /* LOCALIZE_HH_ */
Added: code/player/trunk/server/drivers/localization/ekfvloc/matrix.h
===================================================================
--- code/player/trunk/server/drivers/localization/ekfvloc/matrix.h (rev 0)
+++ code/player/trunk/server/drivers/localization/ekfvloc/matrix.h 2010-12-12 05:49:13 UTC (rev 9007)
@@ -0,0 +1,1122 @@
+////////////////////////////////
+// Matrix TCL Lite v1.13
+// Copyright (c) 1997-2002 Techsoft Pvt. Ltd. (See License.Txt file.)
+
+// Matrix TCL Lite is the free version of matrix template class library for
+// performing matrix calculations with ease and efficiency in C++ programs.
+// However, it supports only basic matrix operations. It is actually the older
+// version of our Matrix TCL Pro package and does not contain separate
+// documentation except some brief description at the top of the header file.
+// This library is free for educational and non-commercial use.
+
+//
+// Matrix.h: Matrix C++ template class include file
+// Web: http://www.techsoftpl.com/matrix/
+// Email: matrix@...
+//
+
+//////////////////////////////
+// Installation:
+//
+// Copy this "matrix.h" file into include directory of your compiler.
+//
+
+//////////////////////////////
+// Note: This matrix template class defines majority of the matrix
+// operations as overloaded operators or methods. It is assumed that
+// users of this class is familiar with matrix algebra. We have not
+// defined any specialization of this template here, so all the instances
+// of matrix will be created implicitly by the compiler. The data types
+// tested with this class are float, double, long double, complex<float>,
+// complex<double> and complex<long double>. Note that this class is not
+// optimized for performance.
+//
+// Since implementation of exception, namespace and template are still
+// not standardized among the various (mainly old) compilers, you may
+// encounter compilation error with some compilers. In that case remove
+// any of the above three features by defining the following macros:
+//
+// _NO_NAMESPACE: Define this macro to remove namespace support.
+//
+// _NO_EXCEPTION: Define this macro to remove exception handling
+// and use old style of error handling using function.
+//
+// _NO_TEMPLATE: If this macro is defined matrix class of double
+// type will be generated by default. You can also
+// generate a different type of matrix like float.
+//
+// _SGI_BROKEN_STL: For SGI C++ v.7.2.1 compiler.
+//
+// Since all the definitions are also included in this header file as
+// inline function, some compiler may give warning "inline function
+// can't be expanded". You may ignore/disable this warning using compiler
+// switches. All the operators/methods defined in this class have their
+// natural meaning except the followings:
+//
+// Operator/Method Description
+// --------------- -----------
+// operator () : This function operator can be used as a
+// two-dimensional subscript operator to get/set
+// individual matrix elements.
+//
+// operator ! : This operator has been used to calculate inversion
+// of matrix.
+//
+// operator ~ : This operator has been used to return transpose of
+// a matrix.
+//
+// operator ^ : It is used calculate power (by a scalar) of a matrix.
+// When using this operator in a matrix equation, care
+// must be taken by parenthesizing it because it has
+// lower precedence than addition, subtraction,
+// multiplication and division operators.
+//
+// operator >> : It is used to read matrix from input stream as per
+// standard C++ stream operators.
+//
+// operator << : It is used to write matrix to output stream as per
+// standard C++ stream operators.
+//
+// Note that professional version of this package, Matrix TCL Pro 2.11
+// is optimized for performance and supports many more matrix operations.
+// It is available from our web site at <http://www.techsoftpl.com/matrix/>.
+//
+
+#ifndef __cplusplus
+#error Must use C++ for the type matrix.
+#endif
+
+#if !defined(__STD_MATRIX_H)
+#define __STD_MATRIX_H
+
+//////////////////////////////
+// First deal with various shortcomings and incompatibilities of
+// various (mainly old) versions of popular compilers available.
+//
+
+#if defined(__BORLANDC__)
+#pragma option -w-inl -w-pch
+#endif
+
+#if ( defined(__BORLANDC__) || _MSC_VER <= 1000 ) && !defined( __GNUG__ )
+# include <stdio.h>
+# include <stdlib.h>
+# include <math.h>
+# include <iostream.h>
+# include <string.h>
+#else
+# include <cmath>
+# include <cstdio>
+# include <cstdlib>
+# include <string>
+# include <iostream>
+#endif
+
+#if defined(_MSC_VER) && _MSC_VER <= 1000
+# define _NO_EXCEPTION // stdexception is not fully supported in MSVC++ 4.0
+typedef int bool;
+# if !defined(false)
+# define false 0
+# endif
+# if !defined(true)
+# define true 1
+# endif
+#endif
+
+#if defined(__BORLANDC__) && !defined(__WIN32__)
+# define _NO_EXCEPTION // std exception and namespace are not fully
+# define _NO_NAMESPACE // supported in 16-bit compiler
+#endif
+
+#if defined(_MSC_VER) && !defined(_WIN32)
+# define _NO_EXCEPTION
+#endif
+
+#if defined(_NO_EXCEPTION)
+# define _NO_THROW
+# define _THROW_MATRIX_ERROR
+#else
+# if defined(_MSC_VER)
+# if _MSC_VER >= 1020
+# include <stdexcept>
+# else
+# include <stdexcpt.h>
+# endif
+# elif defined(__MWERKS__)
+# include <stdexcept>
+# elif (__GNUC__ >= 2 || (__GNUC__ == 2 && __GNUC_MINOR__ >= 8))
+# include <stdexcept>
+# else
+# include <stdexcep>
+# endif
+# define _NO_THROW throw ()
+# define _THROW_MATRIX_ERROR throw (matrix_error)
+#endif
+
+#ifndef __MINMAX_DEFINED
+//# define max(a,b) (((a) > (b)) ? (a) : (b))
+//# define min(a,b) (((a) < (b)) ? (a) : (b))
+#endif
+
+#if defined(_MSC_VER)
+#undef _MSC_EXTENSIONS // To include overloaded abs function definitions!
+#endif
+
+#if ( defined(__BORLANDC__) || _MSC_VER ) && !defined( __GNUG__ )
+inline float abs (float v) { return (float)fabs( v); }
+inline double abs (double v) { return fabs( v); }
+inline long double abs (long double v) { return fabsl( v); }
+#endif
+
+#if defined(__GNUG__) || defined(__MWERKS__) || (defined(__BORLANDC__) && (__BORLANDC__ >= 0x540))
+#define FRIEND_FUN_TEMPLATE <>
+#else
+#define FRIEND_FUN_TEMPLATE
+#endif
+
+#if defined(_MSC_VER) && _MSC_VER <= 1020 // MSVC++ 4.0/4.2 does not
+# define _NO_NAMESPACE // support "std" namespace
+#endif
+
+#if !defined(_NO_NAMESPACE)
+#if defined( _SGI_BROKEN_STL ) // For SGI C++ v.7.2.1 compiler
+namespace std { }
+#endif
+using namespace std;
+#endif
+
+#ifndef _NO_NAMESPACE
+namespace math {
+#endif
+
+#if !defined(_NO_EXCEPTION)
+class matrix_error : public logic_error
+{
+ public:
+ matrix_error (const string& what_arg) : logic_error( what_arg) {}
+};
+#define REPORT_ERROR(ErrormMsg) throw matrix_error( ErrormMsg);
+#else
+inline void _matrix_error (const char* pErrMsg)
+{
+ cout << pErrMsg << endl;
+ exit(1);
+}
+#define REPORT_ERROR(ErrormMsg) _matrix_error( ErrormMsg);
+#endif
+
+#if !defined(_NO_TEMPLATE)
+# define MAT_TEMPLATE template <class T>
+# define matrixT matrix<T>
+#else
+# define MAT_TEMPLATE
+# define matrixT matrix
+# ifdef MATRIX_TYPE
+ typedef MATRIX_TYPE T;
+# else
+ typedef double T;
+# endif
+#endif
+
+
+MAT_TEMPLATE
+class matrix
+{
+public:
+ // Constructors
+ matrix (const matrixT& m);
+ matrix (size_t row = 6, size_t col = 6);
+
+ // Destructor
+ ~matrix ();
+
+ // Assignment operators
+ matrixT& operator = (const matrixT& m) _NO_THROW;
+
+ // Value extraction method
+ size_t RowNo () const { return _m->Row; }
+ size_t ColNo () const { return _m->Col; }
+
+ // Subscript operator
+ T& operator () (size_t row, size_t col) _THROW_MATRIX_ERROR;
+ T operator () (size_t row, size_t col) const _THROW_MATRIX_ERROR;
+
+ // Unary operators
+ matrixT operator + () _NO_THROW { return *this; }
+ matrixT operator - () _NO_THROW;
+
+ // Combined assignment - calculation operators
+ matrixT& operator += (const matrixT& m) _THROW_MATRIX_ERROR;
+ matrixT& operator -= (const matrixT& m) _THROW_MATRIX_ERROR;
+ matrixT& operator *= (const matrixT& m) _THROW_MATRIX_ERROR;
+ matrixT& operator *= (const T& c) _NO_THROW;
+ matrixT& operator /= (const T& c) _NO_THROW;
+ matrixT& operator ^= (const size_t& pow) _THROW_MATRIX_ERROR;
+
+ // Miscellaneous -methods
+ void Null (const size_t& row, const size_t& col) _NO_THROW;
+ void Null () _NO_THROW;
+ void Unit (const size_t& row) _NO_THROW;
+ void Unit () _NO_THROW;
+ void SetSize (size_t row, size_t col) _NO_THROW;
+
+ // Utility methods
+ matrixT Solve (const matrixT& v) const _THROW_MATRIX_ERROR;
+ matrixT Adj () _THROW_MATRIX_ERROR;
+ matrixT Inv () _THROW_MATRIX_ERROR;
+ T Det () const _THROW_MATRIX_ERROR;
+ T Norm () _NO_THROW;
+ T Cofact (size_t row, size_t col) _THROW_MATRIX_ERROR;
+ T Cond () _NO_THROW;
+
+ // Type of matrices
+ bool IsSquare () _NO_THROW { return (_m->Row == _m->Col); }
+ bool IsSingular () _NO_THROW;
+ bool IsDiagonal () _NO_THROW;
+ bool IsScalar () _NO_THROW;
+ bool IsUnit () _NO_THROW;
+ bool IsNull () _NO_THROW;
+ bool IsSymmetric () _NO_THROW;
+ bool IsSkewSymmetric () _NO_THROW;
+ bool IsUpperTriangular () _NO_THROW;
+ bool IsLowerTriangular () _NO_THROW;
+
+ //Added by Mayte
+ matrixT Extract (const size_t r, const size_t c, const size_t rows, const size_t cols) _THROW_MATRIX_ERROR;
+ void Include (const matrixT& small, const size_t r, const size_t c) _THROW_MATRIX_ERROR;
+
+private:
+ struct base_mat
+ {
+ T **Val;
+ size_t Row, Col, RowSiz, ColSiz;
+ int Refcnt;
+
+ base_mat (size_t row, size_t col, T** v)
+ {
+ Row = row; RowSiz = row;
+ Col = col; ColSiz = col;
+ Refcnt = 1;
+
+ Val = new T* [row];
+ size_t rowlen = col * sizeof(T);
+
+ for (size_t i=0; i < row; i++)
+ {
+ Val[i] = new T [col];
+ if (v) memcpy( Val[i], v[i], rowlen);
+ }
+ }
+ ~base_mat ()
+ {
+ for (size_t i=0; i < RowSiz; i++)
+ delete [] Val[i];
+ delete [] Val;
+ }
+ };
+ base_mat *_m;
+
+ void clone ();
+ void realloc (size_t row, size_t col);
+ int pivot (size_t row);
+};
+
+#if defined(_MSC_VER) && _MSC_VER <= 1020
+# undef _NO_THROW // MSVC++ 4.0/4.2 does not support
+# undef _THROW_MATRIX_ERROR // exception specification in definition
+# define _NO_THROW
+# define _THROW_MATRIX_ERROR
+#endif
+
+// constructor
+MAT_TEMPLATE inline
+matrixT::matrix (size_t row, size_t col)
+{
+ _m = new base_mat( row, col, 0);
+
+//Anadido para que al iniciar una matriz todos sus elementos sean 0
+ for (size_t i=0; i < row; i++)
+ for (size_t j=0; j < col; j++)
+ _m->Val[i][j] = 0;
+}
+
+// copy constructor
+MAT_TEMPLATE inline
+matrixT::matrix (const matrixT& m)
+{
+ _m = m._m;
+ _m->Refcnt++;
+}
+
+// Internal copy constructor
+MAT_TEMPLATE inline void
+matrixT::clone ()
+{
+ _m->Refcnt--;
+ _m = new base_mat( _m->Row, _m->Col, _m->Val);
+}
+
+// destructor
+MAT_TEMPLATE inline
+matrixT::~matrix ()
+{
+ if (--_m->Refcnt == 0) delete _m;
+}
+
+
+// assignment operator
+MAT_TEMPLATE inline matrixT&
+matrixT::operator = (const matrixT& m) _NO_THROW
+{
+ m._m->Refcnt++;
+ if (--_m->Refcnt == 0) delete _m;
+ _m = m._m;
+ return *this;
+}
+
+// reallocation method
+MAT_TEMPLATE inline void
+matrixT::realloc (size_t row, size_t col)
+{
+ if (row == _m->RowSiz && col == _m->ColSiz)
+ {
+ _m->Row = _m->RowSiz;
+ _m->Col = _m->ColSiz;
+ return;
+ }
+
+ base_mat *m1 = new base_mat( row, col, NULL);
+ size_t colSize = min(_m->Col,col) * sizeof(T);
+ size_t minRow = min(_m->Row,row);
+
+ for (size_t i=0; i < minRow; i++)
+ memcpy( m1->Val[i], _m->Val[i], colSize);
+
+ if (--_m->Refcnt == 0)
+ delete _m;
+ _m = m1;
+
+ return;
+}
+
+// public method for resizing matrix
+MAT_TEMPLATE inline void
+matrixT::SetSize (size_t row, size_t col) _NO_THROW
+{
+ size_t i,j;
+ size_t oldRow = _m->Row;
+ size_t oldCol = _m->Col;
+
+ if (row != _m->RowSiz || col != _m->ColSiz)
+ realloc( row, col);
+
+ for (i=oldRow; i < row; i++)
+ for (j=0; j < col; j++)
+ _m->Val[i][j] = T(0);
+
+ for (i=0; i < row; i++)
+ for (j=oldCol; j < col; j++)
+ _m->Val[i][j] = T(0);
+
+ return;
+}
+
+// subscript operator to get/set individual elements
+MAT_TEMPLATE inline T&
+matrixT::operator () (size_t row, size_t col) _THROW_MATRIX_ERROR
+{
+ if (row >= _m->Row || col >= _m->Col)
+ REPORT_ERROR( "matrixT::operator(): Index out of range!");
+ if (_m->Refcnt > 1) clone();
+ return _m->Val[row][col];
+}
+
+// subscript operator to get/set individual elements
+MAT_TEMPLATE inline T
+matrixT::operator () (size_t row, size_t col) const _THROW_MATRIX_ERROR
+{
+ if (row >= _m->Row || col >= _m->Col)
+ REPORT_ERROR( "matrixT::operator(): Index out of range!");
+ return _m->Val[row][col];
+}
+
+// input stream function
+MAT_TEMPLATE inline istream&
+operator >> (istream& istrm, matrixT& m)
+{
+ for (size_t i=0; i < m.RowNo(); i++)
+ for (size_t j=0; j < m.ColNo(); j++)
+ {
+ T x;
+ istrm >> x;
+ m(i,j) = x;
+ }
+ return istrm;
+}
+
+// output stream function
+MAT_TEMPLATE inline ostream&
+operator << (ostream& ostrm, const matrixT& m)
+{
+ for (size_t i=0; i < m.RowNo(); i++)
+ {
+ for (size_t j=0; j < m.ColNo(); j++)
+ {
+ T x = m(i,j);
+ ostrm << x << '\t';
+ }
+ ostrm << endl;
+ }
+ return ostrm;
+}
+
+
+// logical equal-to operator
+MAT_TEMPLATE inline bool
+operator == (const matrixT& m1, const matrixT& m2) _NO_THROW
+{
+ if (m1.RowNo() != m2.RowNo() || m1.ColNo() != m2.ColNo())
+ return false;
+
+ for (size_t i=0; i < m1.RowNo(); i++)
+ for (size_t j=0; j < m1.ColNo(); j++)
+ if (m1(i,j) != m2(i,j))
+ return false;
+
+ return true;
+}
+
+// logical no-equal-to operator
+MAT_TEMPLATE inline bool
+operator != (const matrixT& m1, const matrixT& m2) _NO_THROW
+{
+ return (m1 == m2) ? false : true;
+}
+
+// combined addition and assignment operator
+MAT_TEMPLATE inline matrixT&
+matrixT::operator += (const matrixT& m) _THROW_MATRIX_ERROR
+{
+ if (_m->Row != m._m->Row || _m->Col != m._m->Col)
+ REPORT_ERROR( "matrixT::operator+= : Inconsistent matrix sizes in addition!");
+ if (_m->Refcnt > 1) clone();
+ for (size_t i=0; i < m._m->Row; i++)
+ for (size_t j=0; j < m._m->Col; j++)
+ _m->Val[i][j] += m._m->Val[i][j];
+ return *this;
+}
+
+// combined subtraction and assignment operator
+MAT_TEMPLATE inline matrixT&
+matrixT::operator -= (const matrixT& m) _THROW_MATRIX_ERROR
+{
+ if (_m->Row != m._m->Row || _m->Col != m._m->Col)
+ REPORT_ERROR( "matrixT::operator-= : Inconsistent matrix sizes in subtraction!");
+ if (_m->Refcnt > 1) clone();
+ for (size_t i=0; i < m._m->Row; i++)
+ for (size_t j=0; j < m._m->Col; j++)
+ _m->Val[i][j] -= m._m->Val[i][j];
+ return *this;
+}
+
+// combined scalar multiplication and assignment operator
+MAT_TEMPLATE inline matrixT&
+matrixT::operator *= (const T& c) _NO_THROW
+{
+ if (_m->Refcnt > 1) clone();
+ for (size_t i=0; i < _m->Row; i++)
+ for (size_t j=0; j < _m->Col; j++)
+ _m->Val[i][j] *= c;
+ return *this;
+}
+
+// combined matrix multiplication and assignment operator
+MAT_TEMPLATE inline matrixT&
+matrixT::operator *= (const matrixT& m) _THROW_MATRIX_ERROR
+{
+ if (_m->Col != m._m->Row)
+ REPORT_ERROR( "matrixT::operator*= : Inconsistent matrix sizes in multiplication!");
+
+ matrixT temp(_m->Row,m._m->Col);
+
+ for (size_t i=0; i < _m->Row; i++)
+ for (size_t j=0; j < m._m->Col; j++)
+ {
+ temp._m->Val[i][j] = T(0);
+ for (size_t k=0; k < _m->Col; k++)
+ temp._m->Val[i][j] += _m->Val[i][k] * m._m->Val[k][j];
+ }
+ *this = temp;
+
+ return *this;
+}
+
+// combined scalar division and assignment operator
+MAT_TEMPLATE inline matrixT&
+matrixT::operator /= (const T& c) _NO_THROW
+{
+ if (_m->Refcnt > 1) clone();
+ for (size_t i=0; i < _m->Row; i++)
+ for (size_t j=0; j < _m->Col; j++)
+ _m->Val[i][j] /= c;
+
+ return *this;
+}
+
+// combined power and assignment operator
+MAT_TEMPLATE inline matrixT&
+matrixT::operator ^= (const size_t& pow) _THROW_MATRIX_ERROR
+{
+ matrixT temp(*this);
+
+ for (size_t i=2; i <= pow; i++)
+ *this = *this * temp;
+
+ return *this;
+}
+
+// unary negation operator
+MAT_TEMPLATE inline matrixT
+matrixT::operator - () _NO_THROW
+{
+ matrixT temp(_m->Row,_m->Col);
+
+ for (size_t i=0; i < _m->Row; i++)
+ for (size_t j=0; j < _m->Col; j++)
+ temp._m->Val[i][j] = - _m->Val[i][j];
+
+ return temp;
+}
+
+// binary addition operator
+MAT_TEMPLATE inline matrixT
+operator + (const matrixT& m1, const matrixT& m2) _THROW_MATRIX_ERROR
+{
+ matrixT temp = m1;
+ temp += m2;
+ return temp;
+}
+
+// binary subtraction operator
+MAT_TEMPLATE inline matrixT
+operator - (const matrixT& m1, const matrixT& m2) _THROW_MATRIX_ERROR
+{
+ matrixT temp = m1;
+ temp -= m2;
+ return temp;
+}
+
+// binary scalar multiplication operator
+MAT_TEMPLATE inline matrixT
+operator * (const matrixT& m, const T& no) _NO_THROW
+{
+ matrixT temp = m;
+ temp *= no;
+ return temp;
+}
+
+
+// binary scalar multiplication operator
+MAT_TEMPLATE inline matrixT
+operator * (const T& no, const matrixT& m) _NO_THROW
+{
+ return (m * no);
+}
+
+// binary matrix multiplication operator
+MAT_TEMPLATE inline matrixT
+operator * (const matrixT& m1, const matrixT& m2) _THROW_MATRIX_ERROR
+{
+ matrixT temp = m1;
+ temp *= m2;
+ return temp;
+}
+
+// binary scalar division operator
+MAT_TEMPLATE inline matrixT
+operator / (const matrixT& m, const T& no) _NO_THROW
+{
+ return (m * (T(1) / no));
+}
+
+
+// binary scalar division operator
+MAT_TEMPLATE inline matrixT
+operator / (const T& no, const matrixT& m) _THROW_MATRIX_ERROR
+{
+ return (!m * no);
+}
+
+// binary matrix division operator
+MAT_TEMPLATE inline matrixT
+operator / (const matrixT& m1, const matrixT& m2) _THROW_MATRIX_ERROR
+{
+ return (m1 * !m2);
+}
+
+// binary power operator
+MAT_TEMPLATE inline matrixT
+operator ^ (const matrixT& m, const size_t& pow) _THROW_MATRIX_ERROR
+{
+ matrixT temp = m;
+ temp ^= pow;
+ return temp;
+}
+
+// unary transpose operator
+MAT_TEMPLATE inline matrixT
+operator ~ (const matrixT& m) _NO_THROW
+{
+ matrixT temp(m.ColNo(),m.RowNo());
+
+ for (size_t i=0; i < m.RowNo(); i++)
+ for (size_t j=0; j < m.ColNo(); j++)
+ {
+ T x = m(i,j);
+ temp(j,i) = x;
+ }
+ return temp;
+}
+
+// unary inversion operator
+MAT_TEMPLATE inline matrixT
+operator ! (const matrixT m) _THROW_MATRIX_ERROR
+{
+ matrixT temp = m;
+ return temp.Inv();
+}
+
+// inversion function
+MAT_TEMPLATE inline matrixT
+matrixT::Inv () _THROW_MATRIX_ERROR
+{
+ size_t i,j,k;
+ T a1,a2,*rowptr;
+
+ if (_m->Row != _m->Col)
+ REPORT_ERROR( "matrixT::operator!: Inversion of a non-square matrix");
+
+ matrixT temp(_m->Row,_m->Col);
+ if (_m->Refcnt > 1) clone();
+
+
+ temp.Unit();
+ for (k=0; k < _m->Row; k++)
+ {
+ int indx = pivot(k);
+ if (indx == -1)
+ REPORT_ERROR( "matrixT::operator!: Inversion of a singular matrix");
+
+ if (indx != 0)
+ {
+ rowptr = temp._m->Val[k];
+ temp._m->Val[k] = temp._m->Val[indx];
+ temp._m->Val[indx] = rowptr;
+ }
+ a1 = _m->Val[k][k];
+ for (j=0; j < _m->Row; j++)
+ {
+ _m->Val[k][j] /= a1;
+ temp._m->Val[k][j] /= a1;
+ }
+ for (i=0; i < _m->Row; i++)
+ if (i != k)
+ {
+ a2 = _m->Val[i][k];
+ for (j=0; j < _m->Row; j++)
+ {
+ _m->Val[i][j] -= a2 * _m->Val[k][j];
+ temp._m->Val[i][j] -= a2 * temp._m->Val[k][j];
+ }
+ }
+ }
+ return temp;
+}
+
+// solve simultaneous equation
+MAT_TEMPLATE inline matrixT
+matrixT::Solve (const matrixT& v) const _THROW_MATRIX_ERROR
+{
+ size_t i,j,k;
+ T a1;
+
+ if (!(_m->Row == _m->Col && _m->Col == v._m->Row))
+ REPORT_ERROR( "matrixT::Solve():Inconsistent matrices!");
+
+ matrixT temp(_m->Row,_m->Col+v._m->Col);
+ for (i=0; i < _m->Row; i++)
+ {
+ for (j=0; j < _m->Col; j++)
+ temp._m->Val[i][j] = _m->Val[i][j];
+ for (k=0; k < v._m->Col; k++)
+ temp._m->Val[i][_m->Col+k] = v._m->Val[i][k];
+ }
+ for (k=0; k < _m->Row; k++)
+ {
+ int indx = temp.pivot(k);
+ if (indx == -1)
+ REPORT_ERROR( "matrixT::Solve(): Singular matrix!");
+
+ a1 = temp._m->Val[k][k];
+ for (j=k; j < temp._m->Col; j++)
+ temp._m->Val[k][j] /= a1;
+
+ for (i=k+1; i < _m->Row; i++)
+ {
+ a1 = temp._m->Val[i][k];
+ for (j=k; j < temp._m->Col; j++)
+ temp._m->Val[i][j] -= a1 * temp._m->Val[k][j];
+ }
+ }
+ matrixT s(v._m->Row,v._m->Col);
+ for (k=0; k < v._m->Col; k++)
+ for (int m=int(_m->Row)-1; m >= 0; m--)
+ {
+ s._m->Val[m][k] = temp._m->Val[m][_m->Col+k];
+ for (j=m+1; j < _m->Col; j++)
+ s._m->Val[m][k] -= temp._m->Val[m][j] * s._m->Val[j][k];
+ }
+ return s;
+}
+
+// set zero to all elements of this matrix
+MAT_TEMPLATE inline void
+matrixT::Null (const size_t& row, const size_t& col) _NO_THROW
+{
+ if (row != _m->Row || col != _m->Col)
+ realloc( row,col);
+
+ if (_m->Refcnt > 1)
+ clone();
+
+ for (size_t i=0; i < _m->Row; i++)
+ for (size_t j=0; j < _m->Col; j++)
+ _m->Val[i][j] = T(0);
+ return;
+}
+
+// set zero to all elements of this matrix
+MAT_TEMPLATE inline void
+matrixT::Null() _NO_THROW
+{
+ if (_m->Refcnt > 1) clone();
+ for (size_t i=0; i < _m->Row; i++)
+ for (size_t j=0; j < _m->Col; j++)
+ _m->Val[i][j] = T(0);
+ return;
+}
+
+// set this matrix to unity
+MAT_TEMPLATE inline void
+matrixT::Unit (const size_t& row) _NO_THROW
+{
+ if (row != _m->Row || row != _m->Col)
+ realloc( row, row);
+
+ if (_m->Refcnt > 1)
+ clone();
+
+ for (size_t i=0; i < _m->Row; i++)
+ for (size_t j=0; j < _m->Col; j++)
+ _m->Val[i][j] = i == j ? T(1) : T(0);
+ return;
+}
+
+// set this matrix to unity
+MAT_TEMPLATE inline void
+matrixT::Unit () _NO_THROW
+{
+ if (_m->Refcnt > 1) clone();
+ size_t row = min(_m->Row,_m->Col);
+ _m->Row = _m->Col = row;
+
+ for (size_t i=0; i < _m->Row; i++)
+ for (size_t j=0; j < _m->Col; j++)
+ _m->Val[i][j] = i == j ? T(1) : T(0);
+ return;
+}
+
+// private partial pivoting method
+MAT_TEMPLATE inline int
+matrixT::pivot (size_t row)
+{
+ int k = int(row);
+ double amax,temp;
+
+ amax = -1;
+ for (size_t i=row; i < _m->Row; i++)
+ if ( (temp = abs( _m->Val[i][row])) > amax && temp != 0.0)
+ {
+ amax = temp;
+ k = i;
+ }
+ if (_m->Val[k][row] == T(0))
+ return -1;
+ if (k != int(row))
+ {
+ T* rowptr = _m->Val[k];
+ _m->Val[k] = _m->Val[row];
+ _m->Val[row] = rowptr;
+ return k;
+ }
+ return 0;
+}
+
+// calculate the determinant of a matrix
+MAT_TEMPLATE inline T
+matrixT::Det () const _THROW_MATRIX_ERROR
+{
+ size_t i,j,k;
+ T piv,detVal = T(1);
+
+ if (_m->Row != _m->Col)
+ REPORT_ERROR( "matrixT::Det(): Determinant a non-square matrix!");
+
+ matrixT temp(*this);
+ if (temp._m->Refcnt > 1) temp.clone();
+
+ for (k=0; k < _m->Row; k++)
+ {
+ int indx = temp.pivot(k);
+ if (indx == -1)
+ return 0;
+ if (indx != 0)
+ detVal = - detVal;
+ detVal = detVal * temp._m->Val[k][k];
+ for (i=k+1; i < _m->Row; i++)
+ {
+ piv = temp._m->Val[i][k] / temp._m->Val[k][k];
+ for (j=k+1; j < _m->Row; j++)
+ temp._m->Val[i][j] -= piv * temp._m->Val[k][j];
+ }
+ }
+ return detVal;
+}
+
+// calculate the norm of a matrix
+MAT_TEMPLATE inline T
+matrixT::Norm () _NO_THROW
+{
+ T retVal = T(0);
+
+ for (size_t i=0; i < _m->Row; i++)
+ for (size_t j=0; j < _m->Col; j++)
+ retVal += _m->Val[i][j] * _m->Val[i][j];
+ retVal = sqrt( retVal);
+
+ return retVal;
+}
+
+// calculate the condition number of a matrix
+MAT_TEMPLATE inline T
+matrixT::Cond () _NO_THROW
+{
+ matrixT inv = ! (*this);
+ return (Norm() * inv.Norm());
+}
+
+// calculate the cofactor of a matrix for a given element
+MAT_TEMPLATE inline T
+matrixT::Cofact (size_t row, size_t col) _THROW_MATRIX_ERROR
+{
+ size_t i,i1,j,j1;
+
+ if (_m->Row != _m->Col)
+ REPORT_ERROR( "matrixT::Cofact(): Cofactor of a non-square matrix!");
+
+ if (row > _m->Row || col > _m->Col)
+ REPORT_ERROR( "matrixT::Cofact(): Index out of range!");
+
+ matrixT temp (_m->Row-1,_m->Col-1);
+
+ for (i=i1=0; i < _m->Row; i++)
+ {
+ if (i == row)
+ continue;
+ for (j=j1=0; j < _m->Col; j++)
+ {
+ if (j == col)
+ continue;
+ temp._m->Val[i1][j1] = _m->Val[i][j];
+ j1++;
+ }
+ i1++;
+ }
+ T cof = temp.Det();
+ if ((row+col)%2 == 1)
+ cof = -cof;
+
+ return cof;
+}
+
+
+// calculate adjoin of a matrix
+MAT_TEMPLATE inline matrixT
+matrixT::Adj () _THROW_MATRIX_ERROR
+{
+ if (_m->Row != _m->Col)
+ REPORT_ERROR( "matrixT::Adj(): Adjoin of a non-square matrix.");
+
+ matrixT temp(_m->Row,_m->Col);
+
+ for (size_t i=0; i < _m->Row; i++)
+ for (size_t j=0; j < _m->Col; j++)
+ temp._m->Val[j][i] = Cofact(i,j);
+ return temp;
+}
+
+// Determine if the matrix is singular
+MAT_TEMPLATE inline bool
+matrixT::IsSingular () _NO_THROW
+{
+ if (_m->Row != _m->Col)
+ return false;
+ return (Det() == T(0));
+}
+
+// Determine if the matrix is diagonal
+MAT_TEMPLATE inline bool
+matrixT::IsDiagonal () _NO_THROW
+{
+ if (_m->Row != _m->Col)
+ return false;
+ for (size_t i=0; i < _m->Row; i++)
+ for (size_t j=0; j < _m->Col; j++)
+ if (i != j && _m->Val[i][j] != T(0))
+ return false;
+ return true;
+}
+
+// Determine if the matrix is scalar
+MAT_TEMPLATE inline bool
+matrixT::IsScalar () _NO_THROW
+{
+ if (!IsDiagonal())
+ return false;
+ T v = _m->Val[0][0];
+ for (size_t i=1; i < _m->Row; i++)
+ if (_m->Val[i][i] != v)
+ return false;
+ return true;
+}
+
+// Determine if the matrix is a unit matrix
+MAT_TEMPLATE inline bool
+matrixT::IsUnit () _NO_THROW
+{
+ if (IsScalar() && _m->Val[0][0] == T(1))
+ return true;
+ return false;
+}
+
+// Determine if this is a null matrix
+MAT_TEMPLATE inline bool
+matrixT::IsNull () _NO_THROW
+{
+ for (size_t i=0; i < _m->Row; i++)
+ for (size_t j=0; j < _m->Col; j++)
+ if (_m->Val[i][j] != T(0))
+ return false;
+ return true;
+}
+
+// Determine if the matrix is symmetric
+MAT_TEMPLATE inline bool
+matrixT::IsSymmetric () _NO_THROW
+{
+ if (_m->Row != _m->Col)
+ return false;
+ for (size_t i=0; i < _m->Row; i++)
+ for (size_t j=0; j < _m->Col; j++)
+ if (_m->Val[i][j] != _m->Val[j][i])
+ return false;
+ return true;
+}
+
+// Determine if the matrix is skew-symmetric
+MAT_TEMPLATE inline bool
+matrixT::IsSkewSymmetric () _NO_THROW
+{
+ if (_m->Row != _m->Col)
+ return false;
+ for (size_t i=0; i < _m->Row; i++)
+ for (size_t j=0; j < _m->Col; j++)
+ if (_m->Val[i][j] != -_m->Val[j][i])
+ return false;
+ return true;
+}
+
+// Determine if the matrix is upper triangular
+MAT_TEMPLATE inline bool
+matrixT::IsUpperTriangular () _NO_THROW
+{
+ if (_m->Row != _m->Col)
+ return false;
+ for (size_t i=1; i < _m->Row; i++)
+ for (size_t j=0; j < i-1; j++)
+ if (_m->Val[i][j] != T(0))
+ return false;
+ return true;
+}
+
+// Determine if the matrix is lower triangular
+MAT_TEMPLATE inline bool
+matrixT::IsLowerTriangular () _NO_THROW
+{
+ if (_m->Row != _m->Col)
+ return false;
+
+ for (size_t j=1; j < _m->Col; j++)
+ for (size_t i=0; i < j-1; i++)
+ if (_m->Val[i][j] != T(0))
+ return false;
+
+ return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////
+MAT_TEMPLATE inline matrixT
+matrixT::Extract (const size_t r, const size_t c, const size_t rows, const size_t cols) _THROW_MATRIX_ERROR
+{
+// if ((r < 0) || (c < 0))
+// REPORT_ERROR( "matrixT::Extract(): Index must be positive or zero.");
+
+ if (((r+rows) > _m->Row ) || ((c+cols) > _m->Col))
+ REPORT_ERROR( "matrixT::Extract(): Overflow");
+
+ matrixT temp(rows, cols);
+
+ for (size_t i=0; i<rows; i++)
+ for (size_t j=0; j<cols; j++)
+ temp._m->Val[i][j] = _m->Val[r+i][c+j];
+
+ return temp;
+}
+
+MAT_TEMPLATE inline void
+matrixT::Include (const matrixT& small, const size_t r, const size_t c) _THROW_MATRIX_ERROR
+{
+// if ((r < 0) || (c < 0))
+// REPORT_ERROR( "matrixT::Include(): Index must be positive or zero.");
+
+ size_t newRows = _m->Row;
+ size_t newCols = _m->Col;
+
+ if ((r+small._m->Row) > _m->Row)
+ newRows = r+small._m->Row;
+ if ((c+small._m->Col) > _m->Col)
+ newCols = c+small._m->Col;
+
+ SetSize(newRows, newCols);
+
+ for (size_t i=0; i<small._m->Row; i++)
+ for (size_t j=0; j<small._m->Col; j++)
+ _m->Val[r+i][c+j] = small._m->Val[i][j];
+
+}
+
+#ifndef _NO_NAMESPACE
+}
+#endif
+
+#endif //__STD_MATRIX_H
Added: code/player/trunk/server/drivers/localization/ekfvloc/params.cc
===================================================================
--- code/player/trunk/server/drivers/localization/ekfvloc/params.cc (rev 0)
+++ code/player/trunk/server/drivers/localization/ekfvloc/params.cc 2010-12-12 05:49:13 UTC (rev 9007)
@@ -0,0 +1,39 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2010
+ * Alejandro R. Mosteo
+ *
+ *
+ * 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
+ */
+
+
+#include <math.h>
+
+#include "params.hh"
+
+double kMaxEmptyAngle = 2.0 * M_PI / 180.0;
+double kMaxEmptyDistance = 0.1;
+double kMinRegionLength = 0.2;
+int kMinPointsInRegion = 8;
+int kMinPointsInSegment = 5;
+double kConfidence = 95.0;
+double kAlphaILF(void) { return 1000.0 - 10 * kConfidence; }
+bool kCheckResidual = false;
@@ Diff output truncated at 100000 characters. @@
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
Thread View
| Thread | Author | Date |
|---|---|---|
| [Playerstage-commit] SF.net SVN: playerstage:[9007] code/player/trunk/server/drivers/ localization | <jpgr87@us...> |