Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

Split files into FSlam1, FSlam2, Core

  • Loading branch information...
commit 14f0a3d3c362eab233161650e1bd64a96d8d9ad3 1 parent 6422128
Grace Lee authored
Showing with 195 additions and 1,318 deletions.
  1. +7 −1 .gitignore
  2. +4 −2 CMakeLists.txt
  3. +0 −13 README.md
  4. +0 −5 cpp/.gitignore
  5. +0 −37 cpp/CMakeLists.txt
  6. +0 −13 cpp/README.md
  7. +29 −0 cpp/core/CMakeLists.txt
  8. 0  cpp/{ → core}/KF_cholesky_update.cpp
  9. 0  cpp/{ → core}/KF_cholesky_update.h
  10. 0  cpp/{ → core}/KF_joseph_update.cpp
  11. 0  cpp/{ → core}/KF_joseph_update.h
  12. 0  cpp/{ → core}/TransformToGlobal.cpp
  13. 0  cpp/{ → core}/TransformToGlobal.h
  14. 0  cpp/{ → core}/add_control_noise.cpp
  15. 0  cpp/{ → core}/add_control_noise.h
  16. 0  cpp/{ → core}/add_feature.cpp
  17. 0  cpp/{ → core}/add_feature.h
  18. 0  cpp/{ → core}/add_observation_noise.cpp
  19. 0  cpp/{ → core}/add_observation_noise.h
  20. 0  cpp/{ → core}/compute_jacobians.cpp
  21. 0  cpp/{ → core}/compute_jacobians.h
  22. 0  cpp/{ → core}/compute_steering.cpp
  23. 0  cpp/{ → core}/compute_steering.h
  24. 0  cpp/{ → core}/configfile.cpp
  25. 0  cpp/{ → core}/configfile.h
  26. 0  cpp/{ → core}/data_associate_known.cpp
  27. 0  cpp/{ → core}/data_associate_known.h
  28. 0  cpp/{ → core}/example_webmap.mat
  29. 0  cpp/{ → core}/feature_update.cpp
  30. 0  cpp/{ → core}/feature_update.h
  31. 0  cpp/{ → core}/get_observations.cpp
  32. 0  cpp/{ → core}/get_observations.h
  33. 0  cpp/{ → core}/line_plot_conversion.cpp
  34. 0  cpp/{ → core}/line_plot_conversion.h
  35. 0  cpp/{ → core}/multivariate_gauss.cpp
  36. 0  cpp/{ → core}/multivariate_gauss.h
  37. 0  cpp/{ → core}/particle.cpp
  38. 0  cpp/{ → core}/particle.h
  39. 0  cpp/{ → core}/pi_to_pi.cpp
  40. 0  cpp/{ → core}/pi_to_pi.h
  41. 0  cpp/{ → core}/predict_true.cpp
  42. 0  cpp/{ → core}/predict_true.h
  43. 0  cpp/{ → core}/printMat.h
  44. 0  cpp/{ → core}/resample_particles.cpp
  45. 0  cpp/{ → core}/resample_particles.h
  46. 0  cpp/{ → core}/stratified_random.cpp
  47. 0  cpp/{ → core}/stratified_random.h
  48. 0  cpp/{ → core}/stratified_resample.cpp
  49. 0  cpp/{ → core}/stratified_resample.h
  50. 0  cpp/fastslam1/CMakeLists.txt
  51. 0  cpp/{ → fastslam1}/predict.cpp
  52. 0  cpp/{ → fastslam1}/predict.h
  53. +24 −0 cpp/fastslam2/CMakeLists.txt
  54. +9 −9 cpp/fastslam2/fastslam2_sim.cpp
  55. +4 −4 cpp/fastslam2/fastslam2_sim.h
  56. +1 −1  cpp/{ → fastslam2}/main.cpp
  57. +2 −2 cpp/fastslam2/observe_heading.cpp
  58. +1 −1  cpp/fastslam2/observe_heading.h
  59. +2 −2 cpp/fastslam2/predict.h
  60. +0 −9 cpp/fastslam2/predict_true.cpp
  61. +0 −13 cpp/fastslam2/predict_true.h
  62. +4 −4 cpp/fastslam2/sample_proposal.h
  63. +0 −277 cpp/fastslam2_sim.cpp
  64. +0 −22 cpp/fastslam2_sim.h
  65. +0 −79 cpp/gauss_evaluate.cpp
  66. +0 −26 cpp/gauss_evaluate.h
  67. +0 −39 cpp/observe_heading.cpp
  68. +0 −9 cpp/observe_heading.h
  69. +0 −110 cpp/sample_proposal.cpp
  70. +0 −27 cpp/sample_proposal.h
  71. BIN  matlab/.add_control_noise.m.swp
  72. +2 −1  matlab/compute_jacobians.m
  73. +1 −1  matlab/fastslam1/predict.m
  74. +4 −1 matlab/fastslam2/fastslam2_sim.m
  75. +67 −36 testcode/add_feature.cpp
  76. +0 −44 testcode/compute_jacobians.cpp
  77. +33 −58 testcode/data_associate_known.cpp
  78. +1 −1  testcode/makefile
  79. +0 −12 testcode/pi_to_pi.cpp
  80. +0 −234 testcode/temp/main.cpp
  81. +0 −225 testcode/temp/old_main.cpp
