00001 /* 00002 * pca_gp_model.h 00003 * 00004 * Created on: Feb 10, 2010 00005 * Author: sturm 00006 */ 00007 00008 #ifndef PCA_GP_MODEL_H_ 00009 #define PCA_GP_MODEL_H_ 00010 00011 #include "prismatic_model.h" 00012 #include "gaussian_process/SingleGP.h" 00013 00014 namespace articulation_models { 00015 00016 class PCAGPModel: public GenericModel { 00017 public: 00018 btVector3 rigid_position; 00019 btVector3 prismatic_dir; 00020 size_t training_samples; 00021 00022 PCAGPModel(); 00023 virtual ~PCAGPModel(); 00024 00025 std::vector<gaussian_process::SingleGP*> gp; 00026 double downsample; 00027 bool initialized; 00028 00029 btTransform pose(size_t index); 00030 void storeData(bool inliersOnly); 00031 00032 // -- params 00033 void readParamsFromModel(); 00034 void writeParamsToModel(); 00035 00036 size_t getDOFs() { return 1; } 00037 00038 bool fitModel(); 00039 void buildGPs(); 00040 V_Configuration predictConfiguration(geometry_msgs::Pose pose); 00041 geometry_msgs::Pose predictPose(V_Configuration q); 00042 00043 void checkInitialized(); 00044 void projectPoseToConfiguration(); 00045 void projectConfigurationToPose(); 00046 void projectConfigurationToJacobian(); 00047 bool evaluateModel(); 00048 bool normalizeParameters(); 00049 00050 }; 00051 00052 } 00053 00054 #endif /* PCA_GP_MODEL_H_ */