Initial Import

This is the import of basically everything as of Winter, 2014.

28sig

@@ -0,0 +1,5 @@
+for r_i = 2.8 sig_0
+noise	30	35	40	45	50	55	60	65	70	75	80	85	90
+sigs	0.27416	0.37316	0.48739	0.61685	0.76154	0.92147	1.0966	1.287	1.4926	1.7135	1.9496	2.2009	2.4674
+sigb	0.40171	0.50071	0.61494	0.7444	0.88909	1.049	1.2242	1.4146	1.6202	1.841	2.0771	2.3284	2.595
+n	6.0273	7.8313	9.9052	12.251	14.869	17.761	20.927	24.367	28.081	32.07	36.333	40.871	45.684

CHANGELOG

@@ -0,0 +1,63 @@
+
+Purpose:
+*As of 1817 this is a LIBRARY-ONLY package.
+*1831 updated API and removed old executables
+*1848 Added ambiguity to IWLS_2D
+*1848 Version used in Staring July 18th 2012
+
+exports: lib/ltracking.so 
+*programatic access to the EKF and various batch estimators for bearing-only tracking. 
+
+
+Future revisions: 
+*include range-only and hybrid tracking.
+*include Active Localization algorithms as static libraries
+
+
+
+======================================================================
+include: tagloc/tracking.h
+
+
+A brief summary is:
+
+Estimates the circumcircle from a list of points on the perimiter.
+void RSN::circumcircle(double px[], double py[], size_t N, double &centerx, double &centery, double& radius);
+
+A maximum likelihood gradient ascent algorithm. Input: All measurements, Output: ML estimate
+void RSN::BOT::Ml_Grad_Asc_2D(const int n, const double sensors[][2], const double bearing_global_radians[], const double sensor_sigma[],double t[2], double target_cov[4], const bool ambig=false, const int max_iters=10000);
+
+An iterated weighted least squares estimation. Input: all measurements, Output: ML estimate + covariance
+void RSN::BOT::IWLS_2D(const int n, const double sensors[][2], const double bearing_global_radians[], const double sensor_sigma[], double t[2], double target_cov[4], const bool ambig=false, const double min_step=.01, const int max_iters=20);
+
+An iterated EKF routine. Input: Prior and current measurement, Output: Posterior (MAP) estimate and covariance. SHOULD experience less linearization error.
+void RSN::BOT::IEKF_2D(const double sensors[2], const double bearing_global_radians, const double sensor_sigma, double t[2], double target_cov[4], const bool ambig=false, const double min_step=.01, const int max_iters=10);
+
+A vanilla EKF update routine. Input: Prior and current measurement, Output: Posterior (MAP) estimate and covariance
+void RSN::BOT::EKF_UP_2D(const double sensor[2], const double bearing_global_radians, const double sensor_sigma, double t[2], double target_cov[4], const bool ambig=false);
+
+=======================================================================
+
+
+Changelog is as follows for this version:
+------------------------------------------------------------------------
+r1817 | jvander | 2012-07-15 21:31:27 -0500 (Sun, 15 Jul 2012) | 1 line
+
+tagloc adjustments and deprications
+------------------------------------------------------------------------
+r1811 | jvander | 2012-07-15 19:28:35 -0500 (Sun, 15 Jul 2012) | 1 line
+
+touched up tracking library
+------------------------------------------------------------------------
+r1803 | jvander | 2012-07-13 11:33:34 -0500 (Fri, 13 Jul 2012) | 1 line
+
+checkpoint: estimators done
+------------------------------------------------------------------------
+r1798 | jvander | 2012-07-11 23:52:11 -0500 (Wed, 11 Jul 2012) | 1 line
+
+tagloc changes. It's worthless with new namespaces. This checkin is safe (will build) before I check in new init / estimation code
+------------------------------------------------------------------------
+r1738 | tokekar | 2012-07-05 14:20:58 -0500 (Thu, 05 Jul 2012) | 1 line
+
+moving things to a stack; code compiles locally, hopefully things don't break, else you know where to revert
+------------------------------------------------------------------------