8 .gitignore
View
@@ -1 +1,7 @@
-*~
+*.swp
+test/**/*
+*~
+*.o
+exec
+build
+.DS_Store
6 CMakeLists.txt
View
@@ -65,6 +65,8 @@ include_directories(${FSLAM_SOURCE_DIR}/cpp)
include_directories(${FSLAM_BINARY_DIR}/cpp)
# Adding build directories
-add_subdirectory(cpp)
+add_subdirectory(cpp/core)
+add_subdirectory(cpp/fastslam1)
+add_subdirectory(cpp/fastslam2)
-# Adding test directories
+# Adding test directories
13 README.md
View
@@ -1,13 +0,0 @@
-FastSLAM
-========
-
-Retrieval
-
-Prediction
-
-Measurement Update
-
-Importance Weight
-
-Resampling
-
5 cpp/.gitignore
View
@@ -1,5 +0,0 @@
-*.swp
-test/**/*
-*~
-*.o
-.DS_Store
37 cpp/CMakeLists.txt
View
@@ -1,37 +0,0 @@
-include_directories(${EIGEN3_INCLUDE_DIR})
-
-include_directories(${CMAKE_CURRENT_BINARY_DIR}/..)
-
-# Produce a FastSLAM shared library. (For re-use of executables)
-add_library(FastSLAM
- KF_cholesky_update.cpp add_observation_noise.cpp fastslam2_sim.cpp predict.cpp stratified_resample.cpp
- KF_joseph_update.cpp compute_jacobians.cpp feature_update.cpp multivariate_gauss.cpp predict_true.cpp
- TransformToGlobal.cpp compute_steering.cpp gauss_evaluate.cpp observe_heading.cpp resample_particles.cpp
- add_control_noise.cpp configfile.cpp get_observations.cpp particle.cpp sample_proposal.cpp
- add_feature.cpp data_associate_known.cpp line_plot_conversion.cpp pi_to_pi.cpp stratified_random.cpp
- )
-target_link_libraries(FastSLAM) # We don't depend on anything
-
-install(FILES
- KF_cholesky_update.h compute_jacobians.h gauss_evaluate.h pi_to_pi.h stratified_random.h
- KF_joseph_update.h compute_steering.h get_observations.h predict.h stratified_resample.h
- TransformToGlobal.h configfile.h line_plot_conversion.h predict_true.h
- add_control_noise.h data_associate_known.h multivariate_gauss.h printMat.h
- add_feature.h fastslam2_sim.h observe_heading.h resample_particles.h
- add_observation_noise.h feature_update.h particle.h sample_proposal.h
- DESTINATION include/fslam
- )
-
-install(TARGETS
- FastSLAM
- DESTINATION lib
- )
-
-# Add an executable that currently runs the simulator
-set(FSLAM_USED_LIBS FastSLAM)
-add_fslam_tool(fastslam_simulation main.cpp)
-
-# Add files that are just example data
-install( FILES example_webmap.mat
- PERMISSIONS GROUP_READ OWNER_READ
- DESTINATION bin )
13 cpp/README.md
View
@@ -1,13 +0,0 @@
-FastSLAM
-========
-
-Retrieval
-
-Prediction
-
-Measurement Update
-
-Importance Weight
-
-Resampling
-
29 cpp/core/CMakeLists.txt
View
@@ -0,0 +1,29 @@
+include_directories(${EIGEN3_INCLUDE_DIR})
+
+include_directories(${CMAKE_CURRENT_BINARY_DIR}/..)
+
+# Produce a FastSLAM shared library. (For re-use of executables)
+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
+ )
+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
+ DESTINATION include/fslam/core
+ )
+
+install(TARGETS
+ FastSLAM_core
+ DESTINATION lib
+ )
+
+# Add files that are just example data
+install( FILES example_webmap.mat
+ PERMISSIONS GROUP_READ OWNER_READ
+ DESTINATION bin )
0  cpp/KF_cholesky_update.cpp → cpp/core/KF_cholesky_update.cpp
View
File renamed without changes
0  cpp/KF_cholesky_update.h → cpp/core/KF_cholesky_update.h
View
File renamed without changes
0  cpp/KF_joseph_update.cpp → cpp/core/KF_joseph_update.cpp
View
File renamed without changes
0  cpp/KF_joseph_update.h → cpp/core/KF_joseph_update.h
View
File renamed without changes
0  cpp/TransformToGlobal.cpp → cpp/core/TransformToGlobal.cpp
View
File renamed without changes
0  cpp/TransformToGlobal.h → cpp/core/TransformToGlobal.h
View
File renamed without changes
0  cpp/add_control_noise.cpp → cpp/core/add_control_noise.cpp
View
File renamed without changes
0  cpp/add_control_noise.h → cpp/core/add_control_noise.h
View
File renamed without changes
0  cpp/add_feature.cpp → cpp/core/add_feature.cpp
View
File renamed without changes
0  cpp/add_feature.h → cpp/core/add_feature.h
View
File renamed without changes
0  cpp/add_observation_noise.cpp → cpp/core/add_observation_noise.cpp
View
File renamed without changes
0  cpp/add_observation_noise.h → cpp/core/add_observation_noise.h
View
File renamed without changes
0  cpp/compute_jacobians.cpp → cpp/core/compute_jacobians.cpp
View
File renamed without changes
0  cpp/compute_jacobians.h → cpp/core/compute_jacobians.h
View
File renamed without changes
0  cpp/compute_steering.cpp → cpp/core/compute_steering.cpp
View
File renamed without changes
0  cpp/compute_steering.h → cpp/core/compute_steering.h
View
File renamed without changes
0  cpp/configfile.cpp → cpp/core/configfile.cpp
View
File renamed without changes
0  cpp/configfile.h → cpp/core/configfile.h
View
File renamed without changes
0  cpp/data_associate_known.cpp → cpp/core/data_associate_known.cpp
View
File renamed without changes
0  cpp/data_associate_known.h → cpp/core/data_associate_known.h
View
File renamed without changes
0  cpp/example_webmap.mat → cpp/core/example_webmap.mat
View
File renamed without changes
0  cpp/feature_update.cpp → cpp/core/feature_update.cpp
View
File renamed without changes
0  cpp/feature_update.h → cpp/core/feature_update.h
View
File renamed without changes
0  cpp/get_observations.cpp → cpp/core/get_observations.cpp
View
File renamed without changes
0  cpp/get_observations.h → cpp/core/get_observations.h
View
File renamed without changes
0  cpp/line_plot_conversion.cpp → cpp/core/line_plot_conversion.cpp
View
File renamed without changes
0  cpp/line_plot_conversion.h → cpp/core/line_plot_conversion.h
View
File renamed without changes
0  cpp/multivariate_gauss.cpp → cpp/core/multivariate_gauss.cpp
View
File renamed without changes
0  cpp/multivariate_gauss.h → cpp/core/multivariate_gauss.h
View
File renamed without changes
0  cpp/particle.cpp → cpp/core/particle.cpp
View
File renamed without changes
0  cpp/particle.h → cpp/core/particle.h
View
File renamed without changes
0  cpp/pi_to_pi.cpp → cpp/core/pi_to_pi.cpp
View
File renamed without changes
0  cpp/pi_to_pi.h → cpp/core/pi_to_pi.h
View
File renamed without changes
0  cpp/predict_true.cpp → cpp/core/predict_true.cpp
View
File renamed without changes
0  cpp/predict_true.h → cpp/core/predict_true.h
View
File renamed without changes
0  cpp/printMat.h → cpp/core/printMat.h
View
File renamed without changes
0  cpp/resample_particles.cpp → cpp/core/resample_particles.cpp
View
File renamed without changes
0  cpp/resample_particles.h → cpp/core/resample_particles.h
View
File renamed without changes
0  cpp/stratified_random.cpp → cpp/core/stratified_random.cpp
View
File renamed without changes
0  cpp/stratified_random.h → cpp/core/stratified_random.h
View
File renamed without changes
0  cpp/stratified_resample.cpp → cpp/core/stratified_resample.cpp
View
File renamed without changes
0  cpp/stratified_resample.h → cpp/core/stratified_resample.h
View
File renamed without changes
0  cpp/fastslam1/CMakeLists.txt
View
No changes.
0  cpp/predict.cpp → cpp/fastslam1/predict.cpp
View
File renamed without changes
0  cpp/predict.h → cpp/fastslam1/predict.h
View
File renamed without changes
24 cpp/fastslam2/CMakeLists.txt
View
@@ -0,0 +1,24 @@
+include_directories(${EIGEN3_INCLUDE_DIR})
+
+include_directories(${CMAKE_CURRENT_BINARY_DIR}/..)
+
+# Produce a FastSLAM shared library. (For re-use of executables)
+add_library(FastSLAM2
+ fastslam2_sim.cpp gauss_evaluate.cpp observe_heading.cpp predict.cpp sample_proposal.cpp
+)
+target_link_libraries(FastSLAM2 FastSLAM_core) # We don't depend on anything
+
+install(FILES
+ fastslam2_sim.h gauss_evaluate.h observe_heading.h predict.h sample_proposal.h
+ DESTINATION include/fslam/fastslam2
+ )
+
+install(TARGETS
+ FastSLAM2
+ DESTINATION lib
+ )
+
+# Add an executable that currentlyi runs the simulator
+set(FSLAM_USED_LIBS FastSLAM2)
+add_fslam_tool(fastslam2_simulation main.cpp)
+
18 cpp/fastslam2/fastslam2_sim.cpp
View
@@ -3,18 +3,18 @@
#include <vector>
#include "fastslam2_sim.h"
-#include "add_control_noise.h"
+#include "core/add_control_noise.h"
#include "predict.h"
#include "observe_heading.h"
-#include "get_observations.h"
-#include "add_observation_noise.h"
-#include "TransformToGlobal.h"
-#include "line_plot_conversion.h"
-#include "data_associate_known.h"
+#include "core/get_observations.h"
+#include "core/add_observation_noise.h"
+#include "core/TransformToGlobal.h"
+#include "core/line_plot_conversion.h"
+#include "core/data_associate_known.h"
#include "sample_proposal.h"
-#include "feature_update.h"
-#include "resample_particles.h"
-#include "add_feature.h"
+#include "core/feature_update.h"
+#include "core/resample_particles.h"
+#include "core/add_feature.h"
using namespace config;
using namespace std;
8 cpp/fastslam2/fastslam2_sim.h
View
@@ -8,10 +8,10 @@
#include <vector>
#include <Eigen/Dense>
-#include "configfile.h"
-#include "compute_steering.h"
-#include "predict_true.h"
-#include "particle.h"
+#include "core/configfile.h"
+#include "core/compute_steering.h"
+#include "core/predict_true.h"
+#include "core/particle.h"
using namespace std;
using namespace Eigen;
2  cpp/main.cpp → cpp/fastslam2/main.cpp
View
@@ -8,7 +8,7 @@
#include <Eigen/Dense>
#include "fastslam2_sim.h"
-#include "particle.h"
+#include "core/particle.h"
using namespace Eigen;
using namespace std;
4 cpp/fastslam2/observe_heading.cpp
View
@@ -1,6 +1,6 @@
#include "observe_heading.h"
-#include "pi_to_pi.h"
-#include "KF_joseph_update.h"
+#include "core/pi_to_pi.h"
+#include "core/KF_joseph_update.h"
#include <iostream>
#include <vector>
2  cpp/fastslam2/observe_heading.h
View
@@ -2,7 +2,7 @@
#define OBSERVE_HEADING_H
#include<Eigen/Dense>
-#include "particle.h"
+#include "core/particle.h"
void observe_heading(Particle &particle, float phi, int useheading);
4 cpp/fastslam2/predict.h
View
@@ -2,8 +2,8 @@
#define PREDICT_H
#include <Eigen/Dense>
-#include "particle.h"
-#include "multivariate_gauss.h"
+#include "core/particle.h"
+#include "core/multivariate_gauss.h"
using namespace Eigen;
9 cpp/fastslam2/predict_true.cpp
View
@@ -1,9 +0,0 @@
-#include "predict_true.h"
-#include "pi_to_pi.h"
-
-void predict_true(VectorXf &xv, float V, float G, int WB, float dt)
-{
- xv(0) = xv(0) + V*dt*cos(G+xv(2));
- xv(1) = xv(1) + V*dt*sin(G+xv(2));
- xv(2) = pi_to_pi(xv(2) + V*dt*sin(G)/WB);
-}
13 cpp/fastslam2/predict_true.h
View
@@ -1,13 +0,0 @@
-#ifndef PREDICT_TRUE_H
-#define PREDICT_TRUE_H
-
-#include <Eigen/Dense>
-
-using namespace Eigen;
-
-void predict_true(VectorXf &xv,float V,float G,int WB,float dt);
-//V is m/s
-// G is steering angle
-//WB = WHEELBASE
-
-#endif //PREDICT_TRUE_H
8 cpp/fastslam2/sample_proposal.h
View
@@ -4,11 +4,11 @@
#include <vector>
#include <Eigen/Dense>
-#include "particle.h"
-#include "compute_jacobians.h"
-#include "multivariate_gauss.h"
+#include "core/particle.h"
+#include "core/compute_jacobians.h"
+#include "core/multivariate_gauss.h"
#include "gauss_evaluate.h"
-#include "pi_to_pi.h"
+#include "core/pi_to_pi.h"
using namespace Eigen;
using namespace std;
277 cpp/fastslam2_sim.cpp
View
@@ -1,277 +0,0 @@
-#include <iostream>
-#include <math.h>
-#include <vector>
-
-#include "fastslam2_sim.h"
-#include "add_control_noise.h"
-#include "predict.h"
-#include "observe_heading.h"
-#include "get_observations.h"
-#include "add_observation_noise.h"
-#include "TransformToGlobal.h"
-#include "line_plot_conversion.h"
-#include "data_associate_known.h"
-#include "sample_proposal.h"
-#include "feature_update.h"
-#include "resample_particles.h"
-#include "add_feature.h"
-
-using namespace config;
-using namespace std;
-
-vector<Particle> fastslam2_sim(MatrixXf lm, MatrixXf wp)
-{
- if (SWITCH_PREDICT_NOISE) {
- printf("Sampling from predict noise usually OFF for FastSLAM 2.0\n");
- }
- if (SWITCH_SAMPLE_PROPOSAL == 0) {
- printf("Sampling from optimal proposal is usually ON for FastSLAM 2.0\n");
- }
- //normally initialized configfile.h
- Q << pow(sigmaV,2), 0,
- 0 , pow(sigmaG,2);
-
- R << sigmaR*sigmaR, 0,
- 0, sigmaB*sigmaB;
-
- float veh[2][3] = {{0,-WHEELBASE,-WHEELBASE},{0,-1,1}};
-
- //vector of particles (their count will change)
- vector<Particle> particles(NPARTICLES);
- for (int i=0; i<particles.size(); i++){
- particles[i] = Particle();
- }
-
-
- //initialize particle weights as uniform
- float uniformw = 1.0/(float)NPARTICLES;
- for (unsigned int p = 0; p < NPARTICLES; p++) {
- particles[p].setW(uniformw);
- }
-
- VectorXf xtrue(3);
- xtrue.setZero();
-
- float dt = DT_CONTROLS; //change in time btw predicts
- float dtsum = 0; //change in time since last observation
-
- vector<int> ftag; //identifier for each landmark
- for (int i=0; i< lm.cols(); i++) {
- ftag.push_back(i);
- }
-
- //data ssociation table
- VectorXf da_table(lm.cols());
- for (int s=0; s<da_table.size(); s++) {
- da_table[s] = -1;
- }
-
- int iwp = 0; //index to first waypoint
- float G = 0; //initial steer angle
- MatrixXf plines; //will later change to list of points
-
- if (SWITCH_SEED_RANDOM !=0) {
- srand(SWITCH_SEED_RANDOM);
- }
-
- MatrixXf Qe = MatrixXf(Q);
- MatrixXf Re = MatrixXf(R);
-
- if (SWITCH_INFLATE_NOISE ==1) {
- Qe = 2*Q;
- Re = 2*R;
- }
-
- if (SWITCH_PROFILE) {
- //TODO:
- }
-
- vector<int> ftag_visible;
- MatrixXf z;
- int count =0;
-
-
- //Main loop
- while (iwp !=-1) {
- compute_steering(xtrue, wp, iwp, AT_WAYPOINT, G, RATEG, MAXG, dt);
- if (iwp ==-1 && NUMBER_LOOPS > 1) {
- iwp = 0;
- NUMBER_LOOPS = NUMBER_LOOPS-1;
- }
- predict_true(xtrue,V,G,WHEELBASE,dt);
-
- //add process noise
- //TODO: need to truly randomize function in multivariate_gauss
- float* VnGn = new float[2];
- //add_control_noise(V,G,Q,SWITCH_CONTROL_NOISE,VnGn);
- float Vn = V;//VnGn[0];
- float Gn = G;//VnGn[1];
-
- //Predict step
- for (unsigned int i=0; i< NPARTICLES; i++) {
- predict(particles[i],Vn,Gn,Qe,WHEELBASE,dt,SWITCH_PREDICT_NOISE);
- observe_heading(particles[i], xtrue(2), SWITCH_HEADING_KNOWN); //if heading known, observe heading
- }
-
- //Observe step
- dtsum = dtsum+dt;
- if (dtsum >= DT_OBSERVE) {
- dtsum=0;
-
- //Compute true data, then add noise
- ftag_visible = vector<int>(ftag); //modify the copy, not the ftag
- z = get_observations(xtrue,lm,ftag_visible,MAX_RANGE);
- //TODO
- //add_observation_noise(z,R,SWITCH_SENSOR_NOISE);
- if(!z.isZero()){
- plines = make_laser_lines(z,xtrue);
- }
-
- //Compute (known) data associations
- int Nf = (particles[0].xf()).cols();
- vector<int> idf;
- if (ftag_visible.size() ==2) {
- cout<<"INPUT GET OBSERVE"<<endl;
- cout<<"xtrue"<<endl;
- cout<<xtrue<<endl;
- cout<<"lm"<<endl;
- cout<<lm<<endl;
- cout<<"ftag_visible"<<endl;
- for (int i=0; i<ftag.size(); i++) {
- cout<<ftag[i]<<" ";
- }
- cout<<endl;
- cout<<"max range: "<<MAX_RANGE<<endl;
- cout<<"OUTPUT GET_OBSERVE"<<endl;
- cout<<"ftag_visible"<<endl;
- for (int i=0; i<ftag_visible.size(); i++) {
- cout<<ftag_visible[i]<<" ";
- }
- cout<<"z"<<endl;
- cout<<z<<endl;
- cout<<endl;
- }
-
- MatrixXf zf(z.rows(),ftag_visible.size());
- zf.setZero();
- MatrixXf zn(z.rows(),ftag_visible.size());
- zn.setZero();
-
- bool testflag= false;
- if(ftag_visible.size() ==2) {
- testflag = true;
- cout<<"INPUT TO DATA_ASSOC"<<endl;
- cout<<"z"<<endl;
- cout<<z<<endl;
- cout<<"ftag_visible"<<endl;
- for (int i=0; i<ftag_visible.size(); i++) {
- cout<<ftag_visible[i]<<" ";
- }
- cout<<endl;
- cout<<"da_table"<<endl;
- for (int i=0; i<da_table.size(); i++) {
- cout<<da_table[i]<<" ";
- }
- cout<<endl;
- cout<<"Nf"<<endl;
- cout<<Nf<<endl;
- cout<<"zf"<<endl;
- cout<<zf<<endl;
- for (int i=0; i<idf.size(); i++) {
- cout<<idf[i]<<" ";
- }
- cout<<endl;
- cout<<"zn"<<endl;
- cout<<zn<<endl;
- cout<<"idf"<<endl;
- for (int i=0; i<idf.size(); i++) {
- cout<<idf[i]<<" ";
- }
- }
- data_associate_known(z,ftag_visible,da_table,Nf,zf,idf,zn);
- if(testflag==true) {
- cout<<"OUTPUT DATA_ASSOC"<<endl;
- cout<<"da_table"<<endl;
- for (int i=0; i<da_table.size(); i++) {
- cout<<da_table[i]<<" ";
- }
- cout<<endl;
- cout<<"zf"<<endl;
- cout<<zf<<endl;
- for (int i=0; i<idf.size(); i++) {
- cout<<idf[i]<<" ";
- }
- cout<<endl;
- cout<<"zn"<<endl;
- cout<<zn<<endl;
- cout<<"idf"<<endl;
- for (int i=0; i<idf.size(); i++) {
- cout<<idf[i]<<" ";
- }
- cout<<endl;
- }
- //observe map features
- if (!zf.isZero()) {
- //isample from 'optimal' proposal distribution, then update map
-
- for (unsigned i=0; i<NPARTICLES; i++) {
- sample_proposal(particles[i], zf, idf, Re);
- feature_update(particles[i],zf,idf,Re);
- }
- //resample
- resample_particles(particles,NEFFECTIVE,SWITCH_RESAMPLE);
- }
-
- //Observe new features, augment map
- //TODO: xv gets depreicated here
- if (!zn.isZero()) {
- for (unsigned i=0; i<NPARTICLES; i++) {
- if (zf.isZero()) {//sample from proposal distribution if we have not already done so above
- VectorXf xv = multivariate_gauss(particles[i].xv(),
- particles[i].Pv(),1);
- //TODO: xv[0] seems to have an approximation error from chol.
- particles[i].setXv(xv);
- MatrixXf pv(3,3);
- pv.setZero();
- particles[i].setPv(pv);
- }
- add_feature(particles[i], zn, Re);
- }
- }
- count++;
- if (VnGn) {
- delete[] VnGn;
- }
- }
- }
- cout<<"done with all functions and will return particles"<<endl<<flush;
- return particles;
-}
-
-MatrixXf make_laser_lines(MatrixXf rb, VectorXf xv)
-{
- if (rb.isZero()) {
- return MatrixXf(0,0);
- }
-
- int len = rb.cols();
- MatrixXf lnes(4,len);
-
- MatrixXf globalMat(2,rb.cols());
- int j;
- for (j=0; j<globalMat.cols();j++) {
- globalMat(0,j) = rb(0,j)*cos(rb(1,j));
- globalMat(1,j) = rb(0,j)*sin(rb(1,j));
- }
-
- TransformToGlobal(globalMat,xv);
-
- for (int c=0; c<lnes.cols();c++) {
- lnes(0,c) = xv(0);
- lnes(1,c) = xv(1);
- lnes(2,c) = globalMat(0,c);
- lnes(3,c) = globalMat(1,c);
- }
-
- return line_plot_conversion(lnes);
-}
22 cpp/fastslam2_sim.h
View
@@ -1,22 +0,0 @@
-#ifndef FASTSLAM2_SIM_H
-#define FASTSLAM2_SIM_H
-
-#include <iostream>
-#include <fstream>
-#include <sstream>
-#include <stdlib.h>
-#include <vector>
-#include <Eigen/Dense>
-
-#include "configfile.h"
-#include "compute_steering.h"
-#include "predict_true.h"
-#include "particle.h"
-
-using namespace std;
-using namespace Eigen;
-
-vector<Particle> fastslam2_sim(MatrixXf lm, MatrixXf wp);
-MatrixXf make_laser_lines(MatrixXf rb, VectorXf xv);
-
-#endif //FASTSLAM2_SIM_H
79 cpp/gauss_evaluate.cpp
View
@@ -1,79 +0,0 @@
-#include "gauss_evaluate.h"
-#include <algorithm>
-#include <iostream>
-
-#define pi 3.14159
-
-using namespace std;
-
-
-float gauss_evaluate(VectorXf v, MatrixXf S, int logflag)
-{
- #if 0
- //cout<<"in gauss_eval"<<endl;
- //cout<<"v"<<endl;
- //cout<<v<<endl;
- //cout<<endl;
- //cout<<"S"<<endl;
- //cout<<S<<endl;
- #endif
-
- int D = v.size();
- MatrixXf Sc = S.llt().matrixL();
- //cout<<"Sc"<<endl;
- //cout<<Sc<<endl;
- //cout<<endl;
- //cout<<"v"<<endl;
- //cout<<v<<endl;
-
- //normalised innovation
- VectorXf nin = Sc.jacobiSvd(ComputeThinU | ComputeThinV).solve(v);
- //cout<<"nin in gauss_eval"<<endl;
- //cout<<nin<<endl;
-
- #if 0
- //cout<<"nin"<<endl;
- //cout<<nin<<endl;
- #endif
-
- int s;
- //VectorXf E(nin.cols());
- float E=0;
- for (s=0; s<nin.size(); s++) {
- nin(s) = pow(nin(s),2);
- E += nin(s);
- }
- E=-0.5*E;
- //Note: above is a fast way to compute sets of inner-products
-
- unsigned i;
- float C,w;
- unsigned m = min(Sc.rows(), Sc.cols());
-
- if (logflag !=1) {
- float prod = 1;
- for (i=0; i<m; i++) {
- prod=prod*Sc(i,i); //multiply the diagonals
- }
- C = pow((2*pi),(D/2))*prod; //normalizing term(makes Gaussian hyper volume =1
- //cout<<"C in gauss_eval"<<endl;
- //cout<<C<<endl;
- //cout<<"E in gauss_eval"<<endl;
- //cout<<E<<endl;
- //cout<<"exp(E)"<<endl;
- //cout<<exp(E)<<endl;
- w = exp(E)/C; //likelihood
- //cout<<"w in gauss eval"<<endl;
- //cout<<w<<endl;
- } else {
- float sum=0;
- for (i=0; i<m; i++) {
- sum+=log(Sc(i,i));
- }
- C = 0.5*D*log(2*pi) + sum; //log of normalising term
- w = E-C; //log-likelihood
- //cout<<"w in gauss eval 2"<<endl;
- //cout<<w<<endl;
- }
- return w;
-}
26 cpp/gauss_evaluate.h
View
@@ -1,26 +0,0 @@
-#ifndef GAUSS_EVALUATE_H
-#define GAUSS_EVALUATE_H
-
-#include <Eigen/Dense>
-
-using namespace Eigen;
-
-float gauss_evaluate(VectorXf v, MatrixXf S, int logflag);
-
-/*
-**INPUTS:
-** v - a set of innovation vectors
-** S - the covariance matrix for the innovations
-** logflat - <optional> - if 1 computes the log-likelihood, otherwise computes
-** the likelihood.
-**
-**OUTPUT:
-** w- set of Gaussian likelihoods or log-likelihoods for ech v(:i).
-**
-**This implementaiton uses the Cholesky factor of S to compute the likelihoods
-**and so is more numerically stable than a simple full covariance form.
-**This function is identical to gauss_likelihood().
-*/
-
-#endif
-
39 cpp/observe_heading.cpp
View
@@ -1,39 +0,0 @@
-#include "observe_heading.h"
-#include "pi_to_pi.h"
-#include "KF_joseph_update.h"
-
-#include <iostream>
-#include <vector>
-
-//observe_heading doesn't get called unless I change SWITCH_HEADING KNOWN
-
-using namespace std;
-
-void observe_heading(Particle &particle, float phi, int useheading)
-{
- if (useheading ==0){
- return;
- }
- float sigmaPhi = 0.01*pi/180.0;
- cout<<"sigmaPhi "<<sigmaPhi<<endl;
- VectorXf xv = particle.xv();
- cout<<"xv"<<endl;
- cout<<xv<<endl;
- MatrixXf Pv = particle.Pv();
- cout<<"Pv"<<endl;
- cout<<Pv<<endl;
- MatrixXf H(1,3);
- H<<0,0,1;
-
- float v = pi_to_pi(phi-xv(2));
- cout<<"v"<<endl;
- cout<<v<<endl;
- KF_joseph_update(xv,Pv,v,pow(sigmaPhi,2),H);
- cout<<"KF_Joseph = xv"<<endl;
- cout<<xv<<endl;
- cout<<"KF_Joseph = Pv"<<endl;
- cout<<Pv<<endl;
-
- particle.setXv(xv);
- particle.setPv(Pv);
-}
9 cpp/observe_heading.h
View
@@ -1,9 +0,0 @@
-#ifndef OBSERVE_HEADING_H
-#define OBSERVE_HEADING_H
-
-#include<Eigen/Dense>
-#include "particle.h"
-
-void observe_heading(Particle &particle, float phi, int useheading);
-
-#endif
110 cpp/sample_proposal.cpp
View
@@ -1,110 +0,0 @@
-#include "sample_proposal.h"
-#include <iostream>
-#include <Eigen/SVD>
-#include <iomanip>
-#include "assert.h"
-
-//compute proposal distribution, then sample from it, and compute new particle weight
-void sample_proposal(Particle &particle, MatrixXf z, vector<int> idf, MatrixXf R)
-{
- VectorXf xv(particle.xv()); //robot position
- MatrixXf Pv(particle.Pv()); //controls (motion command)
-
- VectorXf xv0(xv);
- MatrixXf Pv0(Pv);
-
- vector<MatrixXf> Hv;
- vector<MatrixXf> Hf;
- vector<MatrixXf> Sf;
-
- MatrixXf zpi(z.rows(),1);
- zpi.setZero();
- MatrixXf Hvi;
- MatrixXf Hfi;
- MatrixXf Sfi;
-
- VectorXf vi(z.rows());
-
- //process each feature, incrementally refine proposal distribution
- unsigned i,r;
- for (i =0; i<idf.size(); i++) {
- vector<int> j;
- j.push_back(idf[i]);
-
- compute_jacobians(particle,j,R,zpi,&Hv,&Hf,&Sf);
- assert(zpi.cols() == 1);
-
- Hvi = Hv[i];
- Hfi = Hf[i];
- Sfi = Sf[i].inverse();
-
- for (r=0; r<z.rows(); r++) {
- vi[r] = z(r,i) - zpi(r,0);
- }
-
- vi[1] = pi_to_pi(vi[1]);
-
- //proposal covariance
- MatrixXf Pv_inv = Pv.llt().solve(MatrixXf::Identity(Pv.rows(), Pv.cols()));
- Pv = Hvi.transpose() * Sfi * Hvi + Pv_inv;//Pv.inverse();
- Pv = Pv.llt().solve(MatrixXf::Identity(Pv.rows(), Pv.cols()));//Pv.inverse();
-
- //proposal mean
- xv = xv + Pv * Hvi.transpose() * Sfi *vi;
- particle.setXv(xv);
- particle.setPv(Pv);
- }
-
- //sample from proposal distribution
- VectorXf xvs = multivariate_gauss(xv,Pv,1);
- particle.setXv(xvs);
- MatrixXf zeros(3,3);
- zeros.setZero();
- particle.setPv(zeros);
-
- //compute sample weight: w = w* p(z|xk) p(xk|xk-1) / proposal
- float like = likelihood_given_xv(particle, z, idf, R);
- float prior = gauss_evaluate(delta_xv(xv0,xvs), Pv0,0);
- float prop = gauss_evaluate(delta_xv(xv,xvs),Pv,0);
-
- particle.setW(particle.w() * like * prior / prop);
-}
-
-float likelihood_given_xv(Particle particle, MatrixXf z, vector<int>idf, MatrixXf R)
-{
- float w = 1;
- vector<int> idfi;
-
- vector<MatrixXf> Hv;
- vector<MatrixXf> Hf;
- vector<MatrixXf> Sf;
-
- MatrixXf zp;
- MatrixXf Sfi;
- VectorXf v(z.rows());
-
- unsigned i,k;
-
- for (i=0; i<idf.size(); i++){
- idfi.push_back(idf[i]);
- zp.resize(z.rows(), idfi.size());
- compute_jacobians(particle,idfi,R,zp,&Hv,&Hf,&Sf);
-
- for (k=0; k<z.rows(); k++) {
- v(k) = z(k,i)-zp(k,i);
- }
- v(1) = pi_to_pi(v(1));
-
- w = w*gauss_evaluate(v,Sf[0],0);
- }
- return w;
-}
-
-VectorXf delta_xv(VectorXf xv1, VectorXf xv2)
-{
- //compute innovation between two xv estimates, normalising the heading component
- VectorXf dx = xv1-xv2;
- dx(2) = pi_to_pi(dx(2));
- return dx;
-}
-
27 cpp/sample_proposal.h
View
@@ -1,27 +0,0 @@
-#ifndef SAMPLE_PROPOSAL_H
-#define SAMPLE_PROPOSAL_H
-
-#include <vector>
-#include <Eigen/Dense>
-
-#include "particle.h"
-#include "compute_jacobians.h"
-#include "multivariate_gauss.h"
-#include "gauss_evaluate.h"
-#include "pi_to_pi.h"
-
-using namespace Eigen;
-using namespace std;
-
-void sample_proposal(Particle &particle, MatrixXf z, vector<int> idf, MatrixXf R);
-
-float likelihood_given_xv(Particle particle, MatrixXf z, vector<int>idf, MatrixXf R);
-VectorXf delta_xv(VectorXf xv1, VectorXf xv2);
-
-#if 0
-template<typename _Matrix_Type_>
-bool pseudoInverse(const _Matrix_Type_ &a, _Matrix_Type_ &result, double
-epsilon = numeric_limits<typename _Matrix_Type_::Scalar>::epsilon());
-#endif
-
-#endif
BIN  matlab/.add_control_noise.m.swp
View
Binary file not shown
3  matlab/compute_jacobians.m
View
@@ -16,5 +16,6 @@
dy/d2, -dx/d2, -1];
Hf(:,:,i)= [ dx/d, dy/d; % Jacobian wrt feature states
-dy/d2, dx/d2];
- Sf(:,:,i)= Hf(:,:,i) * Pf(:,:,i) * Hf(:,:,i)' + R; % innovation covariance of 'feature observation given the vehicle'
+ Sf(:,:,i)= Hf(:,:,i) * Pf(:,:,i) * Hf(:,;w
+:,i)' + R; % innovation covariance of 'feature observation given the vehicle'
end
2  matlab/fastslam1/predict.m
View
@@ -12,4 +12,4 @@
particle.xv= [xv(1) + V*dt*cos(G+xv(3,:));
xv(2) + V*dt*sin(G+xv(3,:));
pi_to_pi(xv(3) + V*dt*sin(G)/WB)];
-
+
5 matlab/fastslam2/fastslam2_sim.m
View
@@ -135,6 +135,7 @@
p(i).da= [];
end
+%don't implement this
function p= make_covariance_ellipses(particle)
N= 10;
inc= 2*pi/N;
@@ -158,6 +159,7 @@
end
end
+%don't implement this
function p= make_ellipse(x,P,circ)
% make a single 2-D ellipse
r= sqrtm_2by2(P);
@@ -168,6 +170,7 @@
%
%
+%don't implement this
function h= setup_animations(lm,wp)
figure
plot(lm(1,:),lm(2,:),'g*')
@@ -181,7 +184,7 @@
h.xvp= plot(0,0,'r.','erasemode','xor'); % estimated vehicle (particles)
h.cov= plot(0,0,'erasemode','xor'); % covariances of max weight particle
-
+%dont implement this
function do_plot(h, particles, xtrue, plines, veh)
xvp = [particles.xv];
103 testcode/add_feature.cpp
View
@@ -9,24 +9,30 @@ using namespace std;
// add new features
//
void add_feature(Particle &particle, MatrixXf z, MatrixXf R)
-{
+{
+ cout<<"ADD_FEATURE"<<endl;
+ cout<<"z"<<endl;
+ cout<<z<<endl;
+ cout<<"R"<<endl;
+ cout<<R<<endl;
+ cout<<"particle.w"<<endl;
+ cout<<particle.w()<<endl;
+ cout<<"particle.xv"<<endl;
+ cout<<particle.xv()<<endl;
+ cout<<"particle.Pv"<<endl;
+ cout<<particle.Pv()<<endl;
+ cout<<"particle.xf"<<endl;
+ cout<<particle.xf()<<endl;
+ cout<<"particle.Pf"<<endl;
+ for (int i=0; i<(particle.Pf()).size();i++) {
+ cout<<particle.Pf()[i]<<endl;
+ }
+
int lenz = z.cols();
MatrixXf xf(2,lenz);
vector<MatrixXf> Pf;
VectorXf xv = particle.xv();
- //TODO: this doesn't match :(
- //TODO: fix this!!!
- #if 0
- cout<<"xv in add_feature"<<endl;
- cout<<xv<<endl;
- cout<<"should be"<<endl;
- cout<<"0.7648"<<endl;
- cout<<"0.0221"<<endl;
- cout<<"0.0050"<<endl;
- cout<<endl;
- #endif
-
float r,b,s,c;
MatrixXf Gz(2,2);
@@ -49,43 +55,68 @@ void add_feature(Particle &particle, MatrixXf z, MatrixXf R)
ii.push_back(i);
}
- MatrixXf xfCopy = particle.xf();
- vector<MatrixXf> pfCopy(particle.Pf());
+ //MatrixXf xfCopy;// = particle.xf();
+ //vector<MatrixXf> pfCopy(particle.Pf());
+
+ cout<<"xf"<<endl;
+ cout<<xf<<endl;
+
+//TODO: there is a bug here. xfCopy (which is particle.xf) should grow in size
+//based on length of ii
+//new xfCopy columns = max(ii.size(),xfCopy.cols())
+//initialize to 0
+
+ //stupid dynamically sized matlab matrices :((((
+ int prows = particle.xf().rows();
+ int pcols = particle.xf().cols();
+ int iisize = ii.size();
+
+ //max value in ii
+ vector<int>::iterator maxelem;
+ maxelem = std::max_element(ii.begin(),ii.end());
+ int xfc = std::max((*maxelem)+1,pcols);
- for (unsigned c=0; c<ii.size(); c++) {
- #if 0
- cout<<"ii"<<endl;
- cout<<ii[c]<<endl;
- #endif
+ cout<<"particle.xf().cols()"<<endl;
+ cout<<pcols<<endl;
+ cout<<"max"<<endl;
+ cout<<xfc<<endl;
+ MatrixXf xfCopy(prows,xfc);
+ xfCopy.setZero();
+ for (int i =0; i<prows; i++) {
+ for (int j=0; j<pcols; j++) {
+ xfCopy(i,j) =particle.xf()(i,j);
+ }
+ }
+
+ //stupid matlab... I need to dynamically
+ //pfCopy
+ int pfcols = particle.Pf().size();
+ int Pfc = std::max((*maxelem)+1,pfcols);
+ vector<MatrixXf> pfCopy(Pfc);
+ for (int i=0; i<pfcols; i++) {
+ pfCopy[i] = particle.Pf()[i];
+ }
+
+ for (unsigned l=0; l<ii.size(); l++) {
+ if (ii.size() == 0) {
+ break;
+ }
if (xfCopy.isZero()) {
xfCopy = xf;
} else {
- for (unsigned r=0; r<xf.rows(); r++) {
- xfCopy(r,ii[c]) = xf(r,c);
+ for (unsigned k=0; k<xf.rows(); k++) {
+ xfCopy(k,ii[l]) = xf(k,l);
}
}
if (pfCopy.empty()) {
pfCopy = Pf;
} else {
- pfCopy[ii[c]] = Pf[c];
+ pfCopy[ii[l]] = Pf[l];
}
}
- #if 0
- cout<<"xf in add_feature"<<endl;
- cout<<xf<<endl;
- cout<<"should be"<<endl;
- cout<<"3.4011 25.7814"<<endl;
- cout<<"-25.6172 3.6347"<<endl;
- cout<<endl;
-
- cout<<"xfCopy"<<endl;
- cout<<xfCopy<<endl;
- cout<<endl;
- #endif
-
particle.setXf(xfCopy);
particle.setPf(pfCopy);
}
44 testcode/compute_jacobians.cpp
View
@@ -13,25 +13,10 @@ void compute_jacobians(Particle particle,
{
VectorXf xv = particle.xv();
- #if 0
- cout<<"compute_jacobians: particle.xf()"<<endl;
- cout<<particle.xf()<<endl;
- #endif
-
int rows = (particle.xf()).rows();
MatrixXf xf(rows,idf.size());
vector<MatrixXf> Pf;
- #if 0
- cout<<"compute jacobians:"<<endl;
- cout<<"particle.xf().rows() "<<(particle.xf()).rows()<<endl;
- cout<<"particle.xf().cols() "<<(particle.xf()).cols()<<endl;
- cout<<"idf.size() "<<idf.size()<<endl;
- cout<<"idf max "<<idf[idf.size()-1]<<endl;
- cout<<"xf.rows() "<<xf.rows()<<endl;
- cout<<"xf.cols() "<<xf.cols()<<endl;
- cout<<endl;
- #endif
unsigned i;
int r;
for (unsigned i=0; i<idf.size(); i++) {
@@ -47,40 +32,11 @@ void compute_jacobians(Particle particle,
MatrixXf HvMat(2,3);
MatrixXf HfMat (2,2);
-#if 0
- cout<<"before for loop in compute_jacobians"<<endl;
- cout<<"idf.size()"<<idf.size()<<endl;
- cout<<"xv "<<endl;
- cout<<xv<<endl;
- cout<<"should be 1.5052"<<endl;
- cout<<" -0.0293"<<endl;
- cout<<" -0.0079"<<endl;
- cout<<endl;
- cout<<"xf"<<endl;
- cout<<xf<<endl;
- cout<<"should be 3.4011"<<endl;
- cout<<" -25.6172"<<endl;
- cout<<endl;
- cout<<"Pf"<<endl;
- cout<<Pf[0]<<endl;
- cout<<"should be 0.2005 0.0195"<<endl;
- cout<<" 0.0196 0.0120"<<endl;
- cout<<endl;
-#endif
-
for (i=0; i<idf.size(); i++) {
dx = xf(0,i) - xv(0);
dy = xf(1,i) - xv(1);
d2 = pow(dx,2) + pow(dy,2);
d = sqrt(d2);
-
-#if 0
- cout<<"dx"<<dx<<endl;
- cout<<"dy"<<dy<<endl;
- cout<<"d2"<<d2<<endl;
- cout<<"d"<<d<<endl;
- cout<<"about to print zp"<<endl;
-#endif
//predicted observation
zp(0,i) = d;
91 testcode/data_associate_known.cpp
View
@@ -1,81 +1,56 @@
#include "data_associate_known.h"
#include <iostream>
-void data_associate_known(MatrixXf z, vector<int> ftag_visible, VectorXf &da_table, int Nf, \
+void data_associate_known(MatrixXf z, vector<int> idz, VectorXf &table, int Nf, \
MatrixXf &zf, vector<int> &idf, MatrixXf &zn)
{
- cout<<"z (r,c) = "<<z.rows()<<" "<<z.cols()<<endl;
- zf.resize(z.rows(), ftag_visible.size());
- zf.setZero();
- cout<<"zf (r,c) = "<<zf.rows()<<" "<<zf.cols()<<endl;
- zn.resize(z.rows(), ftag_visible.size());
- cout<<"zn (r,c) = "<<zn.rows()<<" "<<zn.cols()<<endl;
- zn.setZero();
idf.clear();
vector<int> idn;
unsigned i,ii,r;
- for (i =0; i< ftag_visible.size(); i++){
- ii = ftag_visible[i];
- if (da_table(ii) ==-1) { //new feature
+ //this extra loopis required to set the dimension of zn and zf
+ int zncols = 0;
+ int zfcols = 0;
+ for (i =0; i< idz.size(); i++){
+ ii = idz[i];
+ if (table(ii) ==-1) { //new feature
+ zncols++;
+ }
+ else {
+ zfcols++;
+ }
+ }
+
+ //resize
+ zn.resize(z.rows(),zncols);
+ zn.setZero();
+ zf.resize(z.rows(),zfcols);
+ zf.setZero();
+
+ int znc,zfc;
+ znc =0;
+ zfc =0;
+ for (i =0; i< idz.size(); i++){
+ ii = idz[i];
+ if (table(ii) ==-1) { //new feature
for (r=0; r<z.rows();r++) {
- zn(r,i) = z(r,i);
+ zn(r,znc) = z(r,i);
}
+ znc++;
idn.push_back(ii);
}
else {
for (r=0; r<z.rows(); r++) {
- zf(r,i) = z(r,i);
+ zf(r,zfc) = z(r,i);
}
- idf.push_back(da_table(ii));
+ zfc++;
+ idf.push_back(table(ii));
}
}
- //TODO: not sure if these are necessary...
- if (zf == MatrixXf::Zero(zf.rows(),zf.cols())) {
- zf = MatrixXf();
- }
-
- if (zn == MatrixXf::Zero(zn.rows(),zn.cols())) {
- zn = MatrixXf();
- }
- #if 0
- //test code
- vector<int>::iterator idniter;
- cout<<"idn: "<<endl;
- for (idniter=idn.begin(); idniter!=idn.end(); idniter++) {
- cout<<(*idniter)<<" ";
- }
- cout<<endl;
- cout<<"idn should be 0 and 21"<<endl;
-
- vector<int>::iterator idfiter;
- cout<<"idf: "<<endl;
- for (idfiter=idf.begin(); idfiter!=idf.end(); idfiter++) {
- cout<<(*idfiter)<<" ";
- }
- cout<<endl;
- cout<<"idf should be empty"<<endl;
-
- cout<<"Nf is "<<Nf<<endl;
- cout<<"Nf should be 0"<<endl;
- #endif
-
- //TODO: look into this later (da_table(ii) == 0) but counter starts from index 0...
- //add new feature IDs to lookup da_table
- vector<int> counter;
- for (unsigned int c=0; c<zn.cols(); c++) {
- counter.push_back(c+Nf);
+ for (int i=0; i<idn.size(); i++) {
+ table(idn[i]) = Nf+i;
}
-
- for (unsigned int d=0; d<idn.size(); d++) {
- da_table(idn[d]) = counter[d];
- }
-
- #if 0
- cout<<"da_table"<<endl;
- cout<<da_table<<endl;
- #endif
}
2  testcode/makefile
View
@@ -31,7 +31,7 @@ endif
CC := gcc
CXX := g++
-CXXFLAGS := -g -Wall -O3 -fmessage-length=0 -DGL_GLEXT_PROTOTYPES $(INCLUDE) $(MACROS)
+CXXFLAGS := -g -Wall -O0 -fmessage-length=0 -DGL_GLEXT_PROTOTYPES $(INCLUDE) $(MACROS)
LDFLAGS := $(FRAMEWORK) $(LIBRARY)
#-----------------------------------------
%.o : %.cpp
12 testcode/pi_to_pi.cpp
View
@@ -36,15 +36,3 @@ float pi_to_pi(float ang)
return ang;
}
-#if 0
-vector<int> find1(VectorXf input)
-{
- vector<int> index;
- for (int i =0; i<input.size(); i++) {
- if ((input[i] > 2*pi) || (input[i] < -2*pi)){
- index.push_back(i);
- }
- }
- return index;
-}
-#endif
234 testcode/temp/main.cpp
View
@@ -1,234 +0,0 @@
-#include <stdio.h>
-#include <algorithm>
-#include <iterator>
-#include <unistd.h>
-#include <errno.h>
-#include <string>
-#include <vector>
-#include <Eigen/Dense>
-#include <iostream>
-#include <math.h>
-#include <vector>
-
-#include "fastslam2_sim.h"
-#include "particle.h"
-#include "add_control_noise.h"
-#include "predict.h"
-#include "observe_heading.h"
-#include "get_observations.h"
-#include "add_observation_noise.h"
-#include "TransformToGlobal.h"
-#include "line_plot_conversion.h"
-#include "data_associate_known.h"
-#include "sample_proposal.h"
-#include "feature_update.h"
-#include "resample_particles.h"
-#include "add_feature.h"
-#include "stratified_resample.h"
-
-using namespace Eigen;
-using namespace std;
-using namespace config;
-
-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[])
-{
- MatrixXf lm;
- MatrixXf wp;
-
- read_input_file("example_webmap.mat", &lm, &wp);
-
- MatrixXf z(2,2);
- z<< 25.7745,25.2762,-1.4734,0.1384;
- vector<int> idz;
- idz.push_back(0);
- idz.push_back(21);
-
- VectorXf da_table(35);
- for (int i=0; i<35; i++) {
- da_table[i] = -1;
- }
- da_table[0] = 0;
- da_table[21] = 1;
-
- int Nf = 0;
-
- MatrixXf zf;//(z.rows(), idz.size());
- vector<int> idf;
- MatrixXf zn;//(z.rows(), idz.size());
- data_associate_known(z,idz,da_table,Nf,zf,idf,zn);
-
- cout<<"da_table"<<endl;
- cout<<da_table<<endl;
- cout<<"zf"<<endl;
- cout<<zf<<endl;
-
- MatrixXf test;
-
- cout<<"idf"<<endl;
- for (unsigned j=0; j<idf.size(); j++) {
- cout<<idf[j]<<" ";
- }
- cout<<endl;
-
- cout<<"zn"<<endl;
- cout<<zn<<endl;
-#if 0
- Particle p = Particle();
- p.setW(0.01);
-
- VectorXf xv(3);
- xv <<1.3237,-0.1459,-0.0326;
- p.setXv(xv);
-
- MatrixXf Pv(3,3);
- Pv.setZero();
- p.setPv(Pv);
-
- MatrixXf xf(2,2);
- xf<<2.9036,25.7074,-25.7157,3.2072;
- p.setXf(xf);
-
- vector<MatrixXf> Pf;
- MatrixXf Pf1(2,2);
- Pf1<<0.2009,0.0168,0.0168,0.0115;
- MatrixXf Pf2(2,2);
- Pf2<<0.0130,-0.0235,-0.0235,0.1916;
- Pf.push_back(Pf1);
- Pf.push_back(Pf2);
- p.setPf(Pf);
-
- MatrixXf z(2,2);
- z<<25.6306,24.6263,-1.4785,0.1662;
-
- vector<int>idf;
- idf.push_back(0);
- idf.push_back(1);
-
- MatrixXf R(2,2);
- R<<0.01,0,0,0.0003;
- feature_update(p,z,idf,R);
-
- cout<<"xv"<<endl;
- cout<<p.xv()<<endl;
- cout<<"Pv"<<endl;
- cout<<p.Pv()<<endl;
- cout<<"xf"<<endl;
- cout<<p.xf()<<endl;
- cout<<"Pf"<<endl;
- cout<<p.Pf()[0]<<endl;
- cout<<p.Pf()[1]<<endl;
-#endif
-}
225 testcode/temp/old_main.cpp
View
@@ -1,225 +0,0 @@
-#include <stdio.h>
-#include <algorithm>
-#include <iterator>
-#include <unistd.h>
-#include <errno.h>
-#include <string>
-#include <vector>
-#include <Eigen/Dense>
-#include <iostream>
-#include <math.h>
-#include <vector>
-
-#include "fastslam2_sim.h"
-#include "particle.h"
-#include "add_control_noise.h"
-#include "predict.h"
-#include "observe_heading.h"
-#include "get_observations.h"
-#include "add_observation_noise.h"
-#include "TransformToGlobal.h"
-#include "line_plot_conversion.h"
-#include "data_associate_known.h"
-#include "sample_proposal.h"
-#include "feature_update.h"
-#include "resample_particles.h"
-#include "add_feature.h"
-#include "stratified_resample.h"
-
-using namespace Eigen;
-using namespace std;
-using namespace config;
-
-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[])
-{
- MatrixXf lm;
- MatrixXf wp;
-
- read_input_file("example_webmap.mat", &lm, &wp);
-
- Particle p = Particle();
- p.setW(0.01);
-
- VectorXf xv(3);
- xv <<1.3237,-0.1459,-0.0326;
- p.setXv(xv);
-
- MatrixXf Pv(3,3);
- Pv.setZero();
- p.setPv(Pv);
-
- MatrixXf xf(2,2);
- xf<<2.9036,25.7074,-25.7157,3.2072;
- p.setXf(xf);
-
- vector<MatrixXf> Pf;
- MatrixXf Pf1(2,2);
- Pf1<<0.2009,0.0168,0.0168,0.0115;
- MatrixXf Pf2(2,2);
- Pf2<<0.0130,-0.0235,-0.0235,0.1916;
- Pf.push_back(Pf1);
- Pf.push_back(Pf2);
- p.setPf(Pf);
-
- MatrixXf z(2,2);
- z<<25.6306,24.6263,-1.4785,0.1662;
-
- #if 0 //these values cause likelihood to return 0
- Particle p = Particle();
- p.setW(0.01);
- VectorXf xv(3);
- xv<<1.3002,-0.1322,-0.0302;
- p.setXv(xv);
-
- MatrixXf Pv(3,3);
- Pv.setZero();
- p.setPv(Pv);
-
- MatrixXf xf(2,2);
- xf<<2.9547,25.7201,-25.7058,3.2473;
- p.setXf(xf);
-
- vector<MatrixXf>Pf;
- MatrixXf Pf1(2,2);
- Pf1<<0.2008,0.0171,0.0171,0.0115;
- MatrixXf Pf2(2,2);
- Pf2<<0.0131,-0.0238,-0.0238,0.1915;
- Pf.push_back(Pf1);
- Pf.push_back(Pf2);
- p.setPf(Pf);
-
- MatrixXf z(2,2);
- z<<25.6306,24.6263,-1.4785,0.1662;
-#endif
- vector<int>idf;
- idf.push_back(0);
- idf.push_back(1);
-
- MatrixXf R(2,2);
- R<<0.01,0,0,0.0003;
- feature_update(p,z,idf,R);
-
-
- cout<<"xv"<<endl;
- cout<<p.xv()<<endl;
- cout<<"Pv"<<endl;
- cout<<p.Pv()<<endl;
- cout<<"xf"<<endl;
- cout<<p.xf()<<endl;
- cout<<"Pf"<<endl;
- cout<<p.Pf()[0]<<endl;
- cout<<p.Pf()[1]<<endl;
-}
Please sign in to comment.
Something went wrong with that request. Please try again.