From 2966be41b7e245deba510733ebff6850802f398b Mon Sep 17 00:00:00 2001
From: Paloma de la Puente <paloma.delapuente@upm.es>
Date: Fri, 23 Oct 2009 01:13:35 +0000
Subject: [PATCH] optimizer added
---
sm/csm/structprior/MeasurementsLikelihood.h | 4 +-
sm/csm/structprior/Optimizer.cpp | 46 ++++++++++++++++++++-
sm/csm/structprior/Optimizer.h | 4 +-
3 files changed, 49 insertions(+), 5 deletions(-)
diff --git a/sm/csm/structprior/MeasurementsLikelihood.h b/sm/csm/structprior/MeasurementsLikelihood.h
index 6763302..1b57a02 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 a0a50ab..9413cd9 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 7db1699..86077b1 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
--
GitLab