diff --git a/sm/CMakeLists.txt b/sm/CMakeLists.txt index 5df4cdd10af0ae121b5878adc28fcde11c2191be..8a7b3efc91a5c8bd84b33a58a573592b34b3d1e9 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 860e6b18c8f34a37e77362c4a1989ec413558459..eceee1d3c107bb4ee7379f357b2bc14110fa6196 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 bae68a8ad615a1dc138146bfab20d3f1e6a53cf8..af5def1c2bc6195080e6fe0e9fe8b8ce3a05126e 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 9145cd4cb31e25ae51feb07db1b014dc67c5bcab..3d33ee90a4969def2ffbeb4d21acd40fb2a486d2 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 d8746e53f54ddc1cbafd88c70a429e8dbc106d14..f4ca15112d85a05db86e37ed976ef69f11d4305a 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 1d2bafe43aa3c58acd332f2013d16ae7741d8ce3..26b4f4b7048ff95faad09cfaefcf7b03830ad6c5 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...)...