CMakeLists.txt

@@ -0,0 +1,124 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+find_package(cmake_modules REQUIRED)
+
+#list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../cmake)
+find_package(cmake_modules REQUIRED)
+find_package(Eigen REQUIRED)
+include_directories(${Eigen_INCLUDE_DIRS})
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+rosbuild_add_boost_directories()
+
+#uncomment if you have defined messages
+rosbuild_genmsg()
+#uncomment if you have defined services
+rosbuild_gensrv()
+
+rosbuild_add_library(tracking src/tracking.cpp src/algorithms.cpp)
+#set_target_properties(tracking PROPERTIES COMPILE_DEFINITIONS "__TL_VERBOSE")
+
+#                      EXECUTABLES
+rosbuild_add_executable(estimate src/estimator.cpp)
+target_link_libraries(estimate tracking)
+rosbuild_link_boost(estimate program_options)
+
+#rosbuild_add_executable(estimate_server src/estimator_server.cpp)
+#target_link_libraries(estimate_server tracking)
+
+#rosbuild_add_executable(onestep src/onestep.cpp)
+#target_link_libraries(onestep tracking)
+
+#rosbuild_add_executable(multistep src/multistep.cpp)
+#target_link_libraries(multistep tracking)
+
+
+#rosbuild_add_executable(fakerend src/faker_rend.cpp)
+#rosbuild_add_executable(fakeout src/faker_out.cpp)
+
+
+#set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin/icra2013)
+rosbuild_add_executable(twostep_one src/twostep.cpp)
+target_link_libraries(twostep_one tracking)
+#set_target_properties(A PROPERTIES COMPILE_FLAGS "-DA")
+rosbuild_add_executable(twostep_other src/twostep.cpp)
+target_link_libraries(twostep_other tracking)
+set_target_properties(twostep_other PROPERTIES COMPILE_DEFINITIONS "MAKE_RECIP")
+
+rosbuild_add_executable(iwls_prior src/iwls_prior.cpp)
+target_link_libraries(iwls_prior tracking)
+
+#rosbuild_add_executable(icra2013 src/icratest.cpp)
+#target_link_libraries(icra2013 tracking)
+#rosbuild_add_executable(icra2013A src/icra_Atest.cpp)
+#target_link_libraries(icra2013A tracking)
+#rosbuild_add_executable(icra2013sim src/icra_sims.cpp)
+#target_link_libraries(icra2013sim tracking)
+
+#                      EXECUTABLES         OUT OF DATE
+##rosbuild_add_executable(cautious_greedy_server src/server.cpp)
+##rosbuild_add_executable(mutl src/newserver.cpp)
+##rosbuild_add_executable(client src/client.cpp)
+##rosbuild_add_executable(init_server src/init.cpp)
+#
+
+#   TESTS FOR THE ABOVE EXECUTABLES AND LIBRARIES
+#rosbuild_add_executable(twostep_test src/twosteptest.cpp)
+#target_link_libraries(twostep_test tracking)
+#set_target_properties(twostep_test PROPERTIES EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin/tests)
+
+#rosbuild_add_executable(rosiwlstest src/rosiwlstest.cpp)
+#target_link_libraries(rosiwlstest tracking)
+
+#rosbuild_add_executable(ellipsetest src/ellipsetest.cpp)
+#target_link_libraries(ellipsetest tracking)
+
+#rosbuild_add_executable(ost src/onesteptest.cpp)
+#target_link_libraries(ost tracking)
+
+#BROKEN
+#rosbuild_add_executable(rosiwlstest src/rosiwlstest.cpp )
+#target_link_libraries(rosiwlstest tracking)
+
+#rosbuild_add_executable(lit src/lineinttest.cpp )
+#target_link_libraries(lit tracking)
+
+#rosbuild_add_executable(cct src/circumcircletest.cpp )
+#target_link_libraries(cct tracking)
+
+#rosbuild_add_executable(batchtest src/batchtest.cpp)
+#target_link_libraries(batchtest tracking)
+
+#rosbuild_add_executable(ekftest src/ekftest.cpp )
+#target_link_libraries(ekftest tracking)
+
+#rosbuild_add_executable(iekftest src/iekftest.cpp )
+#target_link_libraries(iekftest tracking)
+
+#MAYBE BROKEN
+#rosbuild_add_executable(iwlstest src/iwlstest.cpp )
+#target_link_libraries(iwlstest tracking)
+
+#MIGHT ALSO BE BROKEN
+#rosbuild_add_executable(iwlstestprior src/iwlstest_prior.cpp )
+#target_link_libraries(iwlstestprior tracking)
+
+##             OTHER COMMANDS
+#common commands for building c++ executables and libraries
+#target_link_libraries(cct tracking)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#target_link_libraries(example ${PROJECT_NAME})

Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

