void playback_exact_CSpace_R3() { C2A_Model* P = NULL; C2A_Model* Q = NULL; readObjFile(P, "../data/models/CupSpoon/Cup.obj"); readObjFile(Q, "../data/models/CupSpoon/Spoon.obj"); P->ComputeRadius(); Q->ComputeRadius(); Collider3D collider(P, Q); C2A_Model* CSpace; readObjFile(CSpace, "../data/cupspoon.obj"); std::vector<ContactSpaceSampleData> contactspace_samples; std::ifstream in("space_test_3d.txt"); asciiReader(in, contactspace_samples); std::ofstream timing_file("timing_exact_R3.txt"); std::ofstream PD_file("PD_exact_R3.txt"); for(std::size_t i = 0; i < contactspace_samples.size(); ++i) { std::cout << i << std::endl; DataVector q_col(6); DataVector q(3); for(std::size_t j = 0; j < 3; ++j) q[j] = contactspace_samples[i].v[j]; for(std::size_t j = 0; j < 6; ++j) q_col[j] = contactspace_samples[i].v[j]; boost::timer t; //aTimer.Reset(); aTimer.Start(); std::pair<DataVector, double> pd_result; if(!collider.isCollide(q_col)) { pd_result.second = 0; } else { pd_result = Minkowski_Cspace_3D::Exact_PD_R3(q, CSpace); } aTimer.Stop(); PD_file << pd_result.second << " "; //timing_file << t.elapsed() << " "; timing_file << aTimer.GetTime() * 1000 << " "; timing_file.flush(); PD_file.flush(); } }
void test_svm_learner_3d_rotation_carseat() { { C2A_Model* P = NULL; C2A_Model* Q = NULL; readObjFile(P, "../data/car_seat/car.obj"); readObjFile(Q, "../data/car_seat/seat.obj"); P->ComputeRadius(); Q->ComputeRadius(); tools::Profiler::Begin("learner"); ContactSpaceSE3Euler contactspace(P, Q, 0.05 * (P->radius + Q->radius)); std::vector<ContactSpaceSampleData> contactspace_samples = contactspace.uniform_sample(500000); tools::Profiler::End("learner"); std::ofstream out("space_test_3d_rotation.txt"); asciiWriter(out, contactspace_samples); SVMLearner learner; learner.setC(50); learner.setProbability(true); learner.setScaler(contactspace.getScaler()); learner.setUseScaler(true); learner.setGamma(50); std::ofstream scaler_file("scaler_3d_rotation.txt"); scaler_file << contactspace.getScaler() << std::endl; tools::Profiler::Begin("learner"); learner.learn(contactspace_samples, contactspace.active_data_dim()); tools::Profiler::End("learner"); learner.save("model_3d_rotation.txt"); std::cout << "model saved" << std::endl; std::vector<ContactSpaceSampleData> test_samples = contactspace.uniform_sample(10000); std::cout << contactspace_samples.size() << ": " << empiricalErrorRatio(contactspace_samples, learner) << " " << empiricalErrorRatio(test_samples, learner) << std::endl; //std::cout << contactspace_samples.size() << ": " << empiricalErrorRatio(contactspace_samples, learner) << " " << errorRatioOnGrid(contactspace, learner, 5) << " "; //for(std::size_t i = 0; i < contactspace_samples.size(); ++i) // std::cout << "(" << results[i].label << "," << contactspace_samples[i].col << ")"; //std::cout << std::endl; delete P; delete Q; } }
void generateSamples() { C2A_Model* P = NULL; C2A_Model* Q = NULL; readObjFile(P, "../data/models/CupSpoon/Cup.obj"); readObjFile(Q, "../data/models/CupSpoon/Spoon.obj"); P->ComputeRadius(); Q->ComputeRadius(); ContactSpaceR3 contactspace(P, Q, 0.05 * (P->radius + Q->radius)); std::vector<ContactSpaceSampleData> contactspace_samples = contactspace.uniform_sample(10000); std::ofstream out("space_test_3d.txt"); asciiWriter(out, contactspace_samples); }
void Mesh::read(std::istream& is, const char* fileExtension) { bool loaded=false; if(!strcmp(fileExtension, "obj")) { readObjFile(is); loaded=true; } if(!strcmp(fileExtension, "off")) { readOffFile(is); loaded=true; } if(!strcmp(fileExtension, "ply")) { readPlyFile(is); loaded=true; } if(!strcmp(fileExtension, "tri")) { readTriFile(is); loaded=true; } if(!strcmp(fileExtension, "stl")) { readStlFile(is); loaded=true; } if(!loaded) HALT("can't read fileExtension '" <<fileExtension <<"'"); }
bool objLoader::importObj(t3DModel *pModel, char *strFileName) { char strMessage[255] = {0}; // This will be used for error messages if(!pModel || !strFileName) return false; m_FilePointer = fopen(strFileName, "r"); if(!m_FilePointer) { qDebug("Unable to find or open the file: %s", strFileName); return false; } readObjFile(pModel); computeNormals(pModel); fclose(m_FilePointer); return true; }
void playback_R3() { C2A_Model* P = NULL; C2A_Model* Q = NULL; readObjFile(P, "../data/models/CupSpoon/Cup.obj"); readObjFile(Q, "../data/models/CupSpoon/Spoon.obj"); P->ComputeRadius(); Q->ComputeRadius(); Collider3D collider(P, Q); std::vector<ContactSpaceSampleData> contactspace_samples; std::ifstream in("space_test_3d.txt"); asciiReader(in, contactspace_samples); ContactSpaceR3 contactspace(P, Q, 0.05 * (P->radius + Q->radius)); std::ofstream scaler_file("scaler_3d_rotation_cupspoon.txt"); scaler_file << contactspace.getScaler() << std::endl; std::vector<ContactSpaceSampleData> train_samples = contactspace.uniform_sample(100000); SVMLearner learner; learner.setDim(contactspace.active_data_dim()); learner.setC(20); learner.setScaler(contactspace.getScaler()); learner.setUseScaler(true); learner.setGamma(50); learner.learn(train_samples, contactspace.active_data_dim()); learner.save("model_R3.txt"); // flann::HierarchicalClusteringIndex<ContactSpaceSE3Euler::DistanceType>* query_index = learner.constructIndexOfSupportVectorsForQuery<ContactSpaceSE3Euler, flann::HierarchicalClusteringIndex, flann::HierarchicalClusteringIndexParams>(); std::vector<ContactSpaceSampleData> support_samples; learner.collectSupportVectors(support_samples); ExtendedModel<ContactSpaceR3, flann::Index> extended_model = constructExtendedModelForModelDecisionBoundary<ContactSpaceR3, SVMLearner, flann::Index, flann::KDTreeIndexParams>(contactspace, learner, support_samples, 0.01, 50); std::ofstream timing_file("timing_APD_R3.txt"); std::ofstream PD_file("PD_APD_R3.txt"); for(std::size_t i = 0; i < contactspace_samples.size(); ++i) { std::cout << i << std::endl; DataVector q_col(6); DataVector q(3); for(std::size_t j = 0; j < 3; ++j) q[j] = contactspace_samples[i].v[j]; for(std::size_t j = 0; j < 6; ++j) q_col[j] = contactspace_samples[i].v[j]; boost::timer t; aTimer.Reset(); aTimer.Start(); if(!collider.isCollide(q_col)) { PD_file << 0 << " "; } else { QueryResult pd_result = PD_query(learner, contactspace, extended_model.index, extended_model.samples, q); PD_file << pd_result.PD << " "; } // timing_file << t.elapsed() << " "; timing_file << aTimer.GetTime() * 1000 << " "; timing_file.flush(); PD_file.flush(); } }
void playback() { std::vector<std::vector<DataVector> > frames; std::string frame_file_name = "../data/models/CupSpoon/CupSpoon_NoJump.ani"; bool use_euler = true; frames = readAnimationFile(frame_file_name, use_euler); C2A_Model* P = NULL; C2A_Model* Q = NULL; readObjFile(P, "../data/models/CupSpoon/Cup.obj"); readObjFile(Q, "../data/models/CupSpoon/Spoon.obj"); P->ComputeRadius(); Q->ComputeRadius(); Collider3D collider(P, Q); { double Ix, Iy, Iz; inertia_weight(Q, Ix, Iy, Iz); distance_weight[0] = 1; distance_weight[1] = 1; distance_weight[2] = 1; std::cout << Ix << " " << Iy << " " << Iz << std::endl; distance_weight[3] = Ix; distance_weight[4] = Iy; distance_weight[5] = Iz; } ContactSpaceSE3Euler contactspace(P, Q, 0.05 * (P->radius + Q->radius)); std::ofstream scaler_file("scaler_3d_rotation_cupspoon.txt"); scaler_file << contactspace.getScaler() << std::endl; std::vector<ContactSpaceSampleData> contactspace_samples = contactspace.uniform_sample(500000); SVMLearner learner; learner.setDim(contactspace.active_data_dim()); learner.setC(20); learner.setScaler(contactspace.getScaler()); learner.setUseScaler(true); learner.setGamma(50); learner.learn(contactspace_samples, contactspace.active_data_dim()); learner.save("model.txt"); // flann::HierarchicalClusteringIndex<ContactSpaceSE3Euler::DistanceType>* query_index = learner.constructIndexOfSupportVectorsForQuery<ContactSpaceSE3Euler, flann::HierarchicalClusteringIndex, flann::HierarchicalClusteringIndexParams>(); std::vector<ContactSpaceSampleData> support_samples; learner.collectSupportVectors(support_samples); ExtendedModel<ContactSpaceSE3Euler, flann::HierarchicalClusteringIndex> extended_model = constructExtendedModelForModelDecisionBoundary<ContactSpaceSE3Euler, SVMLearner, flann::HierarchicalClusteringIndex, flann::HierarchicalClusteringIndexParams>(contactspace, learner, support_samples, 0.01, 50); std::ofstream timing_file("timing_APD.txt"); std::ofstream PD_file("PD_APD.txt"); std::vector<DataVector> result_qs; for(std::size_t i = 0; i < frames.size(); ++i) { DataVector q = relative3D(frames[i][0], frames[i][1]); boost::timer t; if(!collider.isCollide(q)) { result_qs.push_back(frames[i][1]); PD_file << 0 << " "; } else { // QueryResult pd_result = PD_query(learner, contactspace, query_index, q); QueryResult pd_result = PD_query(learner, contactspace, extended_model.index, extended_model.samples, q); DataVector q2 = unrelative3D(frames[i][0], pd_result.v); result_qs.push_back(q2); PD_file << pd_result.PD << " "; } timing_file << t.elapsed() << " "; timing_file.flush(); PD_file.flush(); } std::ofstream new_animation_stream("new_animation_APD.ani"); new_animation_stream << frames.size() << "f" << std::endl; for(std::size_t i = 0; i < frames.size(); ++i) { new_animation_stream << i << "f" << std::endl; double R[3][3]; double T[3]; if(frames[i][0].dim() == 6) { ConfigEuler2RotTran(R, T, frames[i][0]); } else if(frames[i][0].dim() == 7) { ConfigQuat2RotTrans(R, T, frames[i][0]); } for(int j = 0; j < 3; ++j) for(int k = 0; k < 3; ++k) { // new_animation_stream << R[j][k] << " "; new_animation_stream << R[k][j] << " "; } for(int j = 0; j < 3; ++j) new_animation_stream << T[j] << " "; new_animation_stream << std::endl; if(result_qs[i].dim() == 6) { ConfigEuler2RotTran(R, T, result_qs[i]); } else if(result_qs[i].dim() == 7) { ConfigQuat2RotTrans(R, T, result_qs[i]); } for(int j = 0; j < 3; ++j) for(int k = 0; k < 3; ++k) { // new_animation_stream << R[j][k] << " "; new_animation_stream << R[k][j] << " "; } for(int j = 0; j < 3; ++j) new_animation_stream << T[j] << " "; new_animation_stream << std::endl; } }