From c6138826f7cfa3853ae7c60dae2423f7e3b13ef3 Mon Sep 17 00:00:00 2001 From: Paloma de la Puente <paloma.delapuente@upm.es> Date: Fri, 23 Oct 2009 18:46:54 +0000 Subject: [PATCH] Newton implementation --- sm/csm/structprior/ConstraintManager.cpp | 4 ++ sm/csm/structprior/ConstraintManager.h | 3 + sm/csm/structprior/Optimizer.cpp | 92 +++++++++++++++++++----- sm/csm/structprior/Optimizer.h | 4 +- 4 files changed, 85 insertions(+), 18 deletions(-) diff --git a/sm/csm/structprior/ConstraintManager.cpp b/sm/csm/structprior/ConstraintManager.cpp index 5d0b68b..951125a 100644 --- a/sm/csm/structprior/ConstraintManager.cpp +++ b/sm/csm/structprior/ConstraintManager.cpp @@ -84,6 +84,8 @@ void ConstraintManager::ApplyConstraintsAlphas(std::vector<double> x_vector) Values v = c->ApplyConstraint(alpha_values); constraints.push_back(c); e += v.error; + //update grad + //update hess? } @@ -111,6 +113,8 @@ void ConstraintManager::ApplyConstraintsAlphas(std::vector<double> x_vector) 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 index 0bc60bd..1266f91 100644 --- a/sm/csm/structprior/ConstraintManager.h +++ b/sm/csm/structprior/ConstraintManager.h @@ -16,6 +16,9 @@ public: //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; diff --git a/sm/csm/structprior/Optimizer.cpp b/sm/csm/structprior/Optimizer.cpp index 9413cd9..9375464 100644 --- a/sm/csm/structprior/Optimizer.cpp +++ b/sm/csm/structprior/Optimizer.cpp @@ -1,5 +1,9 @@ #include "Optimizer.h" +#include <gsl/gsl_linalg.h> + #include <gsl/gsl_blas.h> + + Optimizer::Optimizer(LDP ld, MeasurementsLikelihood ml, ConstraintManager cm) { laser_data = ld; @@ -16,34 +20,90 @@ Optimizer::~Optimizer(void) std::vector<double> Optimizer::OptimizeAlphas() { std::vector<double> x; - if (measurements_likelihood.function_type == L2) + + std::vector<double> alpha0_vector; + std::vector<double> covs_vector; + for(int l=0;l<laser_data->nrays;l++) { - std::vector<double> alpha0_vector; - std::vector<double> covs_vector; - for(int i=0;i<laser_data->nrays;i++) - { - alpha0_vector.push_back(laser_data->alpha[i]); - x.push_back(laser_data->alpha[i]); - covs_vector.push_back(laser_data->cov_alpha[i]); - - } - + 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); + 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) + //******************************************************************************** + if (measurements_likelihood.function_type == L1) //L1, regularized problem { @@ -51,13 +111,13 @@ std::vector<double> Optimizer::OptimizeAlphas() } -void Optimizer::OptimizeRanges() +std::vector<double> Optimizer::OptimizeRanges() { } -void Optimizer::OptimizePoses() +/*std::vector<Pose> Optimizer::OptimizePoses() { -} +}*/ diff --git a/sm/csm/structprior/Optimizer.h b/sm/csm/structprior/Optimizer.h index 86077b1..2942892 100644 --- a/sm/csm/structprior/Optimizer.h +++ b/sm/csm/structprior/Optimizer.h @@ -18,8 +18,8 @@ public: ConstraintManager constraint_manager; //methods std::vector<double> OptimizeAlphas(); - void OptimizeRanges(); - void OptimizePoses(); + std::vector<double> OptimizeRanges(); + //std::vector<Pose> OptimizePoses(); }; #endif -- GitLab