diff --git a/sm/csm/structprior/MeasurementsLikelihood.h b/sm/csm/structprior/MeasurementsLikelihood.h index 67633022a25c4560680d9cfab99cfb60578fb5cb..1b57a0265651a1b9d10a1d3cbafc5de4fd0e1ff0 100644 --- a/sm/csm/structprior/MeasurementsLikelihood.h +++ b/sm/csm/structprior/MeasurementsLikelihood.h @@ -17,10 +17,8 @@ public: MeasurementsLikelihood(int likelihood_function, int measurements_number); virtual ~MeasurementsLikelihood(void); //class variables -protected: - LDP laser_data; - int function_type; public: + int function_type; double error; std::vector<double> grad; std::vector<std::vector<double> > hess; diff --git a/sm/csm/structprior/Optimizer.cpp b/sm/csm/structprior/Optimizer.cpp index a0a50ab1fa2e099a68c5483c58e484d88c74f182..9413cd9cbf081b46c2a4e6409d056fa7309c511f 100644 --- a/sm/csm/structprior/Optimizer.cpp +++ b/sm/csm/structprior/Optimizer.cpp @@ -13,7 +13,51 @@ Optimizer::~Optimizer(void) } -void Optimizer::OptimizeAlphas() +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 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]); + + } + + for (int k=0;k<1000;k++) + { + // chek... + constraint_manager.ApplyConstraintsAlphas(x); + measurements_likelihood.ComputeAlphaLikelihoods(x,alpha0_vector,covs_vector); + + + + + } + + + } + + + if (measurements_likelihood.function_type == L1) + { + + + } + +} + +void Optimizer::OptimizeRanges() +{ + +} + +void Optimizer::OptimizePoses() +{ + } diff --git a/sm/csm/structprior/Optimizer.h b/sm/csm/structprior/Optimizer.h index 7db1699e30c6792aa7b791724da440f07c70b258..86077b19b98c57e447ee4cc77f86ebbe331b68c6 100644 --- a/sm/csm/structprior/Optimizer.h +++ b/sm/csm/structprior/Optimizer.h @@ -17,7 +17,9 @@ public: MeasurementsLikelihood measurements_likelihood; ConstraintManager constraint_manager; //methods - void OptimizeAlphas(); + std::vector<double> OptimizeAlphas(); + void OptimizeRanges(); + void OptimizePoses(); }; #endif