From f023a41595be237b95ebb479efdb68f3e6cb8162 Mon Sep 17 00:00:00 2001
From: Paloma de la Puente <paloma.delapuente@upm.es>
Date: Thu, 22 Oct 2009 17:33:09 +0000
Subject: [PATCH] changes
---
sm/CMakeLists.txt | 1 +
sm/csm/CMakeLists.txt | 1 +
sm/csm/structprior/Constraint.cpp | 25 +++++++++++++++++++----
sm/csm/structprior/ConstraintManager.cpp | 26 ++++++++++++------------
sm/csm/structprior/ConstraintManager.h | 9 +++++---
sm/csm/structprior/structprior_test.cpp | 9 +++++---
6 files changed, 48 insertions(+), 23 deletions(-)
diff --git a/sm/CMakeLists.txt b/sm/CMakeLists.txt
index 5df4cdd..8a7b3ef 100644
--- a/sm/CMakeLists.txt
+++ b/sm/CMakeLists.txt
@@ -161,6 +161,7 @@ SET(csm_sources
./csm/utils.c
./csm/structprior/ConstraintManager.cpp
./csm/structprior/Constraint.cpp
+./csm/structprior/MeasurementsLikelihood.cpp
./lib/egsl/egsl.c
./lib/egsl/egsl_conversions.c
./lib/egsl/egsl_misc.c
diff --git a/sm/csm/CMakeLists.txt b/sm/csm/CMakeLists.txt
index 860e6b1..eceee1d 100644
--- a/sm/csm/CMakeLists.txt
+++ b/sm/csm/CMakeLists.txt
@@ -34,6 +34,7 @@ SET(csm_sources
structprior/ConstraintManager.cpp
structprior/Constraint.cpp
+ structprior/MeasurementsLikelihood.cpp
gpm/gpm.c
)
diff --git a/sm/csm/structprior/Constraint.cpp b/sm/csm/structprior/Constraint.cpp
index bae68a8..af5def1 100644
--- a/sm/csm/structprior/Constraint.cpp
+++ b/sm/csm/structprior/Constraint.cpp
@@ -20,13 +20,16 @@ Values Constraint::ApplyConstraint(double alphas[], double params[])
{
Values v;
double err = 0;
- double grd[3] = {0,0,0};
+ double* grd;
+ int size = 3;
switch(type)
{
+ //*********************************************************************************
case EQUAL_TO_EITHER:
{
- double e1 = 0.5*(alphas[1]-alphas[0]);
+ size = 3;
+ double e1 = 0.5*(alphas[1]-alphas[0])*(alphas[1]-alphas[0]);
double e2 = 0.5*(alphas[2]-alphas[1]);
if (e1 < e2)
@@ -43,17 +46,31 @@ Values Constraint::ApplyConstraint(double alphas[], double params[])
break;
}
+ //*********************************************************************************
case LOCK_DIFF:
{
+ size = 2;
+ double bias = params[0]; double threshold = params[1];
+ if (abs( alphas[1] - (alphas[0]+bias) ) < threshold)
+ {
+ e = 0.5*( alphas[1] - (alphas[0]+bias) );
+ }
+ else
+ {
+
+ }
break;
}
-
+ //*********************************************************************************
+
+ default:
+ sm_debug("Unrecognized type of constraint \n");
}
v.error = err;
- for (int i=0;i< 3;i++)
+ for (int i=0;i<size;i++)
v.grad[i] = grd[i];
return v;
diff --git a/sm/csm/structprior/ConstraintManager.cpp b/sm/csm/structprior/ConstraintManager.cpp
index 9145cd4..3d33ee9 100644
--- a/sm/csm/structprior/ConstraintManager.cpp
+++ b/sm/csm/structprior/ConstraintManager.cpp
@@ -1,12 +1,11 @@
#include "ConstraintManager.h"
-ConstraintManager::ConstraintManager(LDP ld, std::vector<int> constraint_types)
+ConstraintManager::ConstraintManager(std::vector<int> constraint_types)
{
equal_to_either_num = 10;
lock_diff_threshold = deg2rad(5);
- laser_data = ld;
constraint_types_to_apply = constraint_types;
}
@@ -31,7 +30,7 @@ void ConstraintManager::ClearConstraints()
}
-void ConstraintManager::ApplyConstraints()
+void ConstraintManager::ApplyConstraintsAlphas(std::vector<double> x_vector)
{
//add as many types of constraints as wished
bool equal_to_either_active = true;
@@ -39,8 +38,9 @@ void ConstraintManager::ApplyConstraints()
bool lock_diff_active = true;
bool apply_lock_diff = false;
- int n = laser_data->nrays;
+ int n = x_vector.size();
+ //***************************************************************************
// select the types of constraints we want to apply
for (int i=0;i< constraint_types_to_apply.size();i++)
{
@@ -59,22 +59,22 @@ void ConstraintManager::ApplyConstraints()
}
}
-
+ //***************************************************************************
if(apply_equal_to_either) //add equal_to_either constraints
{
-
+ //this all could be substituted by a function ApplyEqualToEither
for (int i=equal_to_either_num-1;i<n-equal_to_either_num;i++)
{
int mb = min (i-1, n-i-1);
mb = min (mb, equal_to_either_num-1);
- for (int j=0; j< mb;j++)
+ for (int j=0; j< mb;j++) //constraints with no consecutive points
{
Constraint* c = new Constraint(EQUAL_TO_EITHER);
double* alpha_values;
- alpha_values[0]= laser_data->alpha[i-j];
- alpha_values[1]= laser_data->alpha[i];
- alpha_values[2]= laser_data->alpha[i+j];
+ alpha_values[0]= x_vector[i-j];
+ alpha_values[1]= x_vector[i];
+ alpha_values[2]= x_vector[i+j];
Values v = c->ApplyConstraint(alpha_values);
constraints.push_back(c);
e += v.error;
@@ -84,7 +84,7 @@ void ConstraintManager::ApplyConstraints()
}
}
-
+ //***************************************************************************
if(apply_lock_diff) //add lock_diff constraints
{
for (int i=0;i<n-1;i++)
@@ -92,8 +92,8 @@ void ConstraintManager::ApplyConstraints()
int* ind;
ind[0] = i; ind[1]=i+1;
double* alpha_values;
- alpha_values[0]= laser_data->alpha[i];
- alpha_values[1]= laser_data->alpha[i+1];
+ alpha_values[0]= x_vector[i];
+ alpha_values[1]= x_vector[i+1];
double* p;
p[0] = PI/2; p[1] = lock_diff_threshold;
Constraint* c1 = new Constraint(LOCK_DIFF);
diff --git a/sm/csm/structprior/ConstraintManager.h b/sm/csm/structprior/ConstraintManager.h
index d8746e5..f4ca151 100644
--- a/sm/csm/structprior/ConstraintManager.h
+++ b/sm/csm/structprior/ConstraintManager.h
@@ -10,22 +10,25 @@ class ConstraintManager
{
public:
//constructors
- ConstraintManager(LDP ld, std::vector<int> constraint_types);
+ ConstraintManager(std::vector<int> constraint_types);
virtual ~ConstraintManager(void);
//class variables
double e;
std::vector<Constraint*> constraints;
protected:
- LDP laser_data;
std::vector<int> constraint_types_to_apply;
int equal_to_either_num;
double lock_diff_threshold;
//methods
public:
- void ApplyConstraints();
+ void ApplyConstraintsAlphas(std::vector<double> x_vector);
void ClearConstraints();
+
+protected:
+ void ApplyEqualToEither();
+ void ApplyLockDiff();
diff --git a/sm/csm/structprior/structprior_test.cpp b/sm/csm/structprior/structprior_test.cpp
index 1d2bafe..26b4f4b 100644
--- a/sm/csm/structprior/structprior_test.cpp
+++ b/sm/csm/structprior/structprior_test.cpp
@@ -2,6 +2,7 @@
#include <csm/csm_all.h>
#include "ConstraintManager.h"
+#include "MeasurementsLikelihood.h"
int main(int argc, const char** argv)
@@ -29,10 +30,12 @@ int main(int argc, const char** argv)
cons_types.push_back(EQUAL_TO_EITHER);
//cons_types.push_back(LOCK_DIFF);
- ConstraintManager cons_manager(laserdata, cons_types);
- cons_manager.ApplyConstraints();
+ ConstraintManager cons_manager(cons_types);
- //minimizer.Minimize(x,cons_manager.constraints...)...
+ MeasurementsLikelihood f(L2);
+
+
+ //minimizer.Minimize(laserdata,f,cons_manager...)...
--
GitLab