clean_log.sh

@@ -0,0 +1,11 @@
+#!/bin/bash
+pdir=$(pwd)
+cd `rospack find tagloc`
+if [ -f 'tagloc_log' ]
+then
+	echo "Clearing" >> .old_log
+	date >> .old_log
+	cat tagloc_log >> .old_log
+	rm tagloc_log
+fi
+cd "$pdir"

expt.sh

@@ -0,0 +1,12 @@
+#!/bin/bash
+#bin/server
+rosparam set /tagloc/sigs .25
+rosparam set /tagloc/sigb 1
+rosparam set /tagloc/Cd 20
+screen -dm -S tagloc rosrun tagloc server
+rosrun tagloc client set 49691 -10 -40 900 0 0 800
+rosrun tagloc client set 48971 -10 -40 900 0 0 850
+rosrun tagloc client set 48671 -10 -40 850 0 0 900
+rosrun tagloc client set 10 3 -40 900 -100 -100 900
+rosrun tagloc client get 48971
+rosrun tagloc client get 49691

include/tagloc/algorithms.h

@@ -0,0 +1,13 @@
+#ifndef RSN_TRACKING_ALGORITHMS_H
+#define RSN_TRACKING_ALGORITHMS_H
+
+#include <tagloc/tracking.h>
+#include <Eigen/Dense>
+
+namespace RSN{
+	void onestep_circle(Eigen::MatrixXd &robot_1_world, Eigen::MatrixXd &robot_2_world,Eigen::MatrixXd target_world, double circle_radius, double sensor_variance, double desired_min_info, double scalar=1);
+
+	void onestep(Eigen::MatrixXd &robot_1_world, Eigen::MatrixXd &robot_2_world,Eigen::MatrixXd target_world, Eigen::MatrixXd covariance_matrix, double sensor_variance, double desired_min_info, double scalar=1);
+}
+
+#endif

include/tagloc/tagloc.h

@@ -0,0 +1,2 @@
+#include <tagloc/tracking.h>
+#warning tagloc.h is currently non-functional. Including tracking.h instead.

include/tagloc/tracking.h

