diff --git a/sm/csm/structprior/Constraint.cpp b/sm/csm/structprior/Constraint.cpp index 023df6fee7d19dc329faa67597650d9733fc94c9..b4fa932bb5fa7f9b1b788aeb409d4c0914769e48 100644 --- a/sm/csm/structprior/Constraint.cpp +++ b/sm/csm/structprior/Constraint.cpp @@ -16,9 +16,11 @@ void Constraint::SetType(int t) type = t; } -double Constraint::ApplyConstraint(int* indices, double* params) +Values Constraint::ApplyConstraint(int* indices, double* params) { + Values v; double err = 0; + double grd = 0; switch(type) { @@ -36,5 +38,8 @@ double Constraint::ApplyConstraint(int* indices, double* params) } - return err; + v.error = err; + v.grad = grd; + + return v; } diff --git a/sm/csm/structprior/Constraint.h b/sm/csm/structprior/Constraint.h index 232960622ddc5aeb578674e295feb4f3430fbf3d..b1edb2673a740a6f0117809700898a2d1c537b15 100644 --- a/sm/csm/structprior/Constraint.h +++ b/sm/csm/structprior/Constraint.h @@ -8,6 +8,13 @@ #include <csm/csm_all.h> +struct Values{ + + double error; + double grad; + +}; + class Constraint { @@ -22,7 +29,7 @@ protected: int type; // class methods public: - double ApplyConstraint(int* indices, double* params = NULL); + Values ApplyConstraint(int* indices, double* params = NULL); void SetType(int t); }; diff --git a/sm/csm/structprior/ConstraintManager.cpp b/sm/csm/structprior/ConstraintManager.cpp index decffbfa0df76a81251d25dadf5d581bd060b77a..aece759df9c4bd95f326ad3e915500d6cd377db3 100644 --- a/sm/csm/structprior/ConstraintManager.cpp +++ b/sm/csm/structprior/ConstraintManager.cpp @@ -34,9 +34,9 @@ void ConstraintManager::ClearConstraints() void ConstraintManager::ApplyConstraints() { //add as many types of constraints as wished - bool equal_to_either_active = false; + bool equal_to_either_active = true; bool apply_equal_to_either = false; - bool lock_diff_active = false; + bool lock_diff_active = true; bool apply_lock_diff = false; int n = laser_data->nrays; @@ -46,12 +46,14 @@ void ConstraintManager::ApplyConstraints() if (equal_to_either_active && constraint_types_to_apply[i] == EQUAL_TO_EITHER) { apply_equal_to_either = true; + equal_to_either_active = false; continue; } if (lock_diff_active && constraint_types_to_apply[i] == LOCK_DIFF) { apply_lock_diff = true; + lock_diff_active = false; continue; } @@ -69,9 +71,11 @@ void ConstraintManager::ApplyConstraints() { Constraint* c = new Constraint(EQUAL_TO_EITHER); int* ind; - ind[0] = i-j; ind[1] =i; ind[2] =i+j; - e += c->ApplyConstraint(ind); - constraints.push_back(c); + ind[0] = i-j; ind[1] =i; ind[2] =i+j; + Values v = c->ApplyConstraint(ind); + constraints.push_back(c); + e += v.error; + } } @@ -80,19 +84,22 @@ void ConstraintManager::ApplyConstraints() if(apply_lock_diff) //add lock_diff constraints { - Constraint* c = new Constraint(LOCK_DIFF); for (int i=0;i<n-1;i++) { int* ind; ind[0] = i; ind[1]=i+1; double* p; p[0] = PI/2; p[1] = lock_diff_threshold; - constraints.push_back(c); - e+= c->ApplyConstraint(ind,p); + Constraint* c1 = new Constraint(LOCK_DIFF); + Values v = c1->ApplyConstraint(ind,p); + constraints.push_back(c1); + e += v.error; p[0] = -PI/2; - e+= c->ApplyConstraint(ind,p); - constraints.push_back(c); - + Constraint* c2 = new Constraint(LOCK_DIFF); + v = c2->ApplyConstraint(ind,p); + constraints.push_back(c2); + e += v.error; + } } diff --git a/sm/csm/structprior/structprior_test.cpp b/sm/csm/structprior/structprior_test.cpp index 5c968211570939ae9b2cad1acde8f95d7b66bf14..65032507dec3013593b0ed430f3521dca6262ab7 100644 --- a/sm/csm/structprior/structprior_test.cpp +++ b/sm/csm/structprior/structprior_test.cpp @@ -10,14 +10,19 @@ int main(int argc, const char** argv) if (argc < 2) { sm_error("Provide input file's name as an argument\n"); - return 0; + return -1; } - const char* file_name = argv[0]; + const char* file_name = argv[1]; FILE* input_file = fopen(file_name, "r"); + LDP laserdata; - laserdata = ld_read_smart(input_file); + if(!(laserdata = ld_read_smart(input_file))) { + sm_error("Could not read scan.\n"); + return -1; + } + std::vector<int> cons_types; cons_types.push_back(EQUAL_TO_EITHER); @@ -30,6 +35,6 @@ int main(int argc, const char** argv) - return 1; + return 0; }