From 768cb2aa366223a88eba224ca402888e1a4c3a4b Mon Sep 17 00:00:00 2001
From: Paloma de la Puente <paloma.delapuente@upm.es>
Date: Fri, 23 Oct 2009 19:02:59 +0000
Subject: [PATCH] Newton implementation
---
sm/csm/structprior/Optimizer.cpp | 8 ++++----
sm/csm/structprior/Optimizer.h | 2 +-
2 files changed, 5 insertions(+), 5 deletions(-)
diff --git a/sm/csm/structprior/Optimizer.cpp b/sm/csm/structprior/Optimizer.cpp
index fa9838a..3310b20 100644
--- a/sm/csm/structprior/Optimizer.cpp
+++ b/sm/csm/structprior/Optimizer.cpp
@@ -49,7 +49,7 @@ std::vector<double> Optimizer::OptimizeAlphas()
measurements_likelihood.ComputeAlphaLikelihoods(x,alpha0_vector,covs_vector);
double err_k;
- x = NewtonStep(x,err_k);
+ x = NewtonStep(x,lambda,err_k);
/*int size = measurements_likelihood.grad.size();
double gradient[size];
@@ -127,7 +127,7 @@ std::vector<double> Optimizer::OptimizeRanges()
}*/
-std::vector<double> Optimizer::NewtonStep(std::vector<double> xv, double &err )
+std::vector<double> Optimizer::NewtonStep(std::vector<double> xv,double lambda_parameter,double &err )
{
std::vector<double> x_return;
x_return = xv;
@@ -135,7 +135,7 @@ std::vector<double> Optimizer::NewtonStep(std::vector<double> xv, double &err )
double gradient[size];
for (int i=0;i<size;i++)
{
- gradient[i] = constraint_manager.grad[i] + lambda*measurements_likelihood.grad[i];
+ gradient[i] = constraint_manager.grad[i] + lambda_parameter*measurements_likelihood.grad[i];
}
int size_h = measurements_likelihood.hess.size();
@@ -146,7 +146,7 @@ std::vector<double> Optimizer::NewtonStep(std::vector<double> xv, double &err )
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];
+ hessian[ne] = constraint_manager.hess[i][j] + lambda_parameter*measurements_likelihood.hess[i][j];
ne++;
}
diff --git a/sm/csm/structprior/Optimizer.h b/sm/csm/structprior/Optimizer.h
index fcaa649..8145b37 100644
--- a/sm/csm/structprior/Optimizer.h
+++ b/sm/csm/structprior/Optimizer.h
@@ -21,7 +21,7 @@ public:
std::vector<double> OptimizeRanges();
//std::vector<Pose> OptimizePoses();
- std::vector<double> NewtonStep(std::vector<double> xv, double &err);
+ std::vector<double> NewtonStep(std::vector<double> xv, double lambda_parameter, double &err);
};
#endif
--
GitLab