Permalink
Browse files

starting writing unit tests for fastslam1.0

  • Loading branch information...
1 parent 7ff8673 commit 028fc05962af8c8a3895e65a2563b92b16550d88 @yglee committed Sep 6, 2012
View
@@ -69,6 +69,7 @@ include_directories(${FSLAM_BINARY_DIR}/cpp)
add_subdirectory(cpp/core)
add_subdirectory(cpp/fastslam1)
add_subdirectory(cpp/fastslam2)
+add_subdirectory(cpp/gtest)
# Adding test directories
add_subdirectory(testcode)
View
@@ -20,7 +20,7 @@ endmacro(add_fslam_executable name)
macro(add_fslam_tool name)
add_fslam_executable(${name} ${ARGN})
if( FSLAM_BUILD_TOOLS )
- message( "Installing: " ${name} )
+ message(STATUS "Installing: " ${name} )
install(TARGETS ${name} RUNTIME DESTINATION bin)
endif()
set_target_properties(${name} PROPERTIES FOLDER "tools")
@@ -32,7 +32,7 @@ function(fslam_enable_testing)
include_directories(${GTEST_DIR}/include)
add_library(gtest SHARED EXCLUDE_FROM_ALL
${GTEST_DIR}/src/gtest-all.cc
- ${CMAKE_SOURCE_DIR}/testcode/test_main.cc
+ ${CMAKE_SOURCE_DIR}/cpp/gtest/test_main.cc
)
target_link_libraries(gtest
${VISIONWORKBENCH_CORE_LIBRARY}
@@ -78,4 +78,4 @@ function(fslam_add_test source_file)
# message("Added test name: " ${executable} "\t" ${test_name})
# add_test(${test_name} ${executable} --gtest_filter=${test_name})
# endforeach()
-endfunction(fslam_add_test)
+endfunction(fslam_add_test)
View
@@ -0,0 +1,40 @@
+# This is the CMakeCache file.
+# For build in directory: /Users/ylee8/FastSLAM/cmake
+# It was generated by CMake: /Users/ylee8/packages/local/bin/cmake
+# You can edit this file to change values found and used by cmake.
+# If you do not want to change any of the values, simply exit the editor.
+# If you do want to change a value, simply edit, save, and exit the editor.
+# The syntax for the file is as follows:
+# KEY:TYPE=VALUE
+# KEY is the name of a variable in the cache.
+# TYPE is a hint to GUI's for the type of VALUE, DO NOT EDIT TYPE!.
+# VALUE is the current value for the KEY.
+
+########################
+# EXTERNAL cache entries
+########################
+
+
+########################
+# INTERNAL cache entries
+########################
+
+//This is the directory where this CMakeCache.txt was created
+CMAKE_CACHEFILE_DIR:INTERNAL=/Users/ylee8/FastSLAM/cmake
+//Major version of cmake used to create the current loaded cache
+CMAKE_CACHE_MAJOR_VERSION:INTERNAL=2
+//Minor version of cmake used to create the current loaded cache
+CMAKE_CACHE_MINOR_VERSION:INTERNAL=8
+//Patch version of cmake used to create the current loaded cache
+CMAKE_CACHE_PATCH_VERSION:INTERNAL=9
+//Path to CMake executable.
+CMAKE_COMMAND:INTERNAL=/Users/ylee8/packages/local/bin/cmake
+//Path to cpack program executable.
+CMAKE_CPACK_COMMAND:INTERNAL=/Users/ylee8/packages/local/bin/cpack
+//Path to ctest program executable.
+CMAKE_CTEST_COMMAND:INTERNAL=/Users/ylee8/packages/local/bin/ctest
+//Path to cache edit program executable.
+CMAKE_EDIT_COMMAND:INTERNAL=/Users/ylee8/packages/local/bin/ccmake
+//Path to CMake installation.
+CMAKE_ROOT:INTERNAL=/Users/ylee8/packages/local/share/cmake-2.8
+
@@ -0,0 +1 @@
+# This file is generated by cmake for dependency checking of the CMakeCache.txt file
View
@@ -7,15 +7,16 @@ add_library(FastSLAM_core
add_control_noise.cpp compute_steering.cpp get_observations.cpp stratified_resample.cpp
add_feature.cpp configfile.cpp KF_cholesky_update.cpp multivariate_gauss.cpp resample_particles.cpp TransformToGlobal.cpp
add_observation_noise.cpp data_associate_known.cpp KF_joseph_update.cpp particle.cpp predict_true.cpp
- compute_jacobians.cpp feature_update.cpp line_plot_conversion.cpp pi_to_pi.cpp stratified_random.cpp
+ compute_jacobians.cpp feature_update.cpp line_plot_conversion.cpp pi_to_pi.cpp stratified_random.cpp
+ read_input_file.cpp
)
target_link_libraries(FastSLAM_core) # We don't depend on anything
install(FILES
add_control_noise.h compute_jacobians.h data_associate_known.h KF_cholesky_update.h multivariate_gauss.h stratified_random.h predict_true.h
add_feature.h compute_steering.h feature_update.h KF_joseph_update.h particle.h printMat.h stratified_resample.h
add_observation_noise.h configfile.h get_observations.h line_plot_conversion.h pi_to_pi.h resample_particles.h TransformToGlobal.h
- utilities.h
+ utilities.h read_input_file.h
DESTINATION include/fslam/core
)
@@ -0,0 +1,114 @@
+#include "read_input_file.h"
+#include <fstream>
+
+void read_input_file(const string s, MatrixXf *lm, MatrixXf *wp)
+{
+ using std::ifstream;
+ using std::istringstream;
+
+ if(access(s.c_str(),R_OK) == -1) {
+ std::cerr << "Unable to read input file" << s << std::endl;
+ exit(EXIT_FAILURE);
+ }
+ ifstream in(s.c_str());
+
+ int lineno = 0;
+ int lm_rows =0;
+ int lm_cols =0;
+ int wp_rows =0;
+ int wp_cols =0;
+
+ while(in) {
+ lineno++;
+ string str;
+ getline(in,str);
+ istringstream line(str);
+
+ vector<string> tokens;
+ copy(istream_iterator<string>(line),
+ istream_iterator<string>(),
+ back_inserter<vector<string> > (tokens));
+
+ if(tokens.size() ==0) {
+ continue;
+ }
+ else if (tokens[0][0] =='#') {
+ continue;
+ }
+ else if (tokens[0] == "lm") {
+ if(tokens.size() != 3) {
+ std::cerr<<"Wrong args for lm!"<<std::endl;
+ std::cerr<<"Error occuredon line"<<lineno<<std::endl;
+ std::cerr<<"line:"<<str<<std::endl;
+ exit(EXIT_FAILURE);
+ }
+ lm_rows = strtof(tokens[1].c_str(),NULL);
+ lm_cols = strtof(tokens[2].c_str(),NULL);
+
+ lm->resize(lm_rows,lm_cols);
+ for (int c =0; c<lm_cols; c++) {
+ lineno++;
+ if (!in) {
+ std::cerr<<"EOF after reading" << std::endl;
+ exit(EXIT_FAILURE);
+ }
+ getline(in,str);
+ istringstream line(str);
+ vector<string> tokens;
+ copy(istream_iterator<string>(line),
+ istream_iterator<string>(),
+ back_inserter<vector<string> > (tokens));
+ if(tokens.size() < lm_rows) {
+ std::cerr<<"invalid line for lm coordinate!"<<std::endl;
+ std::cerr<<"Error occured on line "<<lineno<<std::endl;
+ std::cerr<<"line: "<<str<<std::endl;
+ exit(EXIT_FAILURE);
+ }
+
+ for (unsigned r=0; r< lm_rows; r++) {
+ (*lm)(r,c) = strtof(tokens[r].c_str(),NULL);
+ }
+ }
+ }
+ else if (tokens[0] == "wp") {
+ if(tokens.size() != 3) {
+ std::cerr<<"Wrong args for wp!"<<std::endl;
+ std::cerr<<"Error occured on line"<<lineno<<std::endl;
+ std::cerr<<"line:"<<str<<std::endl;
+ exit(EXIT_FAILURE);
+ }
+ wp_rows = strtof(tokens[1].c_str(),NULL);
+ wp_cols = strtof(tokens[2].c_str(),NULL);
+ wp->resize(wp_rows, wp_cols);
+ for (int c =0; c<wp_cols; c++) {
+ lineno++;
+ if (!in) {
+ std::cerr<<"EOF after reading" << std::endl;
+ exit(EXIT_FAILURE);
+ }
+ getline(in,str);
+ istringstream line(str);
+ std::vector<string> tokens;
+ copy(istream_iterator<string>(line),
+ istream_iterator<string>(),
+ back_inserter<std::vector<string> > (tokens));
+ if(tokens.size() < wp_rows) {
+ std::cerr<<"invalid line for wp coordinate!"<<std::endl;
+ std::cerr<<"Error occured on line "<<lineno<<std::endl;
+ std::cerr<<"line: "<<str<<std::endl;
+ exit(EXIT_FAILURE);
+ }
+
+ for (int r=0; r< lm_rows; r++) {
+ (*wp)(r,c) = strtof(tokens[r].c_str(),NULL);
+ }
+ }
+ }
+ else {
+ std::cerr<<"Unkwown command"<<tokens[0] <<std::endl;
+ std::cerr<<"Error occured on line"<<lineno<<std::endl;
+ std::cerr<<"line: "<<str<<std::endl;
+ exit(EXIT_FAILURE);
+ }
+ }
+}
View
@@ -0,0 +1,14 @@
+#include <stdio.h>
+#include <algorithm>
+#include <iterator>
+#include <unistd.h>
+#include <errno.h>
+#include <string>
+#include <vector>
+
+#include <Eigen/Dense>
+
+using namespace Eigen;
+using namespace std;
+
+void read_input_file(const string s, MatrixXf *lm, MatrixXf *wp);
@@ -1,3 +1,4 @@
+
include_directories(${EIGEN3_INCLUDE_DIR})
include_directories(${CMAKE_CURRENT_BINARY_DIR}/..)
@@ -25,7 +25,7 @@ vector<Particle> fastslam1_sim(MatrixXf lm, MatrixXf wp)
printf("Sampling from predict noise usually OFF for FastSLAM 2.0\n");
}
- //normally initialized configfile.h
+ //normally initialized in configfile.h
Q << pow(sigmaV,2), 0,
0 , pow(sigmaG,2);
View
@@ -1,132 +1,11 @@
-#include <stdio.h>
-#include <algorithm>
-#include <iterator>
-#include <unistd.h>
-#include <errno.h>
-#include <string>
-#include <vector>
-#include <Eigen/Dense>
-
#include "fastslam1_sim.h"
+#include "core/read_input_file.h"
#include "core/particle.h"
#include "core/utilities.h"
using namespace Eigen;
using namespace std;
-void read_input_file(const string s, MatrixXf *lm, MatrixXf *wp)
-{
- using std::ifstream;
- using std::istringstream;
-
- if(access(s.c_str(),R_OK) == -1) {
- std::cerr << "Unable to read input file" << s << std::endl;
- exit(EXIT_FAILURE);
- }
- ifstream in(s.c_str());
-
- int lineno = 0;
- int lm_rows =0;
- int lm_cols =0;
- int wp_rows =0;
- int wp_cols =0;
-
- while(in) {
- lineno++;
- string str;
- getline(in,str);
- istringstream line(str);
-
- vector<string> tokens;
- copy(istream_iterator<string>(line),
- istream_iterator<string>(),
- back_inserter<vector<string> > (tokens));
-
- if(tokens.size() ==0) {
- continue;
- }
- else if (tokens[0][0] =='#') {
- continue;
- }
- else if (tokens[0] == "lm") {
- if(tokens.size() != 3) {
- std::cerr<<"Wrong args for lm!"<<std::endl;
- std::cerr<<"Error occuredon line"<<lineno<<std::endl;
- std::cerr<<"line:"<<str<<std::endl;
- exit(EXIT_FAILURE);
- }
- lm_rows = strtof(tokens[1].c_str(),NULL);
- lm_cols = strtof(tokens[2].c_str(),NULL);
-
- lm->resize(lm_rows,lm_cols);
- for (int c =0; c<lm_cols; c++) {
- lineno++;
- if (!in) {
- std::cerr<<"EOF after reading" << std::endl;
- exit(EXIT_FAILURE);
- }
- getline(in,str);
- istringstream line(str);
- vector<string> tokens;
- copy(istream_iterator<string>(line),
- istream_iterator<string>(),
- back_inserter<vector<string> > (tokens));
- if(tokens.size() < lm_rows) {
- std::cerr<<"invalid line for lm coordinate!"<<std::endl;
- std::cerr<<"Error occured on line "<<lineno<<std::endl;
- std::cerr<<"line: "<<str<<std::endl;
- exit(EXIT_FAILURE);
- }
-
- for (unsigned r=0; r< lm_rows; r++) {
- (*lm)(r,c) = strtof(tokens[r].c_str(),NULL);
- }
- }
- }
- else if (tokens[0] == "wp") {
- if(tokens.size() != 3) {
- std::cerr<<"Wrong args for wp!"<<std::endl;
- std::cerr<<"Error occured on line"<<lineno<<std::endl;
- std::cerr<<"line:"<<str<<std::endl;
- exit(EXIT_FAILURE);
- }
- wp_rows = strtof(tokens[1].c_str(),NULL);
- wp_cols = strtof(tokens[2].c_str(),NULL);
- wp->resize(wp_rows, wp_cols);
- for (int c =0; c<wp_cols; c++) {
- lineno++;
- if (!in) {
- std::cerr<<"EOF after reading" << std::endl;
- exit(EXIT_FAILURE);
- }
- getline(in,str);
- istringstream line(str);
- std::vector<string> tokens;
- copy(istream_iterator<string>(line),
- istream_iterator<string>(),
- back_inserter<std::vector<string> > (tokens));
- if(tokens.size() < wp_rows) {
- std::cerr<<"invalid line for wp coordinate!"<<std::endl;
- std::cerr<<"Error occured on line "<<lineno<<std::endl;
- std::cerr<<"line: "<<str<<std::endl;
- exit(EXIT_FAILURE);
- }
-
- for (int r=0; r< lm_rows; r++) {
- (*wp)(r,c) = strtof(tokens[r].c_str(),NULL);
- }
- }
- }
- else {
- std::cerr<<"Unkwown command"<<tokens[0] <<std::endl;
- std::cerr<<"Error occured on line"<<lineno<<std::endl;
- std::cerr<<"line: "<<str<<std::endl;
- exit(EXIT_FAILURE);
- }
- }
-}
-
-
int main (int argc, char *argv[])
{
MyTimer Timer = MyTimer();
Oops, something went wrong.

0 comments on commit 028fc05

Please sign in to comment.