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;
}
}
