From 31c095486d80170f0ead3c89a30998fa5bf0f5e2 Mon Sep 17 00:00:00 2001 From: Paloma de la Puente <paloma.delapuente@upm.es> Date: Fri, 23 Oct 2009 00:04:43 +0000 Subject: [PATCH] likelihood function... --- sm/CMakeLists.txt | 1 + sm/csm/structprior/MeasurementsLikelihood.cpp | 46 +++++++++++++++++-- sm/csm/structprior/MeasurementsLikelihood.h | 4 +- sm/csm/structprior/structprior_test.cpp | 3 +- 4 files changed, 48 insertions(+), 6 deletions(-) diff --git a/sm/CMakeLists.txt b/sm/CMakeLists.txt index 8a7b3ef..0824a24 100644 --- a/sm/CMakeLists.txt +++ b/sm/CMakeLists.txt @@ -162,6 +162,7 @@ SET(csm_sources ./csm/structprior/ConstraintManager.cpp ./csm/structprior/Constraint.cpp ./csm/structprior/MeasurementsLikelihood.cpp +./csm/structprior/Minimizer.cpp ./lib/egsl/egsl.c ./lib/egsl/egsl_conversions.c ./lib/egsl/egsl_misc.c diff --git a/sm/csm/structprior/MeasurementsLikelihood.cpp b/sm/csm/structprior/MeasurementsLikelihood.cpp index 55c6069..8bf0a58 100644 --- a/sm/csm/structprior/MeasurementsLikelihood.cpp +++ b/sm/csm/structprior/MeasurementsLikelihood.cpp @@ -3,6 +3,7 @@ MeasurementsLikelihood::MeasurementsLikelihood(int likelihood_function, int measurements_number) { function_type = likelihood_function; + error = 0; grad.resize(measurements_number); hess.resize(measurements_number); @@ -27,7 +28,7 @@ void MeasurementsLikelihood::ComputeAlphaLikelihoods(std::vector<double> x_vecto if (hess.size() != 0) hess.clear(); - // may be redundant, it's added in case some measurements are pre-discarded + // may be redundant, it's added in case some measurements are pre-discarded or something grad.resize(n); hess.resize(n); for (int i=0;i<n;i++) @@ -60,8 +61,6 @@ void MeasurementsLikelihood::ComputeAlphaLikelihoods(std::vector<double> x_vecto } } - - } @@ -72,6 +71,47 @@ void MeasurementsLikelihood::ComputeAlphaLikelihoods(std::vector<double> x_vecto } + +} + + +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 index 21da7f8..6c13dbb 100644 --- a/sm/csm/structprior/MeasurementsLikelihood.h +++ b/sm/csm/structprior/MeasurementsLikelihood.h @@ -25,8 +25,8 @@ public: 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 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); }; diff --git a/sm/csm/structprior/structprior_test.cpp b/sm/csm/structprior/structprior_test.cpp index c73c6c0..eb723b2 100644 --- a/sm/csm/structprior/structprior_test.cpp +++ b/sm/csm/structprior/structprior_test.cpp @@ -3,6 +3,7 @@ #include "ConstraintManager.h" #include "MeasurementsLikelihood.h" +#include "Minimizer.h" int main(int argc, const char** argv) @@ -35,7 +36,7 @@ int main(int argc, const char** argv) int number_of_measurements = laserdata->nrays; MeasurementsLikelihood f(L2,number_of_measurements); - + Minimizer minimizer; //minimizer.Minimize(laserdata,f,cons_manager...)... -- GitLab