diff --git a/sm/csm/structprior/CMakeLists.txt b/sm/csm/structprior/CMakeLists.txt
deleted file mode 100644
index 3754e7bbab3e8e2891724c61f6c1a995dc77cf9f..0000000000000000000000000000000000000000
--- a/sm/csm/structprior/CMakeLists.txt
+++ /dev/null
@@ -1,10 +0,0 @@
-# This builds the code as itself
-
-cmake_minimum_required(VERSION 2.4)
-
-SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -pedantic -std=c99")
-SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -ggdb")
-SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall")
-
-
-ADD_EXECUTABLE(structprior_test structprior_test.cpp ConstraintManager.cpp Constraint.cpp)
diff --git a/sm/csm/structprior/Constraint.cpp b/sm/csm/structprior/Constraint.cpp
deleted file mode 100644
index af5def1c2bc6195080e6fe0e9fe8b8ce3a05126e..0000000000000000000000000000000000000000
--- a/sm/csm/structprior/Constraint.cpp
+++ /dev/null
@@ -1,77 +0,0 @@
-#include "Constraint.h"
-
-
-Constraint::Constraint(int t)
-{
- type = t;
-}
-
-Constraint::~Constraint(void)
-{
-
-}
-
-void Constraint::SetType(int t)
-{
- type = t;
-}
-
-Values Constraint::ApplyConstraint(double alphas[], double params[])
-{
- Values v;
- double err = 0;
- double* grd;
- int size = 3;
-
- switch(type)
- {
- //*********************************************************************************
- case EQUAL_TO_EITHER:
- {
- size = 3;
- double e1 = 0.5*(alphas[1]-alphas[0])*(alphas[1]-alphas[0]);
- double e2 = 0.5*(alphas[2]-alphas[1]);
-
- if (e1 < e2)
- {
- err = e1;
-
- }
-
- else
- {
- err = e2;
- }
-
-
- break;
- }
- //*********************************************************************************
- case LOCK_DIFF:
- {
- size = 2;
- double bias = params[0]; double threshold = params[1];
- if (abs( alphas[1] - (alphas[0]+bias) ) < threshold)
- {
- e = 0.5*( alphas[1] - (alphas[0]+bias) );
- }
- else
- {
-
- }
-
- break;
- }
- //*********************************************************************************
-
- default:
- sm_debug("Unrecognized type of constraint \n");
-
- }
-
- v.error = err;
- for (int i=0;i<size;i++)
- v.grad[i] = grd[i];
-
- return v;
-}
diff --git a/sm/csm/structprior/Constraint.h b/sm/csm/structprior/Constraint.h
deleted file mode 100644
index 3260aca8d50ebe3bc7ac148e9870e081d02b3cad..0000000000000000000000000000000000000000
--- a/sm/csm/structprior/Constraint.h
+++ /dev/null
@@ -1,36 +0,0 @@
-#ifndef H_CONSTRAINT
-#define H_CONSTRAINT
-
-#define EQUAL_TO_EITHER 1
-#define LOCK_DIFF 2
-
-#define PI 3.14159
-
-#include <csm/csm_all.h>
-
-struct Values{
-
- double error;
- double grad[];
-
-};
-
-
-class Constraint
-{
-public:
-//constructors
- Constraint(int t);
- virtual ~Constraint(void);
-//class variables
- double e;
- double grad;
-protected:
- int type;
-// class methods
-public:
- Values ApplyConstraint(double alphas[], double params[] = NULL);
- void SetType(int t);
-};
-
-#endif
diff --git a/sm/csm/structprior/ConstraintManager.cpp b/sm/csm/structprior/ConstraintManager.cpp
deleted file mode 100644
index 951125a1e2bf760bbcc634da7d4ac5b7c7462bb5..0000000000000000000000000000000000000000
--- a/sm/csm/structprior/ConstraintManager.cpp
+++ /dev/null
@@ -1,124 +0,0 @@
-#include "ConstraintManager.h"
-
-
-ConstraintManager::ConstraintManager()
-{
- sm_debug("ConstraintManager should take parameters");
-
-}
-
-ConstraintManager::ConstraintManager(std::vector<int> constraint_types)
-{
- equal_to_either_num = 10;
- lock_diff_threshold = deg2rad(5);
-
- constraint_types_to_apply = constraint_types;
-
-}
-
-ConstraintManager::~ConstraintManager()
-{
- for (int i=0; i<constraints.size();i++)
- {
- delete constraints[i];
- }
- constraints.clear();
-
-}
-
-void ConstraintManager::ClearConstraints()
-{
- for (int i=0; i<constraints.size();i++)
- {
- delete constraints[i];
- }
- constraints.clear();
-
-}
-
-void ConstraintManager::ApplyConstraintsAlphas(std::vector<double> x_vector)
-{
- //add as many types of constraints as wished
- bool equal_to_either_active = true;
- bool apply_equal_to_either = false;
- bool lock_diff_active = true;
- bool apply_lock_diff = false;
-
- int n = x_vector.size();
-
- //***************************************************************************
- // select the types of constraints we want to apply
- for (int i=0;i< constraint_types_to_apply.size();i++)
- {
- if (equal_to_either_active && constraint_types_to_apply[i] == EQUAL_TO_EITHER)
- {
- apply_equal_to_either = true;
- equal_to_either_active = false;
- continue;
- }
-
- if (lock_diff_active && constraint_types_to_apply[i] == LOCK_DIFF)
- {
- apply_lock_diff = true;
- lock_diff_active = false;
- continue;
- }
-
- }
- //***************************************************************************
- if(apply_equal_to_either) //add equal_to_either constraints
- {
- //this all could be substituted by a function ApplyEqualToEither
- for (int i=equal_to_either_num-1;i<n-equal_to_either_num;i++)
- {
-
- int mb = min (i-1, n-i-1);
- mb = min (mb, equal_to_either_num-1);
- for (int j=0; j< mb;j++) //constraints with no consecutive points
- {
- Constraint* c = new Constraint(EQUAL_TO_EITHER);
- double* alpha_values;
- alpha_values[0]= x_vector[i-j];
- alpha_values[1]= x_vector[i];
- alpha_values[2]= x_vector[i+j];
- Values v = c->ApplyConstraint(alpha_values);
- constraints.push_back(c);
- e += v.error;
- //update grad
- //update hess?
-
- }
-
- }
-
- }
- //***************************************************************************
- if(apply_lock_diff) //add lock_diff constraints
- {
- for (int i=0;i<n-1;i++)
- {
- int* ind;
- ind[0] = i; ind[1]=i+1;
- double* alpha_values;
- alpha_values[0]= x_vector[i];
- alpha_values[1]= x_vector[i+1];
- double* p;
- p[0] = PI/2; p[1] = lock_diff_threshold;
- Constraint* c1 = new Constraint(LOCK_DIFF);
- Values v = c1->ApplyConstraint(alpha_values,p);
- constraints.push_back(c1);
- e += v.error;
- p[0] = -PI/2;
- Constraint* c2 = new Constraint(LOCK_DIFF);
- v = c2->ApplyConstraint(alpha_values,p);
- constraints.push_back(c2);
- e += v.error;
- //update grad
- //update hess?
-
- }
- }
-
-
-
-}
diff --git a/sm/csm/structprior/ConstraintManager.h b/sm/csm/structprior/ConstraintManager.h
deleted file mode 100644
index 1266f91891fe45238f0120242042705506f8fa65..0000000000000000000000000000000000000000
--- a/sm/csm/structprior/ConstraintManager.h
+++ /dev/null
@@ -1,45 +0,0 @@
-#ifndef H_CONSTRAINTMANAGER
-#define H_CONSTRAINTMANAGER
-
-#include <vector>
-
-#include "Constraint.h"
-
-
-class ConstraintManager
-{
-public:
-//constructors
- ConstraintManager();
- ConstraintManager(std::vector<int> constraint_types);
- virtual ~ConstraintManager(void);
-
-//class variables
- double e;
- std::vector<double> grad;
- std::vector<std::vector<double> > hess;
-
- std::vector<Constraint*> constraints;
-protected:
- std::vector<int> constraint_types_to_apply;
- int equal_to_either_num;
- double lock_diff_threshold;
-
-//methods
-public:
- void ApplyConstraintsAlphas(std::vector<double> x_vector);
- void ClearConstraints();
-
-protected:
- void ApplyEqualToEither();
- void ApplyLockDiff();
-
-
-
-
-
-
-};
-
-#endif
-
diff --git a/sm/csm/structprior/MeasurementsLikelihood.cpp b/sm/csm/structprior/MeasurementsLikelihood.cpp
deleted file mode 100644
index 04f63d004e8efd1cc816b9cf428b1d493c3f3641..0000000000000000000000000000000000000000
--- a/sm/csm/structprior/MeasurementsLikelihood.cpp
+++ /dev/null
@@ -1,123 +0,0 @@
-#include "MeasurementsLikelihood.h"
-
-MeasurementsLikelihood::MeasurementsLikelihood()
-{
- sm_debug("MeasurementsLikelihood should take parameters");
-
-}
-
-MeasurementsLikelihood::MeasurementsLikelihood(int likelihood_function, int measurements_number)
-{
- function_type = likelihood_function;
-
- error = 0;
- grad.resize(measurements_number);
- hess.resize(measurements_number);
- for (int i=0;i<measurements_number;i++)
- hess[i].resize(measurements_number);
-
-}
-
-MeasurementsLikelihood::~MeasurementsLikelihood()
-{
-
-}
-
-
-void MeasurementsLikelihood::ComputeAlphaLikelihoods(std::vector<double> x_vector, std::vector<double> alphas0, std::vector<double> alphas_covs )
-{
- int n = x_vector.size();
-
- error = 0;
- if (grad.size() != 0)
- grad.clear();
- if (hess.size() != 0)
- hess.clear();
-
- // may be redundant, it's added in case some measurements are pre-discarded or something (measurements outside the model...)
- grad.resize(n);
- hess.resize(n);
- for (int i=0;i<n;i++)
- hess[i].resize(n);
-
- //****************************************************************************************
- if (function_type == L2)
- {
-
- for (int i=0;i<n;i++)
- {
- double cov_alpha = alphas_covs[i];
-
- double e = 0.5 * (x_vector[i] - alphas0[i]) * (x_vector[i] - alphas0[i]) /cov_alpha;
- double g= - (alphas0[i] - x_vector[i])/ cov_alpha;
- double h = 1/cov_alpha;
-
- error+= e;
- grad[i] = g;
-
- for(int k=0;k<n;k++)
- {
- for (int l=0;l<n;l++)
- {
- if(k==n && l==n)
- hess[k][l] = h;
- else
- hess[k][l] = 0;
-
- }
- }
-
- }
-
-
- }
- //****************************************************************************************
- if (function_type == L1)
- {
-
-
- }
-
-}
-
-
-void MeasurementsLikelihood::ComputeRangeLikelihoods(std::vector<double> x_vector, std::vector<double> ranges0, std::vector<double> ranges_covs)
-{
-
- if (function_type == L2)
- {
-
-
-
-
-
- }
-
-
-
- if (function_type == L1)
- {
-
-
-
-
-
- }
-
-
-
-
-
-
-
-}
-
-
-
-
-
-
-
-
-
-
diff --git a/sm/csm/structprior/MeasurementsLikelihood.h b/sm/csm/structprior/MeasurementsLikelihood.h
deleted file mode 100644
index 1b57a0265651a1b9d10a1d3cbafc5de4fd0e1ff0..0000000000000000000000000000000000000000
--- a/sm/csm/structprior/MeasurementsLikelihood.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef H_MEASUREMENTSLIKELIHOOD
-#define H_MEASUREMENTSLIKELIHOOD
-
-#define L1 1
-#define L2 2
-
-#include <csm/csm_all.h>
-#include <vector>
-
-
-
-class MeasurementsLikelihood
-{
-public:
-//constructors
- MeasurementsLikelihood();
- MeasurementsLikelihood(int likelihood_function, int measurements_number);
- virtual ~MeasurementsLikelihood(void);
- //class variables
-public:
- int function_type;
- double error;
- std::vector<double> grad;
- std::vector<std::vector<double> > hess;
- //methods
-public:
- void ComputeAlphaLikelihoods(std::vector<double> x_vector, std::vector<double> alphas0, std::vector<double> alphas_covs);
- void ComputeRangeLikelihoods(std::vector<double> x_vector, std::vector<double> ranges0, std::vector<double> ranges_covs);
-
-};
-
-#endif
diff --git a/sm/csm/structprior/Optimizer.cpp b/sm/csm/structprior/Optimizer.cpp
deleted file mode 100644
index f7c07cd564c27c91225d404c958293d0c08593c2..0000000000000000000000000000000000000000
--- a/sm/csm/structprior/Optimizer.cpp
+++ /dev/null
@@ -1,191 +0,0 @@
-#include "Optimizer.h"
-
-#include <gsl/gsl_linalg.h>
- #include <gsl/gsl_blas.h>
-
-
-Optimizer::Optimizer(LDP ld, MeasurementsLikelihood ml, ConstraintManager cm)
-{
- laser_data = ld;
- measurements_likelihood = ml;
- constraint_manager = cm;
-
-}
-
-Optimizer::~Optimizer(void)
-{
-
-}
-
-/*void Optimizer::ScanLevelOptimization()
-{
- OptimizeAlphas();
- OptimizeRanges();
-
-
-
-}*/
-
-std::vector<double> Optimizer::OptimizeAlphas()
-{
- std::vector<double> x;
-
- std::vector<double> alpha0_vector;
- std::vector<double> covs_vector;
- for(int l=0;l<laser_data->nrays;l++)
- {
- alpha0_vector.push_back(laser_data->alpha[l]);
- x.push_back(laser_data->alpha[l]);
- covs_vector.push_back(laser_data->cov_alpha[l]);
-
- }
-
- //********************************************************************************
- if (measurements_likelihood.function_type == L2) //L2, Newton minimization
- {
- double lambda = 0.1;
-
- for (int k=0;k<1000;k++)
- {
- // chek...
- if (lambda < 0.0000000000000001)
- {
- printf ("Lambda = %f is too small for meaningful results, exiting.\n", lambda);
- return x;
- }
-
- constraint_manager.ApplyConstraintsAlphas(x);
- measurements_likelihood.ComputeAlphaLikelihoods(x,alpha0_vector,covs_vector);
-
- double err_k;
- x = NewtonStep(x,lambda,err_k);
-
- /*int size = measurements_likelihood.grad.size();
- double gradient[size];
- for (int i=0;i<size;i++)
- {
- gradient[i] = constraint_manager.grad[i] + lambda*measurements_likelihood.grad[i];
- }
-
- int size_h = measurements_likelihood.hess.size();
- if (size_h != size)
- sm_error("Gradient and Hessian dimensions must agree\n");
- double hessian[size_h*size_h];
- int ne = 0;
- for(int i=0;i<size_h;i++)
- for(int j=0;j<size_h;j++)
- {
- hessian[ne] = constraint_manager.hess[i][j] + lambda*measurements_likelihood.hess[i][j];
- ne++;
- }
-
- gsl_matrix_view h = gsl_matrix_view_array(hessian,size_h,size_h);
- gsl_vector_view g = gsl_vector_view_array(gradient,size);
-
- gsl_vector *dx = gsl_vector_alloc (4);
-
- int s;
-
- gsl_permutation * p = gsl_permutation_alloc (size_h);
-
- gsl_linalg_LU_decomp (&h.matrix, p, &s);
-
- gsl_linalg_LU_solve (&h.matrix, p, &g.vector, dx);
-
- for (int i=0;i<size;i++)
- {
- x[i]-= gsl_vector_get(dx,i);
- }
-
- double err_k;
- const gsl_vector* const_g = &g.vector;
- gsl_blas_ddot (const_g, dx, &err_k);
-
-
- gsl_permutation_free (p);
- gsl_vector_free (dx);*/
-
- if (err_k < 0.000000001)
- lambda = lambda * 0.1;
-
-
- }
-
-
- }
-
- //********************************************************************************
- if (measurements_likelihood.function_type == L1) //L1, regularized problem
- {
-
-
- }
-
- return x;
-
-}
-
-std::vector<double> Optimizer::OptimizeRanges()
-{
-
-}
-
-/*std::vector<Pose> Optimizer::OptimizePoses()
-{
-
-
-}*/
-
-std::vector<double> Optimizer::NewtonStep(std::vector<double> xv,double lambda_parameter,double &err )
-{
- std::vector<double> x_return;
- x_return = xv;
- int size = measurements_likelihood.grad.size();
- double gradient[size];
- for (int i=0;i<size;i++)
- {
- gradient[i] = constraint_manager.grad[i] + lambda_parameter*measurements_likelihood.grad[i];
- }
-
- int size_h = measurements_likelihood.hess.size();
- if (size_h != size)
- sm_error("Gradient and Hessian dimensions must agree\n");
- double hessian[size_h*size_h];
- int ne = 0;
- for(int i=0;i<size_h;i++)
- for(int j=0;j<size_h;j++)
- {
- hessian[ne] = constraint_manager.hess[i][j] + lambda_parameter*measurements_likelihood.hess[i][j];
- ne++;
- }
-
- gsl_matrix_view h = gsl_matrix_view_array(hessian,size_h,size_h);
- gsl_vector_view g = gsl_vector_view_array(gradient,size);
-
- gsl_vector *dx = gsl_vector_alloc (4);
-
- int s;
-
- gsl_permutation * p = gsl_permutation_alloc (size_h);
-
- gsl_linalg_LU_decomp (&h.matrix, p, &s);
-
- gsl_linalg_LU_solve (&h.matrix, p, &g.vector, dx);
-
- for (int i=0;i<size;i++)
- {
- x_return[i]-= gsl_vector_get(dx,i);
- }
-
-
- const gsl_vector* const_g = &g.vector;
- gsl_blas_ddot (const_g, dx, &err);
-
-
- gsl_permutation_free (p);
- gsl_vector_free (dx);
-
- return x_return;
-
-
-}
diff --git a/sm/csm/structprior/Optimizer.h b/sm/csm/structprior/Optimizer.h
deleted file mode 100644
index 9814a2591c0ed484c8fe36a4f32162518fc464a7..0000000000000000000000000000000000000000
--- a/sm/csm/structprior/Optimizer.h
+++ /dev/null
@@ -1,29 +0,0 @@
-#ifndef H_OPTIMIZER
-#define H_OPTIMIZER
-
-#include <csm/csm_all.h>
-#include "MeasurementsLikelihood.h"
-#include "ConstraintManager.h"
-
-class Optimizer
-{
-
-//constructors
-public:
- Optimizer(LDP ld, MeasurementsLikelihood ml, ConstraintManager cm);
- virtual ~Optimizer(void);
-//class variables
- LDP laser_data;
- MeasurementsLikelihood measurements_likelihood;
- ConstraintManager constraint_manager;
-//methods
- std::vector<double> OptimizeAlphas();
- std::vector<double> OptimizeRanges();
- //std::vector<Pose> OptimizePoses();
-
- //void ScanLevelOptimization();
-
- std::vector<double> NewtonStep(std::vector<double> xv, double lambda_parameter, double &err);
-
-};
-#endif
diff --git a/sm/csm/structprior/structprior_test.cpp b/sm/csm/structprior/structprior_test.cpp
deleted file mode 100644
index b0a4a9478801283ae11ab644cee42eb1402ae0a3..0000000000000000000000000000000000000000
--- a/sm/csm/structprior/structprior_test.cpp
+++ /dev/null
@@ -1,46 +0,0 @@
-#include <stdio.h>
-#include <csm/csm_all.h>
-
-#include "ConstraintManager.h"
-#include "MeasurementsLikelihood.h"
-#include "Optimizer.h"
-
-
-int main(int argc, const char** argv)
-{
-
- if (argc < 2)
- {
- sm_error("Provide input file's name as an argument\n");
- return -1;
- }
-
- const char* file_name = argv[1];
- FILE* input_file = fopen(file_name, "r");
-
-
- LDP laserdata;
- if(!(laserdata = ld_read_smart(input_file))) {
- sm_error("Could not read scan.\n");
- return -1;
- }
-
-
- // the types of constraints we want to apply (i.e. depending on the environment...)
- std::vector<int> cons_types;
- cons_types.push_back(EQUAL_TO_EITHER);
- //cons_types.push_back(LOCK_DIFF);
-
- ConstraintManager cons_manager(cons_types);
-
- int number_of_measurements = laserdata->nrays;
- MeasurementsLikelihood f(L2,number_of_measurements);
-
- Optimizer optimizer(laserdata,f,cons_manager);
- optimizer.OptimizeAlphas();
-
-
-
- return 0;
-
-}