void shot_detector::processImage()
{
std::cerr << "Processing" << std::endl;
std::string file = "/home/niko/projects/apc/catkin/src/apc_ros/apc_object_detection/visual_hull_refined_smoothed.obj";
loadModel(model, file);
pcl::io::loadPCDFile("/home/niko/projects/apc/catkin/src/apc_ros/apc_object_detection/niko_file.pcd", *scene);
//Downsample the model and the scene so they have rougly the same resolution
pcl::PointCloud<PointType>::Ptr scene_filter (new pcl::PointCloud<PointType> ());
pcl_functions::voxelFilter(scene, scene_filter, voxel_sample_);
scene = scene_filter;
pcl::PointCloud<PointType>::Ptr model_filter (new pcl::PointCloud<PointType> ());
pcl_functions::voxelFilter(model, model_filter, voxel_sample_);
model = model_filter;
// Randomly select a couple of keypoints so we don't calculte descriptors for everything
sampleKeypoints(model, model_keypoints, model_ss_);
sampleKeypoints(scene, scene_keypoints, scene_ss_);
//Calculate the Normals
calcNormals(model, model_normals);
calcNormals(scene, scene_normals);
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
ourcvfh.setInputCloud(scene);
ourcvfh.setInputNormals(scene_normals);
ourcvfh.setSearchMethod(kdtree);
ourcvfh.setEPSAngleThreshold(5.0 / 180.0 * M_PI); // 5 degrees.
ourcvfh.setCurvatureThreshold(1.0);
ourcvfh.setNormalizeBins(false);
// Set the minimum axis ratio between the SGURF axes. At the disambiguation phase,
// this will decide if additional Reference Frames need to be created, if ambiguous.
ourcvfh.setAxisRatio(0.8);
ourcvfh.compute(vfh_scene_descriptors);
ourcvfh.setInputCloud(model);
ourcvfh.setInputNormals(model_normals);
ourcvfh.compute(vfh_model_descriptors);
//Calculate the shot descriptors at each keypoint in the scene
calcSHOTDescriptors(model, model_keypoints, model_normals, model_descriptors);
calcSHOTDescriptors(scene, scene_keypoints, scene_normals, scene_descriptors);
// Compare descriptors and try to find correspondences
//ransac(rototranslations,model,scene);
//refinePose(rototranslations,model,scene);
compare(model_descriptors, scene_descriptors);
groupCorrespondences();
visualizeCorrespondences();
visualizeICP();
/*Eigen::Matrix4f pose;
if(model_scene_corrs->size ()!=0){
groupCorrespondences();
ransac(rototranslations,model,scene);
pose=refinePose(rototranslations,model,scene);
}*/
}
void MeshPlane::init( uint flags )
{
fl = flags;
for ( int i = 0; i < 4; i++ )
{
glm::vec3 pos(planeVerts[i * 3 + 0],
planeVerts[i * 3 + 1],
planeVerts[i * 3 + 2]);
glm::vec3 color(1,0,1);
glm::vec3 normal(0,0,0);
glm::vec2 uv(planeUV[i * 2 + 0],
planeUV[i * 2 + 1]);
addVertex(flags, pos, color, normal, uv);
}
for ( int i = 0; i < 2; i++ )
{
addIndex(planeInds[i * 3 + 0],
planeInds[i * 3 + 1],
planeInds[i * 3 + 2]);
}
stepSize = 0;
if ( flags & BUF_POS )
stepSize += 3;
if ( flags & BUF_COLOR )
stepSize += 3;
if ( flags & BUF_NORMAL )
stepSize += 3;
if ( flags & BUF_UV )
stepSize += 2;
if ( flags & BUF_NORMAL )
calcNormals();
}
void Mesh::setVertices(std::vector<Vertex>& vertices, std::vector<int>& indices, bool needCalcNormals)
{
if (needCalcNormals)
calcNormals(vertices, indices);
// std::vector<float> v;
// for (int i = 0; i < vertices.size(); ++i) {
// v.push_back(vertices[i].getPosition().x);
// v.push_back(vertices[i].getPosition().y);
// v.push_back(vertices[i].getPosition().z);
// v.push_back(vertices[i].getTexCoord().x);
// v.push_back(vertices[i].getTexCoord().y);
// v.push_back(vertices[i].getNormal().x);
// v.push_back(vertices[i].getNormal().y);
// v.push_back(vertices[i].getNormal().z);
// }
m_data = new MeshData(indices.size());
m_loadedMeshes[m_filePath] = m_data;
glBindBuffer(GL_ARRAY_BUFFER, m_data->getVertexBufferID());
glBufferData(GL_ARRAY_BUFFER, vertices.size() * sizeof(vertices[0]), &(vertices[0]), GL_STATIC_DRAW);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->getIndicesBufferID());
glBufferData(GL_ELEMENT_ARRAY_BUFFER, indices.size() * sizeof(int), &(indices[0]), GL_STATIC_DRAW);
}
void TerrainSim::simulate()
{
// Too much spew for regular use.
// LOGI("Simulate tile at (%f,%f)", m_translation.x, m_translation.y);
// The dirty flag is set *asynchronously* in our setParams method, in response to slider touches. We need
// to complete one complete iteration of the loop without the params being modified by the GUI. There *might*
// be a race condition on setting/reading m_dirty but it's not critical. I can live with that.
while (m_dirty)
{
m_dirty = false;
// Note that we deliberately allow the parameters to be asynchronously set while executing these for loops.
// We maintain GUI interactivity with the compromise of visual discontinuities while the parameters are
// being altered (which is acceptable). A final outer while loop execution with m_dirty == false will
// fix the discontinuities after the params stop changing.
for(int32_t j=0; j<m_height; j++)
for(int32_t i=0; i<m_width; i++)
m_u.get(i, j) = computeTerrain(i,j);
// m_dirty may have been asynchronously set true by now. If so, do another iteration round the while loop.
}
calcNormals();
}
/*!
* \brief shot_detector::processCloud The service function. It process the clouds with shot and returns the pose of the object
* \param req
* \param res
* \return
*/
bool shot_detector::processCloud(apc_msgs::shot_detector_srv::Request &req, apc_msgs::shot_detector_srv::Response &res)
{
pcl_functions::convertMsg(req.targetcloud, model);
//loadModel(*model,"/home/niko/projects/apc/catkin/src/apc_ros/apc_object_detection/optimized_poisson_textured_mesh.ply");
pcl_functions::convertMsg(req.pointcloud, scene);
//pcl::io::loadPCDFile("/home/niko/projects/apc/catkin/src/apc_ros/apc_object_detection/niko_file.pcd",*scene);
std::cerr << "Originally positions" << std::endl;
std::cerr << scene->points[1].x << std::endl;
std::cerr << scene->points[1].y << std::endl;
std::cerr << scene->points[1].z << std::endl;
//Downsample the model and the scene so they have rougly the same resolution
pcl::PointCloud<PointType>::Ptr scene_filter (new pcl::PointCloud<PointType> ());
pcl_functions::voxelFilter(scene, scene_filter, voxel_sample_);
scene = scene_filter;
pcl::PointCloud<PointType>::Ptr model_filter (new pcl::PointCloud<PointType> ());
pcl_functions::voxelFilter(model, model_filter, voxel_sample_);
model = model_filter;
// Randomly select a couple of keypoints so we don't calculte descriptors for everything
sampleKeypoints(model, model_keypoints, model_ss_);
sampleKeypoints(scene, scene_keypoints, scene_ss_);
//Calculate the Normals
calcNormals(model, model_normals);
calcNormals(scene, scene_normals);
//Calculate the shot descriptors at each keypoint in the scene
calcSHOTDescriptors(model, model_keypoints, model_normals, model_descriptors);
calcSHOTDescriptors(scene, scene_keypoints, scene_normals, scene_descriptors);
ransac(rototranslations, model, scene);
// Compare descriptors and try to find correspondences
// compare(model_descriptors,scene_descriptors);
Eigen::Matrix4f pose;
// if(model_scene_corrs->size ()!=0){
//groupCorrespondences();
pose = refinePose(rototranslations, model, scene);
//}
std::cerr << pose << std::endl;
if(pose == Eigen::Matrix4f::Identity())
return false;
Eigen::Matrix4d md(pose.cast<double>());
Eigen::Affine3d affine(md);
geometry_msgs::Pose transform;
tf::poseEigenToMsg(affine, transform);
res.pose = transform;
return true;
}
void Mesh::addVertices(std::vector<Vertex>& vertices, std::vector<unsigned int>& indices, bool needCalcNormals)
{
mSize = indices.size();
if (needCalcNormals)
calcNormals(vertices.data(), vertices.size(), indices.data(), indices.size());
glBindBuffer(GL_ARRAY_BUFFER, mVbo);
glBufferData(GL_ARRAY_BUFFER, vertices.size() * sizeof(vertices[0]), vertices.data(), GL_STATIC_DRAW);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, mIbo);
glBufferData(GL_ELEMENT_ARRAY_BUFFER, indices.size() * sizeof(indices[0]), indices.data(), GL_STATIC_DRAW);
}
void Mesh::addVertices(Vertex* vertices, unsigned int vertSize, unsigned int* indices, unsigned int indexSize, bool needCalcNormals)
{
mSize = indexSize;
if(needCalcNormals)
calcNormals(vertices, vertSize, indices, indexSize);
glBindBuffer(GL_ARRAY_BUFFER, mVbo);
glBufferData(GL_ARRAY_BUFFER, vertSize * sizeof(Vertex), vertices, GL_STATIC_DRAW);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, mIbo);
glBufferData(GL_ELEMENT_ARRAY_BUFFER, indexSize * sizeof(int), indices, GL_STATIC_DRAW);
}
Ball::Ball(float radius, unsigned int pointCount) : shape{radius, pointCount} {
shape.setFillColor({255, 0, 0});
shape.setOrigin({radius, radius});
origin = shape.getOrigin();
friction = 1.0f;
position = {0.0f, 0.0f};
velocity = {0.0f, 0.0f};
for (int i=0; i < shape.getPointCount(); ++i) {
points.emplace_back(shape.getPoint(i));
}
calcNormals();
}
void MeshCube::init( uint flags )
{
fl = flags;
for ( int i = 0; i < 8*3; i++ )
{
uint si = i % 8;
uint face = i / 8;
uint normalface = i / 4;
glm::vec3 pos(cubeVerts[si * 3 + 0],
cubeVerts[si * 3 + 1],
cubeVerts[si * 3 + 2]);
glm::vec3 color(1, 0, 1);
glm::vec3 normal(0, 0, 0);
glm::vec2 uv(0,0);
addVertex(flags, pos, color, normal, uv);
}
for ( int i = 0; i < 12; i++ )
{
uint face = i / 4;
addIndex(cubeInds[i * 3 + 0] + face * 8,
cubeInds[i * 3 + 1] + face * 8,
cubeInds[i * 3 + 2] + face * 8);
}
stepSize = 0;
if ( flags & BUF_POS )
stepSize += 3;
if ( flags & BUF_COLOR )
stepSize += 3;
if ( flags & BUF_NORMAL )
stepSize += 3;
if ( flags & BUF_UV )
stepSize += 2;
if ( flags & BUF_NORMAL )
calcNormals( );
}
void init(TRIANGLE * t, int n)
{
for(int i = 0; i < n; ++i)
{
vec3f v1(t[i].p[0].x,t[i].p[0].y,t[i].p[0].z);
vec3f v2(t[i].p[1].x,t[i].p[1].y,t[i].p[1].z);
vec3f v3(t[i].p[2].x,t[i].p[2].y,t[i].p[2].z);
Tri tri;
tri.i = insertPoint(v1);
tri.j = insertPoint(v2);
tri.k = insertPoint(v3);
tri.norm = vec3f(t[i].norm.x,t[i].norm.y, t[i].norm.z);
tris.push_back(tri);
}
std::cout << "Inserted " << vecs.size() << " unique points from " << n*3 << "original points\n";
calcNormals();
}
//------------------------------------------------------------------------------
void
createOsdMesh( const char * shape, int level, int kernel, Scheme scheme=kCatmark ) {
// generate Hbr representation from "obj" description
OpenSubdiv::OsdHbrMesh * hmesh = simpleHbr<OpenSubdiv::OsdVertex>(shape, scheme, g_orgPositions);
g_normals.resize(g_orgPositions.size(),0.0f);
g_positions.resize(g_orgPositions.size(),0.0f);
calcNormals( hmesh, g_orgPositions, g_normals );
// save coarse topology (used for coarse mesh drawing)
g_coarseEdges.clear();
g_coarseEdgeSharpness.clear();
g_coarseVertexSharpness.clear();
int nf = hmesh->GetNumFaces();
for(int i=0; i<nf; ++i) {
OpenSubdiv::OsdHbrFace *face = hmesh->GetFace(i);
int nv = face->GetNumVertices();
for(int j=0; j<nv; ++j) {
g_coarseEdges.push_back(face->GetVertex(j)->GetID());
g_coarseEdges.push_back(face->GetVertex((j+1)%nv)->GetID());
g_coarseEdgeSharpness.push_back(face->GetEdge(j)->GetSharpness());
}
}
int nv = hmesh->GetNumVertices();
for(int i=0; i<nv; ++i) {
g_coarseVertexSharpness.push_back(hmesh->GetVertex(i)->GetSharpness());
}
// generate Osd mesh from Hbr mesh
if (g_osdmesh) delete g_osdmesh;
g_osdmesh = new OpenSubdiv::OsdMesh();
g_osdmesh->Create(hmesh, level, kernel);
if (g_vertexBuffer) {
delete g_vertexBuffer;
g_vertexBuffer = NULL;
}
// Hbr mesh can be deleted
delete hmesh;
// update element array buffer
if (g_elementArrayBuffer) delete g_elementArrayBuffer;
g_elementArrayBuffer = g_osdmesh->CreateElementArrayBuffer(level);
g_scheme = scheme;
// compute model bounding
float min[3] = { FLT_MAX, FLT_MAX, FLT_MAX};
float max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
for (size_t i=0; i <g_orgPositions.size()/3; ++i) {
for(int j=0; j<3; ++j) {
float v = g_orgPositions[i*3+j];
min[j] = std::min(min[j], v);
max[j] = std::max(max[j], v);
}
}
for (int j=0; j<3; ++j) {
g_center[j] = (min[j] + max[j]) * 0.5f;
g_size += (max[j]-min[j])*(max[j]-min[j]);
}
g_size = sqrtf(g_size);
updateGeom();
}
obvious::Matrix RandomNormalMatching::match(obvious::Matrix* M,
const bool* maskM,
obvious::Matrix* NM,
obvious::Matrix* S,
const bool* maskS,
double phiMax,
const double transMax,
const double resolution)
{
obvious::Matrix TBest(3, 3);
TBest.setIdentity();
const int pointsInM = M->getRows();
const int pointsInS = S->getRows();
if(pointsInM != pointsInS)
{
LOGMSG(DBG_ERROR, "Model and scene need to be of same size, size of M: " << pointsInM << ", size of S: " << pointsInS);
return TBest;
}
if(pointsInM < 3)
{
LOGMSG(DBG_ERROR, "Model and scene contain too less points, size of M: " << pointsInM << ", size of S: " << pointsInS);
return TBest;
}
// ----------------- Model ------------------
obvious::Matrix* NMpca = new Matrix(pointsInM, 2); // Normals for model
double* phiM = new double[pointsInM]; // Orientation of model points
bool* maskMpca = new bool[pointsInM]; // Validity mask of model points
memcpy(maskMpca, maskM, pointsInM*sizeof(bool));
if(NM)
{
calcPhi(NM, maskM, phiM);
}
else // if normals are not supplied
{
calcNormals(M, NMpca, maskM, maskMpca, _pcaSearchRange/2);
calcPhi(NMpca, maskMpca, phiM);
}
vector<unsigned int> idxMValid = extractSamples(M, maskMpca, _pcaSearchRange/2);
#if USEKNN
initKDTree(M, idxMValid);
#endif
// -------------------------------------------
// ----------------- Scene -------------------
obvious::Matrix* NSpca = new Matrix(pointsInS, 2); // Normals for scene
double* phiS = new double[pointsInS]; // Orientation of scene points
bool* maskSpca = new bool[pointsInS]; // Validity mask of scene points
memcpy(maskSpca, maskS, pointsInS*sizeof(bool));
// Determine number of valid samples in local scene neighborhood
// only from these points a valid orientation is computable
unsigned int validPoints = 0;
for(int i=0; i<pointsInS; i++)
if(maskSpca[i]) validPoints++;
// Probability of point masking
double probability = 180.0/(double)validPoints;
if(probability<0.99)
subsampleMask(maskSpca, pointsInS, probability);
calcNormals(S, NSpca, maskS, maskSpca, _pcaSearchRange/2);
calcPhi(NSpca, maskSpca, phiS);
vector<unsigned int> idxSValid = extractSamples(S, maskSpca, _pcaSearchRange/2);
// -------------------------------------------
// --------------- Control set ---------------
vector<unsigned int> idxControl; //represents the indices of points used for Control in S.
obvious::Matrix* Control = pickControlSet(S, idxSValid, idxControl);
obvious::Matrix* NControl = new obvious::Matrix(idxControl.size(), 2);
for(unsigned int i=0; i<Control->getCols(); i++)
{
(*NControl)(i, 0) = (*NSpca)(idxControl[i], 0);
(*NControl)(i, 1) = (*NSpca)(idxControl[i], 1);
}
unsigned int pointsInC = Control->getCols();
unsigned int cntMatchThresh = pointsInC / 3; // TODO: Determine meaningful parameter
double* phiControl = new double[pointsInC]; // Orientation of control points
calcPhi(NControl, NULL, phiControl);
// -------------------------------------------//
// Determine frustum, i.e., direction of leftmost and rightmost model point
double thetaBoundMin = atan2((*M)(idxMValid.front(),1), (*M)(idxMValid.front(),0)); // real bounding
double thetaBoundMax = atan2((*M)(idxMValid.back(),1), (*M)(idxMValid.back(),0)); // real bounding
LOGMSG(DBG_DEBUG, "Valid points in scene: " << idxSValid.size() << ", valid points in model: " << idxMValid.size() << ", Control set: " << Control->getCols());
LOGMSG(DBG_DEBUG, "Model phi min:: " << rad2deg(thetaBoundMin) << ", Model phi max: " << rad2deg(thetaBoundMax));
if(idxSValid.size() < 3)
{
LOGMSG(DBG_ERROR, "Too less valid points in scene, matchable size: " << idxSValid.size());
return TBest;
}
if(idxMValid.size() < 3)
{
LOGMSG(DBG_ERROR, "Too less valid points in model, matchable size: " << idxMValid.size());
return TBest;
}
// Check for maximum meaningful trials
unsigned int trials = _trials;
if(idxMValid.size()<_trials)
trials = idxMValid.size();
if(_trace)
{
_trace->reset();
_trace->setModel(M, idxMValid);
_trace->setScene(S, idxSValid);
}
// Calculate search "radius", i.e., maximum difference in polar indices because of rotation
phiMax = min(phiMax, M_PI * 0.5);
int span;
if(resolution > 1e-6)
{
span = floor(phiMax / resolution);
if(span > (int)pointsInM) span = (int)pointsInM;
}
else
{
LOGMSG(DBG_ERROR, "Resolution not properly set: resolution = " << resolution);
return TBest;
}
srand (time(NULL));
double bestRatio = 0.0;
unsigned int bestCnt = 0;
double bestErr = 1e12;
#ifndef DEBUG
// trace is only possible for single threaded execution
if(_trace)
{
omp_set_num_threads(1);
LOGMSG(DBG_WARN, "Configured single-threaded execution due to application of trace module");
}
#endif
//Timer t;
//t.start();
vector<unsigned int> idxTrials = idxMValid;
#pragma omp parallel
{
bool* maskControl = new bool[pointsInC];
double* thetaControl = new double[pointsInC];
#pragma omp for
for(unsigned int trial = 0; trial < trials; trial++)
{
int idx;
#pragma omp critical
{
const int randIdx = rand() % (idxTrials.size());
idx = idxTrials[randIdx];
// remove chosen element to avoid picking same index a second time
idxTrials.erase(idxTrials.begin() + randIdx);
}
// leftmost scene point
const int iMin = max(idx-span, _pcaSearchRange/2);
// rightmost scene point
const int iMax = min(idx+span, pointsInS-_pcaSearchRange/2);
for(int i=iMin; i<iMax; i++)
{
if(maskSpca[i])
{
double phi = phiM[idx] - phiS[i];
if(phi>M_PI) phi -= 2.0*M_PI;
else if(phi<-M_PI) phi += 2.0*M_PI;
if(fabs(phi) < phiMax)
{
obvious::Matrix T = obvious::MatrixFactory::TransformationMatrix33(phi, 0, 0);
// Calculate translation
const double sx = (*S)(i,0);
const double sy = (*S)(i,1);
T(0, 2) = (*M)(idx,0) - (T(0, 0) * sx + T(0, 1) * sy);
T(1, 2) = (*M)(idx,1) - (T(1, 0) * sx + T(1, 1) * sy);
// Transform control set
obvious::Matrix STemp = T * (*Control);
unsigned int pointsInControl = STemp.getCols();
// Determine number of control points in field of view
unsigned int maxCntMatch = 0;
for(unsigned int j=0; j<pointsInControl; j++)
{
thetaControl[j] = atan2(STemp(1, j), STemp(0, j));
if(thetaControl[j]>thetaBoundMax || thetaControl[j]<thetaBoundMin)
{
maskControl[j] = false;
}
else
{
maskControl[j] = true;
maxCntMatch++;
}
}
// Determine how many nearest neighbors (model <-> scene) are close enough
unsigned int cntMatch = 0;
flann::Matrix<int> indices(new int[1], 1, 1);
flann::Matrix<double> dists(new double[1], 1, 1);
double errSum = 0;
//double scoreSum = 0.0;
for(unsigned int s = 0; s < pointsInControl; s++)
{
// clip points outside of model frustum
if(maskControl[s])
{
#if USEKNN
// find nearest neighbor of control point
double q[2];
q[0] = STemp(0, s);
q[1] = STemp(1, s);
flann::Matrix<double> query(q, 1, 2);
flann::SearchParams p(-1, 0.0);
_index->knnSearch(query, indices, dists, 1, p);
const int idxQuery = idxMValid[indices[0][0]];
double distConsensus = dists[0][0];
#else
// speeded-up NN search through back projection
const int idxQuery = round((thetaControl[s]-thetaMin) / resolution);
if(!maskM[idxQuery]) continue;
double distX = (*M)(idxQuery, 0) - STemp(0, s);
double distY = (*M)(idxQuery, 1) - STemp(1, s);
double distConsensus = distX*distX + distY*distY;
#endif
#if NORMALCONSENSUS
// Experimental idea: rate matching results additionally with normal consensus
// consensus score is in range [0, 1] -> perfect match = 0
double normalConsensus = (1.0 - cos(phiM[idxQuery] - phiControl[s] - phi))/2.0;
// Normalized error (weight distance and normal consensus)
double err = distConsensus*_scaleDistance + normalConsensus*_scaleOrientation;
#else
double err = distConsensus*_scaleDistance;
#endif
errSum += err;
if(err<1.0)
cntMatch++;
}
}
delete[] indices.ptr();
delete[] dists.ptr();
if(cntMatch <= cntMatchThresh)
continue;
// Experimental rating
double ratio = (double)cntMatch / (double) maxCntMatch;
#pragma omp critical
{
// Rating from Markus Kuehn
double equalThres = 1e-5;
bool rateCondition = ((ratio-bestRatio) > equalThres) && (cntMatch > bestCnt);
bool similarityCondition = fabs( (ratio-bestRatio) < equalThres ) && (cntMatch == bestCnt) && errSum < bestErr;
bool goodMatch = rateCondition ||similarityCondition;
if(goodMatch)
{
bestRatio = ratio;
bestCnt = cntMatch;
bestErr = errSum;
TBest = T;
}
}
if(_trace)
{
//trace is only possible for single threaded execution
vector<unsigned int> idxM;
idxM.push_back(idx);
vector<unsigned int> idxS;
idxS.push_back(i);
_trace->addAssignment(M, idxM, S, idxS, &STemp, errSum, trial);
}
}// if phiMax
} // if maskS
} // for i
} // for trials
delete [] maskControl;
} // OMP
//cout << "elapsed: " << t.elapsed() << endl;
//t.reset();
delete NMpca;
delete NSpca;
delete [] phiM;
delete [] phiS;
delete [] phiControl;
delete [] maskMpca;
delete [] maskSpca;
delete Control;
return TBest;
}
void Model::calcNormals(bool smooth) {
calcNormals(0, mVertices.size(), smooth);
}
//------------------------------------------------------------------------------
void
createOsdMesh(int level, int kernel) {
Ptex::String ptexError;
PtexTexture *ptexColor = PtexTexture::open(g_ptexColorFile, ptexError, true);
// generate Hbr representation from ptex
OpenSubdiv::OsdHbrMesh * hmesh = createPTexGeo<OpenSubdiv::OsdVertex>(ptexColor);
if(hmesh == NULL) return;
g_normals.resize(g_positions.size(),0.0f);
calcNormals( hmesh, g_positions, g_normals );
// generate Osd mesh from Hbr mesh
if (g_osdmesh) delete g_osdmesh;
g_osdmesh = new OpenSubdiv::OsdMesh();
g_osdmesh->Create(hmesh, level, kernel);
if (g_vertexBuffer) {
delete g_vertexBuffer;
g_vertexBuffer = NULL;
}
// Hbr mesh can be deleted
delete hmesh;
// generate oOsdPTexture
if (g_osdPTexDisplacement) delete g_osdPTexDisplacement;
if (g_osdPTexOcclusion) delete g_osdPTexOcclusion;
g_osdPTexDisplacement = NULL;
g_osdPTexOcclusion = NULL;
OpenSubdiv::OsdPTexture::SetGutterWidth(g_gutterWidth);
OpenSubdiv::OsdPTexture::SetPageMargin(g_gutterWidth*8);
OpenSubdiv::OsdPTexture::SetGutterDebug(g_gutterDebug);
if (g_osdPTexImage) delete g_osdPTexImage;
g_osdPTexImage = OpenSubdiv::OsdPTexture::Create(ptexColor, 0 /*targetmemory*/);
ptexColor->release();
if (g_ptexDisplacementFile) {
PtexTexture *ptexDisplacement = PtexTexture::open(g_ptexDisplacementFile, ptexError, true);
g_osdPTexDisplacement = OpenSubdiv::OsdPTexture::Create(ptexDisplacement, 0);
ptexDisplacement->release();
}
if (g_ptexOcclusionFile) {
PtexTexture *ptexOcclusion = PtexTexture::open(g_ptexOcclusionFile, ptexError, true);
g_osdPTexOcclusion = OpenSubdiv::OsdPTexture::Create(ptexOcclusion, 0);
ptexOcclusion->release();
}
// create element array buffer
if (g_elementArrayBuffer) delete g_elementArrayBuffer;
g_elementArrayBuffer = g_osdmesh->CreateElementArrayBuffer(level);
// create ptex coordinates buffer
if (g_ptexCoordinatesTextureBuffer) delete g_ptexCoordinatesTextureBuffer;
g_ptexCoordinatesTextureBuffer = g_osdmesh->CreatePtexCoordinatesTextureBuffer(level);
updateGeom();
linkProgram();
linkDebugProgram();
}
//------------------------------------------------------------------------------
void
createOsdMesh(int level, int kernel) {
Ptex::String ptexError;
PtexTexture *ptexColor = PtexTexture::open(g_ptexColorFile, ptexError, true);
// generate Hbr representation from ptex
OpenSubdiv::OsdHbrMesh * hmesh = createPTexGeo<OpenSubdiv::OsdVertex>(ptexColor);
if(hmesh == NULL) return;
g_normals.resize(g_positions.size(),0.0f);
calcNormals( hmesh, g_positions, g_normals );
// generate Osd mesh from Hbr mesh
if (g_osdmesh) delete g_osdmesh;
g_osdmesh = new OpenSubdiv::OsdMesh();
g_osdmesh->Create(hmesh, level, kernel);
if (g_vertexBuffer) {
delete g_vertexBuffer;
g_vertexBuffer = NULL;
}
// Hbr mesh can be deleted
delete hmesh;
// update element array buffer
const std::vector<int> &indices = g_osdmesh->GetFarMesh()->GetFaceVertices(level);
// generate oOsdPTexture
if (g_osdPTexDisplacement) delete g_osdPTexDisplacement;
if (g_osdPTexOcclusion) delete g_osdPTexOcclusion;
g_osdPTexDisplacement = NULL;
g_osdPTexOcclusion = NULL;
OpenSubdiv::OsdPTexture::SetGutterWidth(g_gutterWidth);
OpenSubdiv::OsdPTexture::SetPageMargin(g_gutterWidth*8);
OpenSubdiv::OsdPTexture::SetGutterDebug(g_gutterDebug);
if (g_osdPTexImage) delete g_osdPTexImage;
g_osdPTexImage = OpenSubdiv::OsdPTexture::Create(ptexColor, 0 /*targetmemory*/);
ptexColor->release();
if (g_ptexDisplacementFile) {
PtexTexture *ptexDisplacement = PtexTexture::open(g_ptexDisplacementFile, ptexError, true);
g_osdPTexDisplacement = OpenSubdiv::OsdPTexture::Create(ptexDisplacement, 0);
ptexDisplacement->release();
}
if (g_ptexOcclusionFile) {
PtexTexture *ptexOcclusion = PtexTexture::open(g_ptexOcclusionFile, ptexError, true);
g_osdPTexOcclusion = OpenSubdiv::OsdPTexture::Create(ptexOcclusion, 0);
ptexOcclusion->release();
}
// bind index buffer
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, g_indexBuffer);
g_numIndices = (int)indices.size();
glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(unsigned int)*g_numIndices, &(indices[0]), GL_STATIC_DRAW);
updateGeom();
linkProgram();
linkDebugProgram();
}
bool C3ds::load(std::string id, std::string file, float scale) {
this->id = id;
std::ifstream in;
ChunkInfo info;
int Offset, SubChunkSize, Value, MatDex, MeshDex, Loop, LOff;
short Val;
float fVal;
bool CopyVal = false;
MatDex = -1;
MeshDex = -1;
in.open(file.c_str(), std::ios::binary);
if (in.fail())
return false;
in.seekg(0, std::ios::end);
fileSize = in.tellg();
in.seekg(0, std::ios::beg);
if (data != NULL)
delete data;
data = new unsigned char[fileSize];
if (data == NULL) {
in.close();
return false;
}
in.read((char*) data, fileSize);
in.close();
Offset = 0;
info = getChunkInfo(Offset);
if (info.ID != 0x4D4D)
return false;
if (info.Size != fileSize)
return false;
countParts(fileSize);
std::cout << "-- 3DS File has " << nMesh << " Meshes, only loading first --"
<< std::endl;
Offset = 6;
while (Offset < fileSize) {
info = getChunkInfo(Offset);
switch (info.ID) {
case 0x0002: // Version
memcpy(&Value, &data[Offset + 6], 4);
//printf("Chunk 0002 (Version) - %d\nOffset %d\n",Value,info.Size);
Offset += info.Size;
break;
case 0x0011: // RGB1
//printf("Chunk 0011 (RGB1) %d %d %d\nOffset %d\n",data[Offset+6],data[Offset+7],data[Offset+8],info.Size);
if (CopyVal) {
color = (glRGBA) {data[Offset+6],data[Offset+7],data[Offset+8]};
CopyVal = false;
}
Offset += info.Size;
break;
case 0x0012: // RGB2
//printf("Chunk 0012 (RGB2) %d %d %d\nOffset %d\n",data[Offset+6],data[Offset+7],data[Offset+8],info.Size);
Offset += info.Size;
break;
case 0x0030: // Quantity value for parent chunks
memcpy(&Val, &data[Offset + 6], 2);
//printf("Chunk 0030 (Qty Value) %d\nOffset %d\n",Val,info.Size);
Offset += info.Size;
break;
case 0x0100: // Config (Ignore)
//printf("Chunk 0100 (Config)\nOffset %d\n",info.Size);
Offset += info.Size;
break;
case 0x3D3D: // Start of Obj
//printf("Chunk 3D3D (Start of Obj)\nOffset %d\n",info.Size);
SubChunkSize = info.Size + Offset; // Set end limit for subchunk
Offset += 6;
break;
case 0x3D3E: // Editor config (Ignore)
//printf("Chunk 3D3E (Editor Config)\nOffset %d\n",info.Size);
Offset += info.Size;
break;
case 0x4000: // Start of Mesh
//printf("Chunk 4000 (Start of Mesh) - %s\nOffset %d\n",&data[Offset+6],info.Size);
Offset += 6;
while (data[Offset] != 0) // Seek end of string
Offset++;
Offset++; // One more to move past the NULL
MeshDex++;
if (MeshDex == 1)
Offset = fileSize;
break;
case 0x4100: // Mesh data
//printf("Chunk 4100 (Mesh Data)\nOffset %d\n",info.Size);
Offset += 6;
break;
case 0x4110: // Vertex List
memcpy(&Val, &data[Offset + 6], 2);
//printf("Chunk 4110 (Vertex List) %d Vertices\nOffset %d\n",Val,info.Size);
numVerts = Val;
vertex = new FVector[Val + 1];
sphereRadius = 0;
for (Loop = 0, LOff = Offset + 8; Loop != Val; ++Loop, LOff += 12) {
memcpy(&(vertex[Loop].x), &data[LOff], 4);
memcpy(&(vertex[Loop].y), &data[LOff + 4], 4);
memcpy(&(vertex[Loop].z), &data[LOff + 8], 4);
if (vertex[Loop].magnitude() > sphereRadius)
sphereRadius = vertex[Loop].magnitude();
}
Offset += info.Size;
break;
case 0x4111: // Vertex Options
//printf("Chunk 4111 (Vertex Options)\nOffset %d\n",info.Size);
Offset += info.Size;
break;
case 0x4120: // Face List
memcpy(&Val, &data[Offset + 6], 2);
//printf("Chunk 4120 (Face List) %d polys\nOffset %d\n",Val,info.Size);
numFaces = Val;
face = new Face[Val + 1];
for (Loop = 0, LOff = Offset + 8; Loop != Val; ++Loop, LOff += 8) {
memcpy(&(face[Loop].p1), &data[LOff], 2);
memcpy(&(face[Loop].p2), &data[LOff + 2], 2);
memcpy(&(face[Loop].p3), &data[LOff + 4], 2);
}
Offset += info.Size;
break;
case 0x4130: // Material Desc
Offset += info.Size;
break;
case 0x4140: // UV Map List
memcpy(&Val, &data[Offset + 6], 2);
//printf("Chunk 4120 (UV Map List)\nOffset %d\n",info.Size);
numTexCoords = Val;
texcoord = new UVTexCoord[Val + 1];
for (Loop = 0, LOff = Offset + 8; Loop != Val; ++Loop, LOff += 8) {
memcpy(&(texcoord[Loop].u), &data[LOff], 4);
memcpy(&(texcoord[Loop].v), &data[LOff + 4], 4);
}
Offset += info.Size;
break;
case 0xA000: // Material Name
//printf("Chunk A000 (Material Name) - %s\nOffset %d\n",&data[Offset+6],info.Size);
//texture=data[Offset+6]; //lstrcpy(Matl[MatDex].Name,(LPSTR)&data[Offset+6]);
Offset += info.Size;
break;
case 0xA010: // Material - Ambient Color
//printf("Chunk A010 (Material - Amb Col)\nOffset %d\n",info.Size);
//memcpy(&Val,&data[Offset+6],2);
ambientColor =
(glRGBA) {data[Offset+6]/255.0f,data[Offset+7]/255.0f,data[Offset+8]/255.0f,data[Offset+9]/255.0f};
Offset += info.Size;
break;
case 0xA020: // Material - Diffuse Color
//printf("Chunk A020 (Material - Dif Col)\nOffset %d\n",info.Size);
CopyVal = true;
diffuseColor =
(glRGBA) {data[Offset+6]/255.0f,data[Offset+7]/255.0f,data[Offset+8]/255.0f,data[Offset+9]/255.0f};
Offset += info.Size;
//Offset+=6;//Info.Size;
break;
case 0xA030: // Material - Spec Color
specColor =
(glRGBA) {data[Offset+6]/255.0f,data[Offset+7]/255.0f,data[Offset+8]/255.0f,data[Offset+9]/255.0f};
Offset += info.Size;
//Offset+=6;//Info.Size;
break;
case 0xA040: // Material - Shininess
//printf("Chunk A040 (Material - Shininess)\nOffset %d\n",info.Size);
shininess = data[Offset + 6];
Offset += 6; //Info.Size;
break;
case 0xA041: // Material - Shine Strength
//printf("Chunk A041 (Material - Shine Strength)\nOffset %d\n",info.Size);
Offset += 6; //Info.Size;
break;
case 0xA050: // Material - Transparency
//printf("Chunk A050 (Material - Transparency)\nOffset %d\n",info.Size);
transparency = data[Offset + 6];
Offset += 6; //Info.Size;
break;
case 0xA100: // Material - Type (Flat,Gourad, Phong, Metal)
memcpy(&Val, &data[Offset + 6], 2);
//printf("Chunk A100 (Material Type) %d\nOffset %d\n",Val,info.Size);
texSmooth = Val;
Offset += info.Size;
break;
case 0xA200: // Material - Start of Texture Info
//printf("Chunk A200 (Material Tex Map)\nOffset %d\n",info.Size);
Offset += 6;
break;
case 0xA300: // Material - Texture Name
//printf("Chunk A300 (Material Tex Map Name) %s\nOffset %d\n",&data[Offset+6],info.Size);
texture = (char *) &data[Offset + 6];
Offset += info.Size;
break;
case 0xA351: // Material - Texture Options
memcpy(&Val, &data[Offset + 6], 2);
//printf("Chunk A351 (Material Tex Options) %d\nOffset %d\n",Val,info.Size);
Offset += info.Size;
break;
case 0xA354: // Material - Texture U Scale
memcpy(&fVal, &data[Offset + 6], 4);
//printf("Chunk A354 (Material Tex U Scale) %f\nOffset %d\n",fVal,info.Size);
TexCoordUScale = fVal;
Offset += info.Size;
break;
case 0xA356: // Material - Texture V Scale
memcpy(&fVal, &data[Offset + 6], 4);
//printf("Chunk A356 (Material Tex V Scale) %f\nOffset %d\n",fVal,info.Size);
TexCoordVScale = fVal;
Offset += info.Size;
break;
case 0xA35A: // Material - Texture V Offset
memcpy(&fVal, &data[Offset + 6], 4);
//printf("Chunk A35A (Material Tex V Offset) %f\nOffset %d\n",fVal,info.Size);
Offset += info.Size;
break;
case 0xA35C: // Material - Texture V Offset
memcpy(&fVal, &data[Offset + 6], 4);
//printf("Chunk A35C (Material Tex Rotation) %f\nOffset %d\n",fVal,info.Size);
Offset += info.Size;
break;
case 0xAFFF: // Material Start
//printf("Chunk AFFF (Start of Material)\nOffset %d\n",info.Size);
MatDex++;
Offset += 6;
break;
default:
Offset += info.Size;
break;
}
}
std::string path = "resources/images/";
if (!TextureMgr::get().load(texture, path + texture)) {
std::cerr << "[Could not load texture '" << texture << "', using none]"
<< std::endl;
texture = "";
}
if (TexCoordUScale <= 0.001f || TexCoordVScale <= 0.001f) {
TexCoordUScale = 1.0f;
TexCoordVScale = 1.0f;
}
/*for(int i=0;i<numTexCoords;i++)
{
texcoord[i].u*=TexCoordUScale;
texcoord[i].v*=TexCoordVScale;
}*/
calcNormals();
//drawelements
buffer = new GLuint[4];
glGenBuffersARB(4, buffer);
glBindBufferARB(GL_ELEMENT_ARRAY_BUFFER_ARB, buffer[0]);
glBufferDataARB(GL_ELEMENT_ARRAY_BUFFER_ARB, (numFaces) * sizeof(Face), face,
GL_STATIC_DRAW_ARB);
//glEnableClientState(GL_VERTEX_ARRAY);
glBindBufferARB(GL_ARRAY_BUFFER_ARB, buffer[1]);
glBufferDataARB(GL_ARRAY_BUFFER_ARB, (numVerts * 3) * sizeof(float), vertex,
GL_STATIC_DRAW_ARB);
//glEnableClientState(GL_NORMAL_ARRAY);
glBindBufferARB(GL_ARRAY_BUFFER_ARB, buffer[2]);
glBufferDataARB(GL_ARRAY_BUFFER_ARB, (numVerts * 3) * sizeof(float), normal,
GL_STATIC_DRAW_ARB);
if (texcoord) {
// glEnableClientState(GL_TEXTURE_COORD_ARRAY);
glBindBufferARB(GL_ARRAY_BUFFER_ARB, buffer[3]);
glBufferDataARB(GL_ARRAY_BUFFER_ARB, (numVerts * 2) * sizeof(float),
texcoord, GL_STATIC_DRAW_ARB);
}
//replace when models have shininess
shininess = 128;
this->scale = scale;
sphereRadius *= (scale * 2);
return true;
}
obvious::Matrix TSD_PDFMatching::match( const obvious::Matrix TSensor,
const obvious::Matrix* M,
const bool* maskM,
const obvious::Matrix* NM,
const obvious::Matrix* S,
const bool* maskS,
double phiMax,
const double transMax,
const double resolution)
{
obvious::Matrix TBest(3, 3);
TBest.setIdentity();
const int pointsInM = M->getRows();
const int pointsInS = S->getRows();
if(pointsInM != pointsInS)
{
LOGMSG(DBG_ERROR, "Model and scene need to be of same size, size of M: " << pointsInM << ", size of S: " << pointsInS);
return TBest;
}
if(pointsInM < 3)
{
LOGMSG(DBG_ERROR, "Model and scene contain too less points, size of M: " << pointsInM << ", size of S: " << pointsInS);
return TBest;
}
// ----------------- Model ------------------
obvious::Matrix* NMpca = new Matrix(pointsInM, 2); // Normals for model
double* phiM = new double[pointsInM]; // Orientation of model points
bool* maskMpca = new bool[pointsInM]; // Validity mask of model points
memcpy(maskMpca, maskM, pointsInM * sizeof(bool));
if(NM)
{
calcPhi(NM, maskM, phiM);
}
else // if normals are not supplied
{
calcNormals(M, NMpca, maskM, maskMpca, _pcaSearchRange/2);
calcPhi(NMpca, maskMpca, phiM);
}
vector<unsigned int> idxMValid = extractSamples(M, maskMpca, _pcaSearchRange / 2);
// -------------------------------------------
// ----------------- Scene -------------------
obvious::Matrix* NSpca = new Matrix(pointsInS, 2); // Normals for scene
double* phiS = new double[pointsInS]; // Orientation of scene points
bool* maskSpca = new bool[pointsInS]; // Validity mask of scene points
memcpy(maskSpca, maskS, pointsInS * sizeof(bool));
// Determine number of valid samples in local scene neighborhood
// only from these points a valid orientation is computable
unsigned int validPoints = 0;
for(int i = 0; i < pointsInS; i++)
if(maskSpca[i])
validPoints++;
// Probability of point masking
double probability = 180.0 / (double)validPoints;
if(probability < 0.99)
subsampleMask(maskSpca, pointsInS, probability);
calcNormals(S, NSpca, maskS, maskSpca, _pcaSearchRange/2);
calcPhi(NSpca, maskSpca, phiS);
vector<unsigned int> idxSValid = extractSamples(S, maskSpca, _pcaSearchRange / 2);
// -------------------------------------------
// --------------- Control set ---------------
vector<unsigned int> idxControl; //represents the indices of points used for Control in S.
obvious::Matrix* Control = pickControlSet(S, idxSValid, idxControl);
obvious::Matrix* NControl = new obvious::Matrix(idxControl.size(), 2);
for(unsigned int i = 0; i < Control->getCols(); i++)
{
(*NControl)(i, 0) = (*NSpca)(idxControl[i], 0);
(*NControl)(i, 1) = (*NSpca)(idxControl[i], 1);
}
unsigned int pointsInC = Control->getCols();
double* phiControl = new double[pointsInC]; // Orientation of control points
calcPhi(NControl, NULL, phiControl);
// -------------------------------------------//
// Determine frustum, i.e., direction of leftmost and rightmost model point
//double thetaMin = -((double)pointsInM - 1.0) / 2.0 * resolution; // theoretical bounding
double thetaBoundMin = atan2((*M)(idxMValid.front(), 1), (*M)(idxMValid.front(), 0)); // real bounding
double thetaBoundMax = atan2((*M)(idxMValid.back(), 1), (*M)(idxMValid.back(), 0)); // real bounding
LOGMSG(DBG_DEBUG, "Valid points in scene: " << idxSValid.size() << ", valid points in model: " << idxMValid.size() << ", Control set: " << Control->getCols());
LOGMSG(DBG_DEBUG, "Model phi min:: " << rad2deg(thetaBoundMin) << ", Model phi max: " << rad2deg(thetaBoundMax));
if(idxSValid.size() < 3)
{
LOGMSG(DBG_ERROR, "Too less valid points in scene, matchable size: " << idxSValid.size());
return TBest;
}
if(idxMValid.size() < 3)
{
LOGMSG(DBG_ERROR, "Too less valid points in model, matchable size: " << idxMValid.size());
return TBest;
}
// Check for maximum meaningful trials
unsigned int trials = _trials;
if(idxMValid.size() < _trials)
trials = idxMValid.size();
if(_trace)
{
_trace->reset();
_trace->setModel(M, idxMValid);
_trace->setScene(S, idxSValid);
}
// Calculate search "radius", i.e., maximum difference in polar indices because of rotation
phiMax = min(phiMax, M_PI * 0.5);
int span;
if(resolution > 1e-6)
{
span = floor(phiMax / resolution);
if(span > (int)pointsInM)
span = (int)pointsInM;
}
else
{
LOGMSG(DBG_ERROR, "Resolution not properly set: resolution = " << resolution);
return TBest;
}
srand(time(NULL));
double bestProb = 0.0;
#ifndef DEBUG
// trace is only possible for single threaded execution
if(_trace)
{
omp_set_num_threads(1);
LOGMSG(DBG_WARN, "Configured single-threaded execution due to application of trace module");
}
#endif
//Timer t;
//t.start();
vector<unsigned int> idxTrials = idxMValid;
bool* maskControl = new bool[pointsInC];
#pragma omp parallel for
for(unsigned int trial = 0; trial < trials; trial++)
{
int idx;
#pragma omp critical
{
const int randIdx = rand() % (idxTrials.size());
idx = idxTrials[randIdx];
// remove chosen element to avoid picking same index a second time
idxTrials.erase(idxTrials.begin() + randIdx);
}
// leftmost scene point
const int iMin = max(idx - span, _pcaSearchRange / 2);
// rightmost scene point
const int iMax = min(idx + span, pointsInS - _pcaSearchRange / 2);
for(int i = iMin; i < iMax; i++)
{
if(maskSpca[i])
{
double phi = phiM[idx] - phiS[i];
if(phi > M_PI)
phi -= 2.0 * M_PI;
else if(phi < -M_PI)
phi += 2.0 * M_PI;
if(fabs(phi) < phiMax)
{
obvious::Matrix T = obvious::MatrixFactory::TransformationMatrix33(phi, 0, 0);
// Calculate translation
const double sx = (*S)(i, 0);
const double sy = (*S)(i, 1);
T(0, 2) = (*M)(idx, 0) - (T(0, 0) * sx + T(0, 1) * sy);
T(1, 2) = (*M)(idx, 1) - (T(1, 0) * sx + T(1, 1) * sy);
obvious::Matrix TMap = TSensor * T;
// Transform control set
obvious::Matrix STemp = TMap * (*Control);
unsigned int pointsInControl = STemp.getCols();
// Rating Daniel Ammon & Tobias Fink
std::vector<double> probOfAllScans; // vector for probabilities of single scans in one measurement
double probOfActualMeasurement = 1.0;
for (unsigned int s = 0; s < pointsInControl; s++) // whole control set
{
obfloat coord[2];
coord[0] = STemp(0, s);
coord[1] = STemp(1, s);
// todo: magic numbers 0.05 / 0.95
obfloat tsd;
if( !_grid.interpolateBilinear(coord, &tsd) )
{
// rating function: clipped probability --> avoid prob of 0
// multiply all probabilities for probability of whole scan
probOfActualMeasurement *= (1.0 - (1.0 - _zrand) * fabs(tsd));
}
else
{
probOfActualMeasurement *= _zrand;
}
} // whole control set
#pragma omp critical
{
// update T and bestProb if better than last iteration
if(probOfActualMeasurement > bestProb)
{
TBest = T;
bestProb = probOfActualMeasurement;
#ifndef DEBUG
if(_trace)
{
//trace is only possible for single threaded execution
vector<unsigned int> idxM;
idxM.push_back(idx);
vector<unsigned int> idxS;
idxS.push_back(i);
_trace->addAssignment(M, idxM, S, idxS, &STemp, 10 * probOfActualMeasurement, trial);
}
#endif
}
}
} // if(fabs(phi) < phiMax)
} // if(maskSpca[i])
} // for i
} // for trials
//cout << "elapsed: " << t.elapsed() << endl;
//t.reset();
delete [] maskControl;
delete NMpca;
delete NSpca;
delete [] phiM;
delete [] phiS;
delete [] phiControl;
delete [] maskMpca;
delete [] maskSpca;
delete Control;
return TBest;
}
// Here is where the real meat of the OSD setup happens. The mesh topology is
// created and stored for later use. Actual subdivision happens in updateGeom
// which gets called at the end of this function and on frame change.
//
void
createOsdContext(int level)
{
//
// Setup an OsdHbr mesh based on the desired subdivision scheme
//
static OpenSubdiv::HbrCatmarkSubdivision<OpenSubdiv::OsdVertex> _catmark;
OsdHbrMesh *hmesh(new OsdHbrMesh(&_catmark));
//
// Now that we have a mesh, we need to add verticies and define the topology.
// Here, we've declared the raw vertex data in-line, for simplicity
//
float verts[] = { 0.000000f, -1.414214f, 1.000000f,
1.414214f, 0.000000f, 1.000000f,
-1.414214f, 0.000000f, 1.000000f,
0.000000f, 1.414214f, 1.000000f,
-1.414214f, 0.000000f, -1.000000f,
0.000000f, 1.414214f, -1.000000f,
0.000000f, -1.414214f, -1.000000f,
1.414214f, 0.000000f, -1.000000f
};
//
// The cube faces are also in-lined, here they are specified as quads
//
int faces[] = {
0,1,3,2,
2,3,5,4,
4,5,7,6,
6,7,1,0,
1,7,5,3,
6,0,2,4
};
//
// Record the original vertex positions and add verts to the mesh.
//
// OsdVertex is really just a place holder, it doesn't care what the
// position of the vertex is, it's just being used here as a means of
// defining the mesh topology.
//
for (unsigned i = 0; i < sizeof(verts)/sizeof(float); i += 3) {
g_orgPositions.push_back(verts[i+0]);
g_orgPositions.push_back(verts[i+1]);
g_orgPositions.push_back(verts[i+2]);
OpenSubdiv::OsdVertex vert;
hmesh->NewVertex(i/3, vert);
}
//
// Now specify the actual mesh topology by processing the faces array
//
const unsigned VERTS_PER_FACE = 4;
for (unsigned i = 0; i < sizeof(faces)/sizeof(int); i += VERTS_PER_FACE) {
//
// Do some sanity checking. It is a good idea to keep this in your
// code for your personal sanity as well.
//
// Note that this loop is not changing the HbrMesh, it's purely validating
// the topology that is about to be created below.
//
for (unsigned j = 0; j < VERTS_PER_FACE; j++) {
OsdHbrVertex * origin = hmesh->GetVertex(faces[i+j]);
OsdHbrVertex * destination = hmesh->GetVertex(faces[i+((j+1)%VERTS_PER_FACE)]);
OsdHbrHalfedge * opposite = destination->GetEdge(origin);
if(origin==NULL || destination==NULL) {
std::cerr <<
" An edge was specified that connected a nonexistent vertex"
<< std::endl;
exit(1);
}
if(origin == destination) {
std::cerr <<
" An edge was specified that connected a vertex to itself"
<< std::endl;
exit(1);
}
if(opposite && opposite->GetOpposite() ) {
std::cerr <<
" A non-manifold edge incident to more than 2 faces was found"
<< std::endl;
exit(1);
}
if(origin->GetEdge(destination)) {
std::cerr <<
" An edge connecting two vertices was specified more than once."
" It's likely that an incident face was flipped"
<< std::endl;
exit(1);
}
}
//
// Now, create current face given the number of verts per face and the
// face index data.
//
/* OsdHbrFace * face = */ hmesh->NewFace(VERTS_PER_FACE, faces+i, 0);
//
// If you had ptex data, you would set it here, for example
//
/* face->SetPtexIndex(ptexIndex) */
}
//
// Apply some tags to drive the subdivision algorithm. Here we set the
// default boundary interpolation mode along with a corner sharpness. See
// the API and the renderman spec for the full list of available operations.
//
hmesh->SetInterpolateBoundaryMethod( OsdHbrMesh::k_InterpolateBoundaryEdgeOnly );
OsdHbrVertex * v = hmesh->GetVertex(0);
v->SetSharpness(2.7f);
//
// Finalize the mesh object. The Finish() call is a signal to the internals
// that optimizations can be made on the mesh data.
//
hmesh->Finish();
//
// Setup some raw vectors of data. Remember that the actual point values were
// not stored in the OsdVertex, so we keep track of them here instead
//
g_normals.resize(g_orgPositions.size(),0.0f);
calcNormals( hmesh, g_orgPositions, g_normals );
//
// At this point, we no longer need the topological structure of the mesh,
// so we bake it down into subdivision tables by converting the HBR mesh
// into an OSD mesh. Note that this is just storing the initial subdivision
// tables, which will be used later during the actual subdivision process.
//
// Again, no vertex positions are being stored here, the point data will be
// sent to the mesh in updateGeom().
//
OpenSubdiv::FarMeshFactory<OpenSubdiv::OsdVertex> meshFactory(hmesh, level);
g_farmesh = meshFactory.Create();
g_osdComputeContext = OpenSubdiv::OsdCpuComputeContext::Create(g_farmesh);
delete hmesh;
//
// Initialize draw context and vertex buffer
//
g_vertexBuffer =
OpenSubdiv::OsdCpuGLVertexBuffer::Create(6, /* 3 floats for position,
+
3 floats for normal*/
g_farmesh->GetNumVertices());
g_drawContext =
OpenSubdiv::OsdGLDrawContext::Create(g_farmesh->GetPatchTables(), false);
g_drawContext->UpdateVertexTexture(g_vertexBuffer);
//
// Setup camera positioning based on object bounds. This really has nothing
// to do with OSD.
//
computeCenterAndSize(g_orgPositions, g_center, &g_size);
//
// Finally, make an explicit call to updateGeom() to force creation of the
// initial buffer objects for the first draw call.
//
updateGeom();
//
// The OsdVertexBuffer provides GL identifiers which can be bound in the
// standard way. Here we setup a single VAO and enable points and normals
// as attributes on the vertex buffer and set the index buffer.
//
GLuint vao;
glGenVertexArrays(1, &vao);
glBindVertexArray(vao);
glBindBuffer(GL_ARRAY_BUFFER, g_vertexBuffer->BindVBO());
glEnableVertexAttribArray(0);
glEnableVertexAttribArray(1);
glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, sizeof (GLfloat) * 6, 0);
glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, sizeof (GLfloat) * 6, (float*)12);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, g_drawContext->GetPatchIndexBuffer());
glBindBuffer(GL_ARRAY_BUFFER, 0);
}
