diff --git a/local_config/local-pdelapuente.cmake b/local_config/local-pdelapuente.cmake new file mode 100644 index 0000000000000000000000000000000000000000..8fed575253e7eb331c227f373c879240fadb683f --- /dev/null +++ b/local_config/local-pdelapuente.cmake @@ -0,0 +1,4 @@ + + +SET(COMPILE_HSM 0) +SET(COMPILE_STRUCTPRIOR 1) diff --git a/sm/CMakeLists.txt b/sm/CMakeLists.txt index dbcc19185f03eccfb6811cde02c42583d7597d28..5df4cdd10af0ae121b5878adc28fcde11c2191be 100644 --- a/sm/CMakeLists.txt +++ b/sm/CMakeLists.txt @@ -159,6 +159,8 @@ SET(csm_sources ./csm/orientation.c ./csm/sm_options.c ./csm/utils.c +./csm/structprior/ConstraintManager.cpp +./csm/structprior/Constraint.cpp ./lib/egsl/egsl.c ./lib/egsl/egsl_conversions.c ./lib/egsl/egsl_misc.c @@ -203,6 +205,17 @@ INSTALL(PROGRAMS hsm_test00 DESTINATION bin) ENDIF(COMPILE_HSM) +IF(COMPILE_STRUCTPRIOR) + +INCLUDE_DIRECTORIES(/sw/include) +LINK_DIRECTORIES(/sw/lib) + +ADD_EXECUTABLE(structprior ./csm/structprior/structprior_test.cpp) + TARGET_LINK_LIBRARIES(structprior csm-static) +INSTALL(PROGRAMS structprior DESTINATION bin) + +ENDIF(COMPILE_STRUCTPRIOR) + SUBDIRS(pkg-config) diff --git a/sm/csm/CMakeLists.txt b/sm/csm/CMakeLists.txt index bb377e368b1e646ce3fc45b6487cba89729eb803..860e6b18c8f34a37e77362c4a1989ec413558459 100644 --- a/sm/csm/CMakeLists.txt +++ b/sm/csm/CMakeLists.txt @@ -31,6 +31,9 @@ SET(csm_sources mbicp/sp_matrix.c hsm/hsm.c hsm/hsm_interface.c + + structprior/ConstraintManager.cpp + structprior/Constraint.cpp gpm/gpm.c ) @@ -40,7 +43,7 @@ IF(CAIRO_FOUND) ENDIF(CAIRO_FOUND) -foreach(dir . hsm mbicp icp gpm) +foreach(dir . hsm mbicp icp gpm structprior) FILE(GLOB csm_headers "${dir}/*.h") foreach(header ${csm_headers} ) INSTALL(FILES ${header} DESTINATION include/csm/${dir}) diff --git a/sm/csm/structprior/CMakeLists.txt b/sm/csm/structprior/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..3754e7bbab3e8e2891724c61f6c1a995dc77cf9f --- /dev/null +++ b/sm/csm/structprior/CMakeLists.txt @@ -0,0 +1,10 @@ +# This builds the code as itself + +cmake_minimum_required(VERSION 2.4) + +SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -pedantic -std=c99") +SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -ggdb") +SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall") + + +ADD_EXECUTABLE(structprior_test structprior_test.cpp ConstraintManager.cpp Constraint.cpp) diff --git a/sm/csm/structprior/Constraint.cpp b/sm/csm/structprior/Constraint.cpp new file mode 100644 index 0000000000000000000000000000000000000000..023df6fee7d19dc329faa67597650d9733fc94c9 --- /dev/null +++ b/sm/csm/structprior/Constraint.cpp @@ -0,0 +1,40 @@ +#include "Constraint.h" + + +Constraint::Constraint(int t) +{ + type = t; +} + +Constraint::~Constraint(void) +{ + +} + +void Constraint::SetType(int t) +{ + type = t; +} + +double Constraint::ApplyConstraint(int* indices, double* params) +{ + double err = 0; + + switch(type) + { + case EQUAL_TO_EITHER: + { + + break; + } + case LOCK_DIFF: + { + + break; + } + + + } + + return err; +} diff --git a/sm/csm/structprior/Constraint.h b/sm/csm/structprior/Constraint.h new file mode 100644 index 0000000000000000000000000000000000000000..232960622ddc5aeb578674e295feb4f3430fbf3d --- /dev/null +++ b/sm/csm/structprior/Constraint.h @@ -0,0 +1,29 @@ +#ifndef H_CONSTRAINT +#define H_CONSTRAINT + +#define EQUAL_TO_EITHER 1 +#define LOCK_DIFF 2 + +#define PI 3.14159 + +#include <csm/csm_all.h> + + +class Constraint +{ +public: +//constructors + Constraint(int t); + virtual ~Constraint(void); +//class variables + double e; + double grad; +protected: + int type; +// class methods +public: + double ApplyConstraint(int* indices, double* params = NULL); + void SetType(int t); +}; + +#endif diff --git a/sm/csm/structprior/ConstraintManager.cpp b/sm/csm/structprior/ConstraintManager.cpp new file mode 100644 index 0000000000000000000000000000000000000000..decffbfa0df76a81251d25dadf5d581bd060b77a --- /dev/null +++ b/sm/csm/structprior/ConstraintManager.cpp @@ -0,0 +1,101 @@ +#include "ConstraintManager.h" + + +ConstraintManager::ConstraintManager(LDP ld, std::vector<int> constraint_types) +{ + equal_to_either_num = 10; + lock_diff_threshold = deg2rad(5); + + laser_data = ld; + constraint_types_to_apply = constraint_types; + +} + +ConstraintManager::~ConstraintManager() +{ + for (int i=0; i<constraints.size();i++) + { + delete constraints[i]; + } + constraints.clear(); + +} + +void ConstraintManager::ClearConstraints() +{ + for (int i=0; i<constraints.size();i++) + { + delete constraints[i]; + } + constraints.clear(); + +} + +void ConstraintManager::ApplyConstraints() +{ + //add as many types of constraints as wished + bool equal_to_either_active = false; + bool apply_equal_to_either = false; + bool lock_diff_active = false; + bool apply_lock_diff = false; + + int n = laser_data->nrays; + + for (int i=0;i< constraint_types_to_apply.size();i++) + { + if (equal_to_either_active && constraint_types_to_apply[i] == EQUAL_TO_EITHER) + { + apply_equal_to_either = true; + continue; + } + + if (lock_diff_active && constraint_types_to_apply[i] == LOCK_DIFF) + { + apply_lock_diff = true; + continue; + } + + } + + if(apply_equal_to_either) //add equal_to_either constraints + { + + 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++) + { + 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); + } + + } + + } + + 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); + p[0] = -PI/2; + e+= c->ApplyConstraint(ind,p); + constraints.push_back(c); + + } + } + + + +} diff --git a/sm/csm/structprior/ConstraintManager.h b/sm/csm/structprior/ConstraintManager.h new file mode 100644 index 0000000000000000000000000000000000000000..d8746e53f54ddc1cbafd88c70a429e8dbc106d14 --- /dev/null +++ b/sm/csm/structprior/ConstraintManager.h @@ -0,0 +1,38 @@ +#ifndef H_CONSTRAINTMANAGER +#define H_CONSTRAINTMANAGER + +#include <vector> + +#include "Constraint.h" + + +class ConstraintManager +{ +public: +//constructors + ConstraintManager(LDP ld, 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 ClearConstraints(); + + + + + + +}; + +#endif + diff --git a/sm/csm/structprior/structprior_test.cpp b/sm/csm/structprior/structprior_test.cpp new file mode 100644 index 0000000000000000000000000000000000000000..5c968211570939ae9b2cad1acde8f95d7b66bf14 --- /dev/null +++ b/sm/csm/structprior/structprior_test.cpp @@ -0,0 +1,35 @@ +#include <stdio.h> +#include <csm/csm_all.h> + +#include "ConstraintManager.h" + + +int main(int argc, const char** argv) +{ + + if (argc < 2) + { + sm_error("Provide input file's name as an argument\n"); + return 0; + } + + const char* file_name = argv[0]; + FILE* input_file = fopen(file_name, "r"); + + LDP laserdata; + laserdata = ld_read_smart(input_file); + + std::vector<int> cons_types; + cons_types.push_back(EQUAL_TO_EITHER); + //cons_types.push_back(LOCK_DIFF); + + ConstraintManager cons_manager(laserdata, cons_types); + cons_manager.ApplyConstraints(); + + //minimizer.Minimize(x,cons_manager.constraints...)... + + + + return 1; + +}