void ModelToCloudFitter::sampleMesh(const shape_msgs::Mesh &mesh,
std::vector<cv::Point3f> &btVectors,
double resolution)
{
btVectors.reserve(mesh.vertices.size());
//vertices themselves get inserted explicitly here. If we inserted them
//as part of triangles, we would insert each vertex multiple times
typedef std::vector<geometry_msgs::Point>::const_iterator I;
for (I i=mesh.vertices.begin(); i!=mesh.vertices.end(); i++)
{
btVectors.push_back(cv::Point3f(i->x,i->y,i->z));
}
//sample triangle surfaces at a specified min-resolution
//and insert the resulting points
for (size_t i=0; i<mesh.triangles.size(); ++i)
{
cv::Point3f v0( mesh.vertices[ mesh.triangles[i].vertex_indices[0] ].x,
mesh.vertices[ mesh.triangles[i].vertex_indices[0] ].y,
mesh.vertices[ mesh.triangles[i].vertex_indices[0] ].z);
cv::Point3f v1( mesh.vertices[ mesh.triangles[i].vertex_indices[1] ].x,
mesh.vertices[ mesh.triangles[i].vertex_indices[1] ].y,
mesh.vertices[ mesh.triangles[i].vertex_indices[1] ].z);
cv::Point3f v2( mesh.vertices[ mesh.triangles[i].vertex_indices[2] ].x,
mesh.vertices[ mesh.triangles[i].vertex_indices[2] ].y,
mesh.vertices[ mesh.triangles[i].vertex_indices[2] ].z);
std::vector<cv::Point3f> triangleVectors = interpolateTriangle(v0, v1, v2, resolution);
btVectors.insert(btVectors.begin(), triangleVectors.begin(), triangleVectors.end());
}
}
void ModelToCloudFitter::sampleMesh(const arm_navigation_msgs::Shape &mesh,
std::vector<btVector3> &btVectors,
double resolution)
{
btVectors.reserve(mesh.vertices.size());
//vertices themselves get inserted explicitly here. If we inserted them
//as part of triangles, we would insert each vertex multiple times
typedef std::vector<geometry_msgs::Point>::const_iterator I;
for (I i=mesh.vertices.begin(); i!=mesh.vertices.end(); i++)
{
btVectors.push_back(btVector3(i->x,i->y,i->z));
}
//sample triangle surfaces at a specified min-resolution
//and insert the resulting points
for (size_t i=0; i<mesh.triangles.size(); i+=3)
{
btVector3 v0( mesh.vertices.at( mesh.triangles.at(i+0) ).x,
mesh.vertices.at( mesh.triangles.at(i+0) ).y,
mesh.vertices.at( mesh.triangles.at(i+0) ).z);
btVector3 v1( mesh.vertices.at( mesh.triangles.at(i+1) ).x,
mesh.vertices.at( mesh.triangles.at(i+1) ).y,
mesh.vertices.at( mesh.triangles.at(i+1) ).z);
btVector3 v2( mesh.vertices.at( mesh.triangles.at(i+2) ).x,
mesh.vertices.at( mesh.triangles.at(i+2) ).y,
mesh.vertices.at( mesh.triangles.at(i+2) ).z);
std::vector<btVector3> triangleVectors = interpolateTriangle(v0, v1, v2, resolution);
btVectors.insert(btVectors.begin(), triangleVectors.begin(), triangleVectors.end());
}
}
bool SetZfromRasterdata::setZWithDelaunay(DM::RasterData * r, std::vector<DM::Component*> &nodes,double offset)
{
SetZfromRasterdata::Delaunay dt;
std::map<Vertex, K::FT, K::Less_xy_2> function_values;
typedef std::vector<DM::Component*>::iterator Iter;
if(!nodes.size())
return false;
//triangulate rester data
double xsellsize = r->getCellSizeX();
double ysellsize = r->getCellSizeY();
double yinit = r->getYOffset()+ysellsize/2;
double xinit = r->getXOffset()+xsellsize/2;
int errorcount = 0;
std::vector<Vertex> vvec;
for(uint index = 0; index < r->getHeight()*r->getWidth(); index++)
{
uint xindex = index/r->getWidth();
uint yindex = index%r->getWidth();
Vertex v(xinit+xindex*xsellsize,yinit+yindex*ysellsize);
vvec.push_back(v);
function_values.insert(std::make_pair(v,r->getCell(xindex,yindex)));
if(index%1000000==0)
DM::Logger(DM::Standard) << QString::number(int(index)).toStdString() << " of " << QString::number(r->getHeight()*r->getWidth()).toStdString();
}
dt.insert(vvec.begin(),vvec.end());
for(Iter it = nodes.begin(); it != nodes.end(); it++)
{
DM::Node *currentnode = static_cast<DM::Node*>((*it));
int li;
typedef Delaunay::Locate_type Locate_type;
Locate_type lt;
Vertex v(currentnode->getX(), currentnode->getY());
dt.locate(v,lt,li);
switch(lt)
{
case Delaunay::OUTSIDE_AFFINE_HULL:
{
errorcount++;
break;
}
case Delaunay::OUTSIDE_CONVEX_HULL:
{
errorcount++;
break;
}
default :
{
double elevation = interpolateTriangle(currentnode,dt,function_values);
currentnode->setZ(elevation+offset);
}
}
}
if(errorcount > 0)
DM::Logger(DM::Error) << "Could not find elevation for " << errorcount << " nodes";
return true;
}