@@ -0,0 +1,76 @@
+#ifndef TAGLOC_BEARING
+#define TAGLOC_BEARING
+
+#define EIGEN2_SUPPORT
+#include <math.h>
+#include <helpers/math.h>
+#include <Eigen/Dense>
+#include <vector>
+
+#define RSN_BEARING (1)
+#define RSN_RANGE (2)
+#define RSN_RANGE_PLUS_BEARING (3)
+
+namespace RSN{
+
+/*
+		void IWLS_2D(const int n, std::vector<std::vector<double> > sensors, const double bearing_global_radians[], const double sensor_sigma[], double t[2], double target_cov[4], const bool ambig=false, const double min_step=.01, const int max_iters=100);
+	void IWLS_2D(const int n, const double sensors[][2], const double measurements[], const int measurement_type[], const double sensor_sigma[], double t[2], double target_cov[4], const bool ambig=false, const double min_step=.01, const int max_iters=100);
+	void Ml_Grad_Asc_2D(const int n, const double sensors[][2], const double measurements[], const int measurement_type[], const double sensor_sigma[],double t[2], double target_cov[4], const bool ambig=false, const int max_iters=10000);
+	void IEKF_2D(const double sensors[2], const double measurements[], const int measurement_type[], const double sensor_sigma, double t[2], double target_cov[4], const bool ambig=false, const double min_step=.01, const int max_iters=10);
+	void EKF_UP_2D(const double sensor[2], const double bearing_global_radians, const int measurement_type, const double sensor_sigma, double t[2], double target_cov[4], const bool ambig=false);
+	Eigen::MatrixXd get_S(const double sensor[2],const int sensor_type, const double target[2],const double cov[4],const double sensor_sigma);
+	Eigen::MatrixXd get_H(const Eigen::MatrixXd sensor,const Eigen::MatrixXd sensor_type, const Eigen::MatrixXd target);
+	Eigen::MatrixXd get_H(const double sensor[2], const int sensor_type, const double target[2]);
+	Eigen::MatrixXd get_FIM(const Eigen::MatrixXd col_wise_sensors, const Eigen::MatixXd vector_of_types, const Eigen::MatrixXd vector_of_sigma, const Eigen::MatrixXd target);
+	*/
+
+	bool in_ellipse(const Eigen::MatrixXd center_pt, const Eigen::MatrixXd Ellipse_Matrix, const Eigen::MatrixXd query_pt, const double scalar=1);
+	double distance_to_ellipse(const Eigen::MatrixXd center_pt, const Eigen::MatrixXd Ellipse_Matrix, const Eigen::MatrixXd query_pt, const double scalar=1);
+	Eigen::MatrixXd closest_pt_ellipse(const Eigen::MatrixXd center_pt, const Eigen::MatrixXd Cov_Matrix, const Eigen::MatrixXd query_pt, const double scalar=1);
+	void circumcircle(const size_t N, double px[], double py[], double &centerx, double &centery, double& radius);
+	void circumcircle(const size_t N, const std::vector<std::vector<double> >p, double &centerx, double &centery, double& radius);
+	void circumcircle(const size_t N, const std::vector<double> px, std::vector<double> py, double &centerx, double &centery, double& radius);
+	void line_intersection(const size_t N, double p[][2] ,double th[], double &px, double &py,double c[4]=NULL);
+	void line_intersection(const size_t N, const std::vector<std::vector<double> >p, std::vector<double> th, double &px, double &py,double c[4]=NULL);
+	void line_intersection(const size_t N, const std::vector<double> px, std::vector<double> py, std::vector<double> th, double &opx, double &opy,double c[4]=NULL);
+
+	Eigen::MatrixXd getCovMatrix(const double covariance[4]);
+
+	/**
+	 * BEARING-ONLY Methods
+	 */
+	namespace BOT{
+		void Ml_Grad_Asc_2D(const int n, const std::vector<double> sx, const std::vector<double> sy, const std::vector<double> bearing_global_radians, const std::vector<double> sensor_sigma,double t[2], double target_cov[4], const bool ambig=false, const int max_iters=10000);
+		void Ml_Grad_Asc_2D(const int n, const double sensors[][2], const double bearing_global_radians[], const double sensor_sigma[],double t[2], double target_cov[4], const bool ambig=false, const int max_iters=10000);
+		void Ml_Grad_Asc_2D(const int n, const std::vector<std::vector<double> >sensors, const std::vector<double> bearing_global_radians, const std::vector<double> sensor_sigma,double t[2], double target_cov[4], const bool ambig=false, const int max_iters=10000);
+
+		void IWLS_2D(const int n, const std::vector<double>sx, const std::vector<double> sy, const std::vector<double> bearing_global_radians, const std::vector<double> sensor_sigma,double target[2], double target_cov[4], const bool ambig=false, double min_step=.01, int max_iters=20);
+		void IWLS_2D(const int n, const std::vector<std::vector<double> >sensors, const std::vector<double> bearing_global_radians, const std::vector<double> sensor_sigma,double target[2], double target_cov[4], const bool ambig=false, double min_step=.01, int max_iters=20);
+		void IWLS_2D(const int n, const double sensors[][2], const double bearing_global_radians[], const double sensor_sigma[], double t[2], double target_cov[4], const bool ambig=false, const double min_step=.01, const int max_iters=20);
+
+		void IEKF_2D(const double sensors[2], const double bearing_global_radians, const double sensor_sigma, double t[2], double target_cov[4], const bool ambig=false, const double min_step=.01, const int max_iters=10);
+		void IEKF_2D(const double sensors_x, const double sensor_y, const double bearing_global_radians, const double sensor_sigma, double t[2], double target_cov[4], const bool ambig=false, const double min_step=.01, const int max_iters=10);
+		void EKF_UP_2D(const double sensor[2], const double bearing_global_radians, const double sensor_sigma, double t[2], double target_cov[4], const bool ambig=false);
+		void EKF_UP_2D(const double sensor_x, const double sensor_y, const double bearing_global_radians, const double sensor_sigma, double t[2], double target_cov[4], const bool ambig=false);
+		Eigen::MatrixXd get_S(const double sensor[2],const double target[2],const double cov[4],const double sensor_sigma);
+		Eigen::MatrixXd get_S(const Eigen::MatrixXd sensor,const Eigen::MatrixXd target, const Eigen::MatrixXd cov,const double sensor_sigma);
+		Eigen::MatrixXd get_H(const double sensor[2],const double target[2]);
+		Eigen::MatrixXd get_H(const Eigen::MatrixXd sensor,const Eigen::MatrixXd target);
+		Eigen::MatrixXd get_FIM(const Eigen::MatrixXd col_wise_sensors, const Eigen::MatrixXd vector_of_sigma, const Eigen::MatrixXd target);
+	}
+
+
+	/**
+	 * RANGE-ONLY Methods
+	 */
+
+	/*
+	namespace ROT{
+		void IWLS_2D(const int n, const double sensors[][2], const double range[], const double sensor_sigma[], double t[2], double target_cov[4], const bool ambig=false, const double min_step=.01, const int max_iters=100);
+	}*/
+
+
+}
+
+#endif

mainpage.dox

@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b tagloc is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/

manifest.xml

@@ -0,0 +1,28 @@
+<package>
+  <description brief="tagloc">
+
+     tagloc
+
+  </description>
+  <author></author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/tagloc</url>
+	<export>
+		<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -ltracking"/>
+	</export>
+	<!--<depend package="eigen"/>-->
+	<rosdep name="eigen3" />
+	<rosdep name="eigen2" />
+	<rosdep name="eigen" />
+	<depend package="helpers"/>
+	<depend package="paramon"/>
+	<depend package="control"/>
+	<depend package="bearing_measurement"/>
+	<depend package="geometry_msgs"/>
+	<depend package="network"/>
+	<depend package="std_srvs"/>
+
+</package>
+
+

msg/Tag.msg

@@ -0,0 +1,6 @@
+#Target frequency
+int32 frequency
+#target pose estimate
+geometry_msgs/Pose2D target_pose
+#target covariance
+float64[4] target_cov

mvtest.sh

@@ -0,0 +1,5 @@
+#!/bin/bash
+bin/client set 10 20 0 8 0 0 1000
+echo "prior:"
+bin/client get 10
+bin/client step 10