Skip to content
Snippets Groups Projects
Commit 8965425e authored by Andrea Censi's avatar Andrea Censi
Browse files

No commit message 

No commit message
parent aaec43dd
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 0 additions and 713 deletions
sm/csm/structprior/CMakeLists.txt deleted 100644 → 0
+ 0
− 10
View file @ aaec43dd
# 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)
sm/csm/structprior/Constraint.cpp deleted 100644 → 0
+ 0
− 77
View file @ aaec43dd
#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;
}
sm/csm/structprior/Constraint.h deleted 100644 → 0
+ 0
− 36
View file @ aaec43dd
#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
sm/csm/structprior/ConstraintManager.cpp deleted 100644 → 0
+ 0
− 124
View file @ aaec43dd
#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?
}
}
}
sm/csm/structprior/ConstraintManager.h deleted 100644 → 0
+ 0
− 45
View file @ aaec43dd
#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
sm/csm/structprior/MeasurementsLikelihood.cpp deleted 100644 → 0
+ 0
− 123
View file @ aaec43dd
#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)
{
}
}
sm/csm/structprior/MeasurementsLikelihood.h deleted 100644 → 0
+ 0
− 32
View file @ aaec43dd
#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
sm/csm/structprior/Optimizer.cpp deleted 100644 → 0
+ 0
− 191
View file @ aaec43dd
#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;
}
sm/csm/structprior/Optimizer.h deleted 100644 → 0
+ 0
− 29
View file @ aaec43dd
#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
sm/csm/structprior/structprior_test.cpp deleted 100644 → 0
+ 0
− 46
View file @ aaec43dd
#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;
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment