diff --git a/sm/csm/structprior/MeasurementsLikelihood.cpp b/sm/csm/structprior/MeasurementsLikelihood.cpp index 545dee0c5e0b8403a8ac8a971addcfda295b7974..55c606966f2e09517fe250d2339505435428338d 100644 --- a/sm/csm/structprior/MeasurementsLikelihood.cpp +++ b/sm/csm/structprior/MeasurementsLikelihood.cpp @@ -1,32 +1,72 @@ #include "MeasurementsLikelihood.h" -MeasurementsLikelihood::MeasurementsLikelihood(int likelihood_function) +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::ComputeLikelihoods(std::vector<double> x_vector) + +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 + 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 error = - + 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) { diff --git a/sm/csm/structprior/MeasurementsLikelihood.h b/sm/csm/structprior/MeasurementsLikelihood.h index d663ab6cf65bd4b619501f972f189467b0dad8c0..21da7f83a20f46e04875b76478d8ca759d46ae6c 100644 --- a/sm/csm/structprior/MeasurementsLikelihood.h +++ b/sm/csm/structprior/MeasurementsLikelihood.h @@ -7,27 +7,25 @@ #include <csm/csm_all.h> #include <vector> -struct LValues{ - double error; - double grad[]; - -}; class MeasurementsLikelihood { public: //constructors - MeasurementsLikelihood(int likelihood_function); + MeasurementsLikelihood(int likelihood_function, int measurements_number); virtual ~MeasurementsLikelihood(void); //class variables protected: + LDP laser_data; int function_type; public: - std::vector<LValues> values_vector; + double error; + std::vector<double> grad; + std::vector<std::vector<double> > hess; //methods public: - void ComputeLikelihoods(std::vector<double> x_vector); + void ComputeAlphaLikelihoods(std::vector<double> x_vector, std::vector<double> alphas0, std::vector<double> alphas_covs ); }; diff --git a/sm/csm/structprior/structprior_test.cpp b/sm/csm/structprior/structprior_test.cpp index 26b4f4b7048ff95faad09cfaefcf7b03830ad6c5..c73c6c012ba885c433ed978b1322549892400b34 100644 --- a/sm/csm/structprior/structprior_test.cpp +++ b/sm/csm/structprior/structprior_test.cpp @@ -32,7 +32,8 @@ int main(int argc, const char** argv) ConstraintManager cons_manager(cons_types); - MeasurementsLikelihood f(L2); + int number_of_measurements = laserdata->nrays; + MeasurementsLikelihood f(L2,number_of_measurements); //minimizer.Minimize(laserdata,f,cons_manager...)...