void
vtree_color_user::compute_features( const std::string& cloud_filename,
FeatureVector &feature_vector )
{
typedef pcl::PointXYZRGB nx_PointT;
typedef pcl::Normal nx_Normal;
typedef pcl::PointCloud<nx_PointT> nx_PointCloud;
typedef pcl::PointCloud<nx_Normal> nx_PointCloud_normal;
typedef pcl::PointCloud<int> nx_PointCloud_int;
typedef pcl::UniformSampling<nx_PointT> nx_Sampler;
typedef pcl::search::KdTree<nx_PointT> nx_SearchMethod;
typedef pcl::NormalEstimation<nx_PointT, nx_Normal> nx_NormalEst;
#if FEATURE == 1
typedef pcl::PFHRGBSignature250 nx_FeatureType;
typedef pcl::PFHRGBEstimation<nx_PointT, nx_Normal, nx_FeatureType> nx_FeatureEst;
#elif FEATURE == 2
typedef pcl::PPFRGBSignature nx_FeatureType;
typedef pcl::PPFRGBEstimation<nx_PointT, nx_Normal, nx_FeatureType> nx_FeatureEst;
#else
#error A valid feature definition is required!
#endif
typedef pcl::PointCloud<nx_FeatureType> nx_PointCloud_feature;
// load the file
nx_PointCloud::Ptr cld_ptr(new nx_PointCloud);
pcl::io::loadPCDFile<nx_PointT> ( cloud_filename, *cld_ptr);
ROS_INFO("[vtree_color_user] Starting keypoint extraction...");
clock_t tic = clock();
nx_PointCloud::Ptr keypoints( new nx_PointCloud);
nx_PointCloud_int::Ptr keypoint_idx(new nx_PointCloud_int);
nx_Sampler uniform_sampling;
uniform_sampling.setInputCloud ( cld_ptr );
uniform_sampling.setRadiusSearch ( keypoint_radius_ );
uniform_sampling.compute( *keypoint_idx );
pcl::copyPointCloud ( *cld_ptr, keypoint_idx->points, *keypoints);
ROS_INFO("[vtree_color_user] No of Keypoints found %d", static_cast<int>(keypoint_idx->size()) );
ROS_INFO("[vtree_color_user] Keypoint extraction took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );
if( keypoints->empty() )
{
ROS_WARN("[vtree_color_user] No keypoints were found...");
return;
}
// Compute normals for the input cloud
ROS_INFO("[vtree_color_user] Starting normal extraction...");
tic = clock();
nx_PointCloud_normal::Ptr normals (new nx_PointCloud_normal);
nx_SearchMethod::Ptr search_method_xyz (new nx_SearchMethod);
nx_NormalEst norm_est;
norm_est.setInputCloud ( cld_ptr );
norm_est.setSearchMethod ( search_method_xyz );
norm_est.setRadiusSearch ( normal_radius_ );
norm_est.compute ( *normals );
ROS_INFO("[vtree_color_user] Normal extraction took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );
// Get features at the computed keypoints
ROS_INFO("[vtree_color_user] Starting feature computation...");
tic = clock();
nx_PointCloud_feature::Ptr features(new nx_PointCloud_feature);
nx_FeatureEst feat_est;
feat_est.setInputCloud ( keypoints );
feat_est.setSearchSurface ( cld_ptr );
feat_est.setInputNormals ( normals );
search_method_xyz.reset(new nx_SearchMethod);
feat_est.setSearchMethod ( search_method_xyz );
feat_est.setRadiusSearch ( feature_radius_ );
feat_est.compute ( *features );
ROS_INFO("[vtree_color_user] No of Features found %d", static_cast<int>(features->size()) );
ROS_INFO("[vtree_color_user] Feature computation took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );
// Rectify the historgram values to ensure they are in [0,100] and create a document
for( nx_PointCloud_feature::iterator iter = features->begin();
iter != features->end(); ++iter)
{
rectify_histogram( iter->histogram );
feature_vector.push_back( FeatureHist( iter->histogram ) );
}
}
ShapeParser::ShapeParseResult ShapeParser::serialized(const XMLNode& node, ParserState& S)
{
auto filename = S.map_asset_filepath(S.def_storage.prop_string(node, "filename"));
int submesh_index = S.def_storage.prop_int(node, "shapeIndex");
bool flipNormals = S.def_storage.prop_bool(node, "flipNormals", false);
bool faceNormals = S.def_storage.prop_bool(node, "faceNormals", false);
float maxSmoothAngle = S.def_storage.prop_float(node, "maxSmoothAngle", 0.0f);
auto name = boost::filesystem::path(filename).stem().string();
auto compiled_tar_folder = S.scene.getFileManager()->getCompiledMeshPath("") + name + "/";
auto get_compiled_submesh_filename = [&](size_t i)
{
return compiled_tar_folder + std::to_string(i) + ".xmsh";
};
if (!boost::filesystem::exists(compiled_tar_folder) || !boost::filesystem::exists(get_compiled_submesh_filename(0)))
{
boost::filesystem::create_directory(compiled_tar_folder);
enum DataPresentFlag : uint32_t
{
VertexNormals = 0x0001,
TextureCoords = 0x0002,
VertexColors = 0x0008,
UseFaceNormals = 0x0010,
SinglePrecision = 0x1000,
DoublePrecision = 0x2000,
};
struct inflateStream
{
std::ifstream& m_childStream;
size_t str_length;
z_stream m_inflateStream;
uint8_t m_inflateBuffer[32768];
inflateStream(std::ifstream& str)
:m_childStream(str)
{
size_t pos = m_childStream.tellg();
m_childStream.seekg(0, m_childStream.end);
str_length = m_childStream.tellg();
m_childStream.seekg(pos, m_childStream.beg);
m_inflateStream.zalloc = Z_NULL;
m_inflateStream.zfree = Z_NULL;
m_inflateStream.opaque = Z_NULL;
m_inflateStream.avail_in = 0;
m_inflateStream.next_in = Z_NULL;
int windowBits = 15;
auto retval = inflateInit2(&m_inflateStream, windowBits);
if (retval != Z_OK)
std::cout << "erro, ret : " << retval << std::endl;
}
void read(void *ptr, size_t size)
{
uint8_t *targetPtr = (uint8_t *)ptr;
while (size > 0) {
if (m_inflateStream.avail_in == 0) {
size_t remaining = str_length - m_childStream.tellg();
m_inflateStream.next_in = m_inflateBuffer;
m_inflateStream.avail_in = (uInt)std::min(remaining, sizeof(m_inflateBuffer));
if (m_inflateStream.avail_in == 0)
std::cout << "more bytes req : " << size << std::endl;
m_childStream.read((char*)m_inflateBuffer, m_inflateStream.avail_in);
}
m_inflateStream.avail_out = (uInt)size;
m_inflateStream.next_out = targetPtr;
int retval = inflate(&m_inflateStream, Z_NO_FLUSH);
switch (retval) {
case Z_STREAM_ERROR:
throw std::runtime_error("inflate(): stream error!");
case Z_NEED_DICT:
throw std::runtime_error("inflate(): need dictionary!");
case Z_DATA_ERROR:
throw std::runtime_error("inflate(): data error!");
case Z_MEM_ERROR:
throw std::runtime_error("inflate(): memory error!");
};
size_t outputSize = size - (size_t)m_inflateStream.avail_out;
targetPtr += outputSize;
size -= outputSize;
if (size > 0 && retval == Z_STREAM_END)
throw std::runtime_error("inflate(): attempting to read past the end of the stream!");
}
}
};
std::ifstream ser_str(filename, std::ios::binary);
uint16_t magic_maj, version_maj;
ser_str.read((char*)&magic_maj, 2);
if (magic_maj != 1052)
throw std::runtime_error("corrupt file");
ser_str.read((char*)&version_maj, 2);
ser_str.seekg(-4, ser_str.end);
uint32_t n_meshes;
ser_str.read((char*)&n_meshes, sizeof(n_meshes));
ser_str.seekg(-(sizeof(uint32_t) + (version_maj == 4 ? sizeof(uint64_t) : sizeof(uint32_t)) * n_meshes), ser_str.end);
std::vector<uint64_t> mesh_offsets(n_meshes);
if (version_maj == 4)
ser_str.read((char*)mesh_offsets.data(), n_meshes * sizeof(uint64_t));
else
{
auto q = std::vector<uint32_t>(n_meshes);
ser_str.read((char*)q.data(), n_meshes * sizeof(uint32_t));
for (size_t i = 0; i < n_meshes; i++)
mesh_offsets[i] = q[i];
}
for (size_t num_submesh = 0; num_submesh < n_meshes; num_submesh++)
{
ser_str.seekg(mesh_offsets[num_submesh], ser_str.beg);
uint16_t magic, version;
ser_str.read((char*)&magic, 2);
if (magic == 0)
break;
ser_str.read((char*)&version, 2);
if (version != 3 && version != 4)
throw std::runtime_error("invalid version in serialized mesh file");
inflateStream comp_str(ser_str);
DataPresentFlag flag;
comp_str.read(&flag, sizeof(flag));
std::string name = "default";
if (version == 4)
{
name = "";
char last_read;
do
{
comp_str.read(&last_read, sizeof(last_read));
name += last_read;
} while (last_read != 0);
}
uint64_t nVertices, nTriangles;
comp_str.read(&nVertices, sizeof(nVertices));
comp_str.read(&nTriangles, sizeof(nTriangles));
std::vector<Vec3f> positions(nVertices), normals(nVertices), colors(nVertices);
std::vector<Vec2f> uvcoords(nVertices);
std::vector<uint32_t> indices(nTriangles * 3);
bool isSingle = true;
auto read_n_vector = [&](int dim, float* buffer)
{
if (isSingle)
comp_str.read((char*)buffer, sizeof(float) * dim * nVertices);
else
{
double* double_storage = (double*)alloca(dim * sizeof(double));
for (size_t i = 0; i < nVertices; i++)
{
comp_str.read((char*)double_storage, dim * sizeof(double));
for (int j = 0; j < dim; j++)
buffer[i * dim + j] = float(double_storage[j]);
}
}
};
read_n_vector(3, (float*)positions.data());
if ((flag & DataPresentFlag::VertexNormals) == DataPresentFlag::VertexNormals)
read_n_vector(3, (float*)normals.data());
if ((flag & DataPresentFlag::TextureCoords) == DataPresentFlag::TextureCoords)
read_n_vector(2, (float*)uvcoords.data());
else std::fill(uvcoords.begin(), uvcoords.end(), Vec2f(0.0f));
if ((flag & DataPresentFlag::VertexColors) == DataPresentFlag::VertexColors)
read_n_vector(3, (float*)colors.data());
comp_str.read((char*)indices.data(), sizeof(uint32_t) * nTriangles * 3);
for (size_t i = 0; i < nTriangles * 3; i += 3)
std::swap(indices[i + 0], indices[i + 2]);
auto compiled_submesh_filename = get_compiled_submesh_filename(num_submesh);
FileOutputStream fOut(compiled_submesh_filename);
fOut << (unsigned int)MeshCompileType::Static;
auto mat = Material(name.size() > 60 ? name.substr(0, 60) : name);
mat.bsdf = CreateAggregate<BSDFALL>(diffuse());
Mesh::CompileMesh(positions.data(), (int)positions.size(), normals.data(), uvcoords.data(), indices.data(), (int)indices.size(), mat, 0.0f, fOut, flipNormals, faceNormals, maxSmoothAngle);
fOut.Close();
}
ser_str.close();
}
auto obj = S.scene.CreateNode(get_compiled_submesh_filename(submesh_index));
parseGeneric(obj, node, S);
return obj;
}
/* Accepts a range of inverted tris. Refacets affected surface so that no tris
are inverted. */
MBErrorCode remove_inverted_tris(MBTag normal_tag, MBRange tris, const bool debug ) {
MBErrorCode result;
bool failures_occur = false;
while(!tris.empty()) {
/* Get a group of triangles to re-facet. They must be adjacent to each other
and in the same surface. */
MBRange tris_to_refacet;
tris_to_refacet.insert( tris.front() );
MBRange surf_set;
result = MBI()->get_adjacencies( tris_to_refacet, 4, false, surf_set );
assert(MB_SUCCESS == result);
if(1 != surf_set.size()) {
std::cout << "remove_inverted_tris: tri is in " << surf_set.size()
<< " surfaces" << std::endl;
return MB_FAILURE;
}
// get all tris in the surface
MBRange surf_tris;
result = MBI()->get_entities_by_type( surf_set.front(), MBTRI, surf_tris );
assert(MB_SUCCESS == result);
/* Find all of the adjacent inverted triangles of the same surface. Keep
searching until a search returns no new triangles. */
bool search_again = true;
while(search_again) {
// Here edges are being created. Remember to delete them. Outside of this
// function. Skinning gets bogged down if unused MBEdges (from other
// surfaces) accumulate.
MBRange tri_edges;
result = MBI()->get_adjacencies( tris_to_refacet, 1, true, tri_edges,
MBInterface::UNION );
assert(MB_SUCCESS == result);
MBRange connected_tris;
result = MBI()->get_adjacencies( tri_edges, 2, false, connected_tris,
MBInterface::UNION );
assert(MB_SUCCESS == result);
result = MBI()->delete_entities( tri_edges );
assert(MB_SUCCESS == result);
MBRange tris_to_refacet2 = intersect( tris_to_refacet, connected_tris );
tris_to_refacet2 = intersect( tris_to_refacet, surf_tris );
if(tris_to_refacet.size() == tris_to_refacet2.size()) search_again = false;
tris_to_refacet.swap( tris_to_refacet2 );
}
// Remove the inverted tris that will be refaceted.
tris = subtract( tris, tris_to_refacet );
// do edges already exist?
MBRange temp;
result = MBI()->get_entities_by_type(0, MBEDGE, temp );
assert(MB_SUCCESS == result);
if(!temp.empty()) MBI()->list_entities( temp );
assert(temp.empty());
// keep enlarging patch until we have tried to refacet the entire surface
int counter=0;
while(true) {
// do edges already exist?
temp.clear();
result = MBI()->get_entities_by_type(0, MBEDGE, temp );
assert(MB_SUCCESS == result);
if(!temp.empty()) MBI()->list_entities( temp );
assert(temp.empty());
counter++;
// Only try enlarging each patch a few times
if(48 == counter) {
failures_occur = true;
if(debug) std::cout << "remove_inverted_tris: ear clipping failed, counter="
<< counter << std::endl;
break;
}
// THIS PROVIDES A BAD EXIT. MUST FIX
// get the edges of the patch of inverted tris
MBRange tri_edges;
result = MBI()->get_adjacencies( tris_to_refacet, 1, true, tri_edges,
MBInterface::UNION );
assert(MB_SUCCESS == result);
// get all adjacent tris to the patch of inverted tris in the surface
MBRange adj_tris;
result = MBI()->get_adjacencies( tri_edges, 2, false, adj_tris,
MBInterface::UNION );
assert(MB_SUCCESS == result);
result = MBI()->delete_entities( tri_edges );
assert(MB_SUCCESS == result);
tris_to_refacet = intersect( surf_tris, adj_tris );
if(tris_to_refacet.empty()) continue;
//gen::print_triangles( tris_to_refacet );
// get an area-weighted normal of the adj_tris
MBCartVect plane_normal(0,0,0);
//for(unsigned int i=0; i<tris_to_refacet.size(); i++) {
for(MBRange::iterator i=tris_to_refacet.begin(); i!=tris_to_refacet.end(); i++) {
MBCartVect norm;
result = MBI()->tag_get_data( normal_tag, &(*i), 1, &norm);
assert(MB_SUCCESS == result);
double area;
result = gen::triangle_area( *i, area );
assert(MB_SUCCESS == result);
if(debug) std::cout << "norm=" << norm << " area=" << area << std::endl;
//plane_normal += norm*area;
plane_normal += norm;
}
plane_normal.normalize();
// do edges already exist?
temp.clear();
result = MBI()->get_entities_by_type(0, MBEDGE, temp );
assert(MB_SUCCESS == result);
if(!temp.empty()) MBI()->list_entities( temp );
assert(temp.empty());
// skin the tris
MBRange unordered_edges;
//MBSkinner tool(MBI());
//result = tool.find_skin( tris_to_refacet, 1, unordered_edges, false );
result = gen::find_skin( tris_to_refacet, 1, unordered_edges, false );
assert(MB_SUCCESS == result);
if(unordered_edges.empty()) {
// do edges already exist?
MBRange temp;
result = MBI()->get_entities_by_type(0, MBEDGE, temp );
assert(MB_SUCCESS == result);
if(!temp.empty()) MBI()->list_entities( temp );
assert(temp.empty());
continue;
}
//std::cout << "remove_inverted_tris: surf_id="
// << gen::geom_id_by_handle(surf_set.front()) << std::endl;
//result = MBI()->list_entities( tris_to_refacet );
//assert(MB_SUCCESS == result);
// assemble into a polygon
std::vector<MBEntityHandle> polygon_of_verts;
result = arc::order_verts_by_edge( unordered_edges, polygon_of_verts );
if(debug) gen::print_loop( polygon_of_verts );
//assert(MB_SUCCESS == result);
if(MB_SUCCESS != result) {
if(debug) std::cout << "remove_inverted_tris: couldn't order polygon by edge" << std::endl;
return MB_FAILURE;
}
// remember to remove edges
result = MBI()->delete_entities( unordered_edges );
assert(MB_SUCCESS == result);
// remove the duplicate endpt
polygon_of_verts.pop_back();
// the polygon should have at least 3 verts
if(3 > polygon_of_verts.size()) {
if(debug) std::cout << "remove_inverted_tris: polygon has too few points" << std::endl;
return MB_FAILURE;
}
// orient the polygon with the triangles (could be backwards)
// get the first adjacent tri
MBEntityHandle edge[2] = { polygon_of_verts[0], polygon_of_verts[1] };
MBRange one_tri;
result = MBI()->get_adjacencies( edge, 2, 2, false, one_tri );
assert(MB_SUCCESS == result);
one_tri = intersect( tris_to_refacet, one_tri );
assert(1 == one_tri.size());
const MBEntityHandle *conn;
int n_conn;
result = MBI()->get_connectivity( one_tri.front(), conn, n_conn );
assert(MB_SUCCESS == result);
assert(3 == n_conn);
if( (edge[0]==conn[1] && edge[1]==conn[0]) ||
(edge[0]==conn[2] && edge[1]==conn[1]) ||
(edge[0]==conn[0] && edge[1]==conn[2]) ) {
reverse( polygon_of_verts.begin(), polygon_of_verts.end() );
if(debug) std::cout << "remove_inverted_tris: polygon reversed" << std::endl;
}
/* facet the polygon. Returns MB_FAILURE if it fails to facet the polygon. */
MBRange new_tris;
result = gen::ear_clip_polygon( polygon_of_verts, plane_normal, new_tris );
// break if the refaceting is successful
if(MB_SUCCESS == result) {
// summarize tri area
for(MBRange::iterator i=new_tris.begin(); i!=new_tris.end(); i++) {
double area;
result = gen::triangle_area( *i, area );
assert(MB_SUCCESS == result);
if(debug) std::cout << " new tri area=" << area << std::endl;
}
// check the new normals
std::vector<MBCartVect> new_normals;
result = gen::triangle_normals( new_tris, new_normals );
if(MB_SUCCESS != result) return result;
// test the new triangles
std::vector<int> inverted_tri_indices;
std::vector<MBCartVect> normals ( new_normals.size(), plane_normal );
result = zip::test_normals( normals, new_normals, inverted_tri_indices );
assert(MB_SUCCESS == result);
if(inverted_tri_indices.empty()) {
// remove the tris that were re-faceted
tris = subtract( tris, tris_to_refacet );
result = MBI()->remove_entities( surf_set.front(), tris_to_refacet );
assert(MB_SUCCESS == result);
result = MBI()->delete_entities( tris_to_refacet );
assert(MB_SUCCESS == result);
// add the new tris to the surf set
result = MBI()->add_entities( surf_set.front(), new_tris );
assert(MB_SUCCESS == result);
// put the new normals on the new tris
result = gen::save_normals( new_tris, normal_tag );
assert(MB_SUCCESS == result);
if(debug) std::cout << "remove_inverted_tris: success fixing a patch" << std::endl;
break;
}
}
// remember to delete the tris that were created from the failed ear clipping
else {
result = MBI()->delete_entities( new_tris );
assert(MB_SUCCESS == result);
}
// If the entire surface could not be ear clipped, give up
if (tris_to_refacet.size() == surf_tris.size()) {
if(debug) std::cout << "remove_inverted_tris: ear clipping entire surface failed"
<< std::endl;
return MB_FAILURE;
}
} // loop until the entire surface has attempted to be refaceted
} // loop over each patch of inverted tris
if(failures_occur) {
if(debug) std::cout << "remove_inverted_facets: at least one failure occured" << std::endl;
return MB_FAILURE;
} else {
return MB_SUCCESS;
}
}
int main() {
std::string dataRoot;
#ifdef DATA_ROOT
dataRoot = DATA_ROOT;
#else
std::cerr << "No DATA_ROOT path found" << std::endl;
return -1;
#endif
GLFWwindow *window = nullptr;
if (initWindow(window)) {
return -1;
}
Shader shader(dataRoot + "/data/shaders/shader.vert", dataRoot + "/data/shaders/shader.frag");
Texture textures(dataRoot + "/data/assets/textures.png");
Texture normals(dataRoot + "/data/assets/normals.png");
std::vector<GLfloat> vertices;
std::vector<GLuint> indices;
std::vector<Quad> quads;
Map map(10, 10);
for (int x = 0; x < map.getWidth(); ++x) {
for (int y = 0; y < map.getHeight(); ++y) {
if (map.isPassable(x, y)) {
quads.push_back(Quad(glm::vec3(x, 0, -y), glm::vec3(0, 0, -1), glm::vec3(1, 0, 0), 0.5f, 0.0f));
quads.push_back(Quad(glm::vec3(x, 1, -y), glm::vec3(1, 0, 0), glm::vec3(0, 0, -1), 0.0f, 0.5f));
}
for (int direction = 0; direction < 4; ++direction) {
int cx = x + dx[direction];
int cy = y + dy[direction];
if (map.isPassable(x, y) && !map.isPassable(cx, cy)) {
quads.push_back(Quad(
glm::vec3(x, 0, -y) + WALL_SHIFT[direction],
glm::vec3(0, 1, 0),
WALL_DIRECTION[direction],
0.0f,
0.0f
));
}
}
}
}
for (int index = 0; index < quads.size(); ++index) {
Quad quad = quads[index];
quad.update(vertices);
indices.push_back(GLuint(4 * index + 0));
indices.push_back(GLuint(4 * index + 1));
indices.push_back(GLuint(4 * index + 2));
indices.push_back(GLuint(4 * index + 2));
indices.push_back(GLuint(4 * index + 3));
indices.push_back(GLuint(4 * index + 0));
}
GLuint VBO, VAO, EBO;
createBuffers(VBO, VAO, EBO, vertices, indices);
glm::mat4 projection = glm::perspective(45.0f, WIDTH / (float) HEIGHT, 0.1f, 100.0f);
glm::vec3 lamp(map.getWidth() / 2, 0.9f, -map.getHeight() / 2);
float time = (float) glfwGetTime();
while (!glfwWindowShouldClose(window)) {
glfwPollEvents();
float cTime = (float) glfwGetTime();
float delta = cTime - time;
time = cTime;
update(map, delta);
glClearColor(117 / 255.0f, 187 / 255.0f, 253 / 255.0f, 1.0f);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
shader.use();
GLint textureLocation = glGetUniformLocation(shader.get(), "textureSampler");
glUniform1i(textureLocation, 0);
GLint normalLocation = glGetUniformLocation(shader.get(), "normalSampler");
glUniform1i(normalLocation, 1);
glActiveTexture(GL_TEXTURE0);
glBindTexture(GL_TEXTURE_2D, textures.get());
glActiveTexture(GL_TEXTURE1);
glBindTexture(GL_TEXTURE_2D, normals.get());
GLfloat timeValue = (GLfloat) glfwGetTime();
GLfloat xValue = (GLfloat) (sin(timeValue) / 4) + 0.25f;
GLint xValueLocation = glGetUniformLocation(shader.get(), "xValue");
glUniform1f(xValueLocation, xValue);
GLfloat yValue = (GLfloat) (cos(timeValue + 0.2) / 4) + 0.25f;
GLint yValueLocation = glGetUniformLocation(shader.get(), "yValue");
glUniform1f(yValueLocation, yValue);
glm::mat4 model;
model = glm::rotate(model, rotation, glm::vec3(0.0f, 1.0f, 0.0f));
model = glm::translate(model, -cameraPos);
GLint modelLocation = glGetUniformLocation(shader.get(), "model");
glUniformMatrix4fv(modelLocation, 1, GL_FALSE, glm::value_ptr(model));
glm::mat4 view;
glm::vec3 eye(0.0f, 0.5f, 0.0f);
view = glm::lookAt(eye, eye + cameraFront, cameraUp);
GLint viewLocation = glGetUniformLocation(shader.get(), "view");
glUniformMatrix4fv(viewLocation, 1, GL_FALSE, glm::value_ptr(view));
GLint projectionLocation = glGetUniformLocation(shader.get(), "projection");
glUniformMatrix4fv(projectionLocation, 1, GL_FALSE, glm::value_ptr(projection));
GLint lightSourceLocation = glGetUniformLocation(shader.get(), "lightSource");
if (spotlight) {
glUniform3f(lightSourceLocation, cameraPos.x, cameraPos.y + 0.5f, cameraPos.z);
} else {
glUniform3f(lightSourceLocation, lamp.x, lamp.y, lamp.z);
}
glm::vec3 lookDirection = glm::normalize(glm::rotate(cameraFront, -rotation, cameraUp));
GLint lookDirectionLocation = glGetUniformLocation(shader.get(), "lookDirection");
glUniform3f(lookDirectionLocation, lookDirection.x, lookDirection.y, lookDirection.z);
GLint spotlightLocation = glGetUniformLocation(shader.get(), "spotlight");
glUniform1i(spotlightLocation, spotlight);
glBindVertexArray(VAO);
glDrawElements(GL_TRIANGLES, (GLuint) indices.size(), GL_UNSIGNED_INT, 0);
glBindVertexArray(0);
glfwSwapBuffers(window);
}
disposeBuffers(VBO, VAO, EBO);
glfwTerminate();
return 0;
}
int
main (int argc, char** argv)
{
if (argc < 2){
std::cerr << "Usage:" << argv[0] << " PCD_filename [-smoothness 7.0] [-distance 0.1]"
<< std::endl;
return 0;
}
const char *filename = argv[1];
double smoothness=7.0, distance=0.01;
for (int i=2; i<argc; i++){
if (strcmp("-smoothness",argv[i])==0){
smoothness=atof(argv[++i]);
}else if(strcmp("-distance", argv[i])==0){
distance=atof(argv[++i]);
}
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PCDReader reader;
reader.read (filename, *cloud);
int npoint = cloud->points.size();
std::cout << "npoint = " << npoint << std::endl;
pcl::search::Search<pcl::PointXYZRGB>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZRGB> > (new pcl::search::KdTree<pcl::PointXYZRGB>);
pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> normal_estimator;
normal_estimator.setSearchMethod (tree);
normal_estimator.setInputCloud (cloud);
normal_estimator.setKSearch (50);
normal_estimator.compute (*normals);
pcl::RegionGrowing<pcl::PointXYZRGB, pcl::Normal> reg;
reg.setMinClusterSize (100);
reg.setMaxClusterSize (40000);
reg.setSearchMethod (tree);
reg.setNumberOfNeighbours (30);
reg.setInputCloud (cloud);
reg.setInputNormals (normals);
reg.setSmoothnessThreshold (smoothness / 180.0 * M_PI);
reg.setCurvatureThreshold (1.0);
std::vector <pcl::PointIndices> clusters;
reg.extract (clusters);
std::cout << "Number of clusters is equal to " << clusters.size () << std::endl;
pcl::visualization::PCLVisualizer viewer("Cloud Viewer");
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (distance);
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_p(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
int count = 1;
for (unsigned int i=0; i<clusters.size(); i++){
std::cout << "cluster" << i << std::endl;
extract.setInputCloud( cloud );
*inliers = clusters[i];
extract.setIndices( inliers );
extract.setNegative( false );
extract.filter( *cluster );
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = cluster;
while(cloud->points.size() > (int)(cluster->points.size())*0.1){
std::cout << "count = " << count << std::endl;
seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
PCL_ERROR ("Could not estimate a planar model for the given dataset.");
return (-1);
}
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
std::cerr << "roll:" << -asin(coefficients->values[1])
<< "[rad](" << -asin(coefficients->values[1])*180/M_PI
<< "[deg]), pitch:" << asin(coefficients->values[0])
<< "[rad](" << asin(coefficients->values[0])*180/M_PI
<< "[deg])" << std::endl;
extract.setInputCloud( cloud );
extract.setIndices( inliers );
extract.setNegative( false );
extract.filter( *cloud_p );
for (unsigned int i=0; i<cloud_p->points.size(); i++){
cloud_p->points[i].r = count&1 ? 255 : 0;
cloud_p->points[i].g = count&2 ? 255 : 0;
cloud_p->points[i].b = count&4 ? 255 : 0;
}
std::stringstream ss;
ss << "cloud_" << count;
viewer.addPointCloud( cloud_p, ss.str() );
// 平面として選ばれなかった側の点に対する処理
extract.setNegative( true );
extract.filter( *cloud_f );
cloud = cloud_f;
// カウントアップ
count++;
}
}
while (!viewer.wasStopped ()){
viewer.spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return (0);
}
std::list<VoronoiShard>
voronoi_convex_hull_shatter(const gl::MeshPtr &the_mesh,
const std::vector<glm::vec3>& the_voronoi_points)
{
// points define voronoi cells in world space (avoid duplicates)
// verts = source (convex hull) mesh vertices in local space
std::list<VoronoiShard> ret;
std::vector<glm::vec3> mesh_verts = the_mesh->geometry()->vertices();
auto convexHC = std::make_shared<btConvexHullComputer>();
btAlignedObjectArray<btVector3> vertices;
btVector3 rbb, nrbb;
btScalar nlength, maxDistance, distance;
std::vector<glm::vec3> sortedVoronoiPoints = the_voronoi_points;
btVector3 normal, plane;
btAlignedObjectArray<btVector3> planes, convexPlanes;
std::set<int> planeIndices;
std::set<int>::iterator planeIndicesIter;
int numplaneIndices;
int i, j, k;
// Normalize verts (numerical stability), convert to world space and get convexPlanes
int numverts = mesh_verts.size();
// auto aabb = the_mesh->boundingBox();
// float scale_val = 1.f;//std::max(std::max(aabb.width(), aabb.height()), aabb.depth());
auto mesh_transform = the_mesh->global_transform() ;//* scale(glm::mat4(), vec3(1.f / scale_val));
std::vector<glm::vec3> world_space_verts;
world_space_verts.resize(mesh_verts.size());
for (i = 0; i < numverts ;i++)
{
world_space_verts[i] = (mesh_transform * vec4(mesh_verts[i], 1.f)).xyz();
}
//btGeometryUtil::getPlaneEquationsFromVertices(chverts, convexPlanes);
// Using convexHullComputer faster than getPlaneEquationsFromVertices for large meshes...
convexHC->compute(&world_space_verts[0].x, sizeof(world_space_verts[0]), numverts, 0.0, 0.0);
int numFaces = convexHC->faces.size();
int v0, v1, v2; // vertices
// get plane equations for the convex-hull n-gons
for (i = 0; i < numFaces; i++)
{
const btConvexHullComputer::Edge* edge = &convexHC->edges[convexHC->faces[i]];
v0 = edge->getSourceVertex();
v1 = edge->getTargetVertex();
edge = edge->getNextEdgeOfFace();
v2 = edge->getTargetVertex();
plane = (convexHC->vertices[v1]-convexHC->vertices[v0]).cross(convexHC->vertices[v2]-convexHC->vertices[v0]).normalize();
plane[3] = -plane.dot(convexHC->vertices[v0]);
convexPlanes.push_back(plane);
}
const int numconvexPlanes = convexPlanes.size();
int numpoints = the_voronoi_points.size();
for (i = 0; i < numpoints ; i++)
{
auto curVoronoiPoint = the_voronoi_points[i];
planes.copyFromArray(convexPlanes);
for (j = 0; j < numconvexPlanes; j++)
{
planes[j][3] += planes[j].dot(type_cast(the_voronoi_points[i]));
}
maxDistance = SIMD_INFINITY;
// sort voronoi points
std::sort(sortedVoronoiPoints.begin(), sortedVoronoiPoints.end(), pointCmp(curVoronoiPoint));
for (j=1; j < numpoints; j++)
{
normal = type_cast(sortedVoronoiPoints[j] - curVoronoiPoint);
nlength = normal.length();
if (nlength > maxDistance)
break;
plane = normal.normalized();
plane[3] = -nlength / btScalar(2.);
planes.push_back(plane);
getVerticesInsidePlanes(planes, vertices, planeIndices);
if (vertices.size() == 0) break;
numplaneIndices = planeIndices.size();
if (numplaneIndices != planes.size())
{
planeIndicesIter = planeIndices.begin();
for (k=0; k < numplaneIndices; k++)
{
if (k != *planeIndicesIter)
planes[k] = planes[*planeIndicesIter];
planeIndicesIter++;
}
planes.resize(numplaneIndices);
}
maxDistance = vertices[0].length();
for (k=1; k < vertices.size(); k++)
{
distance = vertices[k].length();
if (maxDistance < distance)
maxDistance = distance;
}
maxDistance *= btScalar(2.);
}
if (vertices.size() == 0)
continue;
// Clean-up voronoi convex shard vertices and generate edges & faces
convexHC->compute(&vertices[0].getX(), sizeof(btVector3), vertices.size(),0.0,0.0);
// At this point we have a complete 3D voronoi shard mesh contained in convexHC
// Calculate volume and center of mass (Stan Melax volume integration)
numFaces = convexHC->faces.size();
btScalar volume = btScalar(0.);
btVector3 com(0., 0., 0.);
for (j = 0; j < numFaces; j++)
{
const btConvexHullComputer::Edge* edge = &convexHC->edges[convexHC->faces[j]];
v0 = edge->getSourceVertex();
v1 = edge->getTargetVertex();
edge = edge->getNextEdgeOfFace();
v2 = edge->getTargetVertex();
while (v2 != v0)
{
// Counter-clockwise triangulated voronoi shard mesh faces (v0-v1-v2) and edges here...
btScalar vol = convexHC->vertices[v0].triple(convexHC->vertices[v1], convexHC->vertices[v2]);
volume += vol;
com += vol * (convexHC->vertices[v0] + convexHC->vertices[v1] + convexHC->vertices[v2]);
edge = edge->getNextEdgeOfFace();
v1 = v2;
v2 = edge->getTargetVertex();
}
}
com /= volume * btScalar(4.);
volume /= btScalar(6.);
// Shift all vertices relative to center of mass
int numVerts = convexHC->vertices.size();
for (j = 0; j < numVerts; j++)
{
convexHC->vertices[j] -= com;
}
// now create our output geometry with indices
std::vector<gl::Face3> outer_faces, inner_faces;
std::vector<glm::vec3> outer_vertices, inner_vertices;
int cur_outer_index = 0, cur_inner_index = 0;
for (j = 0; j < numFaces; j++)
{
const btConvexHullComputer::Edge* edge = &convexHC->edges[convexHC->faces[j]];
v0 = edge->getSourceVertex();
v1 = edge->getTargetVertex();
edge = edge->getNextEdgeOfFace();
v2 = edge->getTargetVertex();
// determine if it is an inner or outer face
btVector3 cur_plane = (convexHC->vertices[v1] - convexHC->vertices[v0]).cross(convexHC->vertices[v2]-convexHC->vertices[v0]).normalize();
cur_plane[3] = -cur_plane.dot(convexHC->vertices[v0]);
bool is_outside = false;
for(uint32_t q = 0; q < convexPlanes.size(); q++)
{
if(is_equal(convexPlanes[q], cur_plane, 0.01f)){ is_outside = true; break;}
}
std::vector<gl::Face3> *shard_faces = &outer_faces;
std::vector<glm::vec3> *shard_vertices = &outer_vertices;
int *shard_index = &cur_outer_index;
if(!is_outside)
{
shard_faces = &inner_faces;
shard_vertices = &inner_vertices;
shard_index = &cur_inner_index;
}
int face_start_index = *shard_index;
// advance index
*shard_index += 3;
// first 3 verts of n-gon
glm::vec3 tmp[] = { type_cast(convexHC->vertices[v0]),
type_cast(convexHC->vertices[v1]),
type_cast(convexHC->vertices[v2])};
shard_vertices->insert(shard_vertices->end(), tmp, tmp + 3);
shard_faces->push_back(gl::Face3(face_start_index,
face_start_index + 1,
face_start_index + 2));
// add remaining triangles of face (if any)
while (true)
{
edge = edge->getNextEdgeOfFace();
v1 = v2;
v2 = edge->getTargetVertex();
// end of n-gon
if(v2 == v0) break;
shard_vertices->push_back(type_cast(convexHC->vertices[v2]));
shard_faces->push_back(gl::Face3(face_start_index,
*shard_index - 1,
*shard_index));
(*shard_index)++;
}
}
// entry construction
gl::Mesh::Entry e0, e1;
// outer entry
e0.num_vertices = outer_vertices.size();
e0.num_indices = outer_faces.size() * 3;
e0.material_index = 0;
// inner entry
e1.base_index = e0.num_indices;
e1.base_vertex = e0.num_vertices;
e1.num_vertices = inner_vertices.size();
e1.num_indices = inner_faces.size() * 3;
e1.material_index = 1;
// create gl::Mesh object for the shard
auto inner_geom = gl::Geometry::create(), outer_geom = gl::Geometry::create();
// append verts and indices
outer_geom->append_faces(outer_faces);
outer_geom->vertices() = outer_vertices;
outer_geom->compute_face_normals();
inner_geom->append_faces(inner_faces);
inner_geom->append_vertices(inner_vertices);
inner_geom->compute_face_normals();
// merge geometries
outer_geom->append_vertices(inner_geom->vertices());
outer_geom->append_normals(inner_geom->normals());
outer_geom->append_indices(inner_geom->indices());
outer_geom->faces().insert(outer_geom->faces().end(),
inner_geom->faces().begin(), inner_geom->faces().end());
outer_geom->compute_bounding_box();
auto inner_mat = gl::Material::create();
auto m = gl::Mesh::create(outer_geom, gl::Material::create());
m->entries() = {e0, e1};
m->materials().push_back(inner_mat);
m->set_position(curVoronoiPoint + type_cast(com));
// m->transform() *= glm::scale(mat4(), vec3(scale_val));
// compute projected texcoords (outside)
gl::project_texcoords(the_mesh, m);
// compute box mapped texcoords for inside vertices
auto &indices = m->geometry()->indices();
auto &vertices = m->geometry()->vertices();
// aabb
auto out_aabb = the_mesh->bounding_box();
vec3 aabb_extents = out_aabb.halfExtents() * 2.f;
uint32_t base_vertex = m->entries()[1].base_vertex;
uint32_t k = m->entries()[1].base_index, kl = k + m->entries()[1].num_indices;
for(;k < kl; k += 3)
{
gl::Face3 f(indices[k] + base_vertex,
indices[k] + base_vertex + 1,
indices[k] + base_vertex + 2);
// normal
const vec3 &v0 = vertices[f.a];
const vec3 &v1 = vertices[f.b];
const vec3 &v2 = vertices[f.c];
vec3 n = glm::normalize(glm::cross(v1 - v0, v2 - v0));
float abs_vals[3] = {fabsf(n[0]), fabsf(n[1]), fabsf(n[2])};
// get principal direction
int principle_axis = std::distance(abs_vals, std::max_element(abs_vals, abs_vals + 3));
// switch (principle_axis)
// {
// // X-axis
// case 0:
// //ZY plane
// m->geometry()->texCoords()[f.a] = vec2(v0.z - out_aabb.min.z / aabb_extents.z,
// v0.y - out_aabb.min.y / aabb_extents.y);
// m->geometry()->texCoords()[f.b] = vec2(v1.z - out_aabb.min.z / aabb_extents.z,
// v1.y - out_aabb.min.y / aabb_extents.y);
// m->geometry()->texCoords()[f.c] = vec2(v2.z - out_aabb.min.z / aabb_extents.z,
// v2.y - out_aabb.min.y / aabb_extents.y);
// break;
//
// // Y-axis
// case 1:
// // XZ plane
// m->geometry()->texCoords()[f.a] = vec2(v0.x - out_aabb.min.x / aabb_extents.x,
// v0.z - out_aabb.min.z / aabb_extents.z);
// m->geometry()->texCoords()[f.b] = vec2(v1.x - out_aabb.min.x / aabb_extents.x,
// v1.z - out_aabb.min.z / aabb_extents.z);
// m->geometry()->texCoords()[f.c] = vec2(v2.x - out_aabb.min.x / aabb_extents.x,
// v2.z - out_aabb.min.z / aabb_extents.z);
// break;
//
// // Z-axis
// case 2:
// //XY plane
// m->geometry()->texCoords()[f.a] = vec2(v0.x - out_aabb.min.x / aabb_extents.x,
// v0.y - out_aabb.min.y / aabb_extents.y);
// m->geometry()->texCoords()[f.b] = vec2(v1.x - out_aabb.min.x / aabb_extents.x,
// v1.y - out_aabb.min.y / aabb_extents.y);
// m->geometry()->texCoords()[f.c] = vec2(v2.x - out_aabb.min.x / aabb_extents.x,
// v2.y - out_aabb.min.y / aabb_extents.y);
// break;
//
// default:
// break;
// }
}
// push to return structure
ret.push_back({m, volume});
}
LOG_DEBUG << "Generated " << ret.size() <<" voronoi shards";
return ret;
}
int main (int argc, char *argv[])
{
Visualizer vs;
vs.viewer->removeAllPointClouds();
vs.viewer->removeCoordinateSystem();
vs.viewer->setBackgroundColor(0,0,0);
PointCloudPtr_RGB cloud(new PointCloud_RGB);
NormalCloudTPtr normals(new NormalCloudT);
//loadPointCloud_ply("data/scene0.ply", cloud);
//loadPointCloud_ply("data/scene1.ply", cloud);
//loadPointCloud_ply("data/big_table_after.ply", cloud);
loadPointCloud_ply("data/two_tables.ply", cloud);
//loadPointCloud_ply("data/hui_room.ply", cloud);
/******************detect floor and wall************************/
MyPointCloud_RGB floor_cloud;
pcl::ModelCoefficients floor_coefficients;
MyPointCloud floor_rect_cloud;
vector<MyPointCloud_RGB> wall_clouds;
std::vector<MyPointCloud> wall_rect_clouds;
PointCloudPtr_RGB remained_cloud(new PointCloud_RGB);
detect_floor_and_walls(cloud, floor_cloud, floor_coefficients, floor_rect_cloud, wall_clouds, wall_rect_clouds, remained_cloud);
if(floor_cloud.mypoints.size()>0){
Eigen::Matrix4f matrix_transform;
Eigen::Matrix4f matrix_translation_r;
Eigen::Matrix4f matrix_transform_r;
getTemTransformMatrix(floor_coefficients, floor_rect_cloud, matrix_transform, matrix_translation_r, matrix_transform_r);
PointCloudPtr_RGB filter_remained_cloud(new PointCloud_RGB);
remove_outliers(remained_cloud, floor_rect_cloud, wall_rect_clouds, matrix_transform, matrix_translation_r, matrix_transform_r, filter_remained_cloud, vs);
//vs.viewer->addPointCloud(filter_remained_cloud,"filter_remained_cloud");
PointCloudPtr_RGB new_remained_cloud(new PointCloud_RGB);
PointCloud_RGB ct;
pcl::copyPointCloud(*filter_remained_cloud,ct);
pcl::transformPointCloud (ct, *new_remained_cloud, matrix_transform);
/******************pre-segment scene************************/
std::vector<MyPointCloud> sum_support_clouds;
std::vector<MyPointCloud> sum_separation_rect_clouds;
pre_segment_scene(new_remained_cloud, sum_support_clouds, sum_separation_rect_clouds);
/******************segment scene************************/
vector<MyPointCloud> clustering_cloud;
segment_scene(new_remained_cloud, sum_support_clouds, sum_separation_rect_clouds, clustering_cloud, vs);
for(int i = 0; i < clustering_cloud.size(); i++){
PointCloudPtr_RGB clustering_color_cloud(new PointCloud_RGB);
MyPointCloud mpc=clustering_cloud.at(i);
for(int j = 0;j < mpc.mypoints.size(); j++){
Point_RGB pt;
pt.x=mpc.mypoints.at(j).x;
pt.y=mpc.mypoints.at(j).y;
pt.z=mpc.mypoints.at(j).z;
pt.r=new_color_table[i%30][0];
pt.g=new_color_table[i%30][1];
pt.b=new_color_table[i%30][2];
clustering_color_cloud->push_back(pt);
}
std::stringstream st;
st<<"clustering_color_cloud"<<i;
std::string id_str=st.str();
vs.viewer->addPointCloud(clustering_color_cloud,id_str);
}
vs.show();
}
return 0;
}
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
if ( pcl::io::loadPCDFile <pcl::PointXYZRGBA> ("/home/andrew/School/CV-3/data/tripod_3/kinect_1000.pcd",
*cloud) == -1)
{
std::cout << "Cloud reading failed." << std::endl;
return (-1);
}
std::vector<int> idx;
pcl::removeNaNFromPointCloud(*cloud, *cloud, idx);
pcl::search::Search<pcl::PointXYZRGBA>::Ptr tree =
boost::shared_ptr<pcl::search::Search<pcl::PointXYZRGBA> > (new pcl::search::KdTree<pcl::PointXYZRGBA>);
pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZRGBA, pcl::Normal> normal_estimator;
normal_estimator.setSearchMethod (tree);
normal_estimator.setInputCloud (cloud);
normal_estimator.setKSearch (100);
normal_estimator.compute (*normals);
std::cout << "PassThrough" << std::endl;
pcl::IndicesPtr indices (new std::vector <int>);
pcl::PassThrough<pcl::PointXYZRGBA> pass;
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.0, 1.0);
pass.filter (*indices);
std::cout << "Region" << std::endl;
pcl::RegionGrowingRGB<pcl::PointXYZRGBA, pcl::Normal> reg;
//reg.setMinClusterSize (50);
//reg.setMaxClusterSize (10000);
reg.setSearchMethod (tree);
reg.setNumberOfNeighbours (50);
reg.setInputCloud (cloud);
reg.setPointColorThreshold(12);
reg.setRegionColorThreshold(7);
//reg.setIndices (indices);
reg.setInputNormals (normals);
reg.setSmoothnessThreshold (3.0 / 180.0 * M_PI);
reg.setCurvatureThreshold (1.0);
std::vector <pcl::PointIndices> clusters;
reg.extract (clusters);
std::cout << "Number of clusters is equal to " << clusters.size () << std::endl;
std::cout << "First cluster has " << clusters[0].indices.size () << " points." << endl;
std::cout << "These are the indices of the points of the initial" <<
std::endl << "cloud that belong to the first cluster:" << std::endl;
int counter = 0;
while (counter < 5 || counter > clusters[0].indices.size ())
{
std::cout << clusters[0].indices[counter] << std::endl;
counter++;
}
pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();
pcl::visualization::CloudViewer viewer ("Cluster viewer");
viewer.showCloud(colored_cloud);
while (!viewer.wasStopped ())
{
}
return (0);
}
void CubatureControlVolumeSide<Scalar,ArrayPoint,ArrayWeight>::getCubature(ArrayPoint& cubPoints,
ArrayWeight& cubWeights,
ArrayPoint& cellCoords) const
{
// get array dimensions
int numCells = cellCoords.dimension(0);
int numNodesPerCell = cellCoords.dimension(1);
int spaceDim = cellCoords.dimension(2);
int numNodesPerSubCV = subCVCellTopo_->getNodeCount();
// get sub-control volume coordinates (one sub-control volume per node of primary cell)
Intrepid2::FieldContainer<Scalar> subCVCoords(numCells,numNodesPerCell,numNodesPerSubCV,spaceDim);
Intrepid2::CellTools<Scalar>::getSubCVCoords(subCVCoords,cellCoords,*(primaryCellTopo_));
// num edges per primary cell
int numEdgesPerCell = primaryCellTopo_->getEdgeCount();
// Loop over cells
for (int icell = 0; icell < numCells; icell++){
// Get subcontrol volume side midpoints and normals
int iside = 1;
int numNodesPerSide = subCVCellTopo_->getNodeCount(spaceDim-1,iside);
Intrepid2::FieldContainer<int> sideNodes(numNodesPerSide);
for (int i=0; i<numNodesPerSide; i++){
sideNodes(i) = subCVCellTopo_->getNodeMap(spaceDim-1,iside,i);
}
// Loop over primary cell nodes and get side midpoints
// In each primary cell the number of control volume side integration
// points is equal to the number of primary cell edges. In 2d the
// number of edges = number of nodes and this loop defines all side
// points. In 3d this loop computes the side points for all
// subcontrol volume sides for iside = 1. Additional code below
// computes the remaining points for particular 3d topologies.
for (int inode=0; inode < numNodesPerCell; inode++){
for(int idim=0; idim < spaceDim; idim++){
Scalar midpt = 0.0;
for (int i=0; i<numNodesPerSide; i++){
midpt += subCVCoords(icell,inode,sideNodes(i),idim);
}
cubPoints(icell,inode,idim) = midpt/numNodesPerSide;
}
}
// Map side center to reference subcell
//Intrepid2::FieldContainer<Scalar> sideCenterLocal(1,spaceDim-1);
Intrepid2::FieldContainer<double> sideCenterLocal(1,spaceDim-1);
for (int idim = 0; idim < spaceDim-1; idim++){
sideCenterLocal(0,idim) = 0.0;
}
Intrepid2::FieldContainer<Scalar> refSidePoints(1,spaceDim);
iside = 1;
Intrepid2::CellTools<Scalar>::mapToReferenceSubcell(refSidePoints,
sideCenterLocal,
spaceDim-1, iside, *(subCVCellTopo_));
// Array of cell control volume coordinates
Intrepid2::FieldContainer<Scalar> cellCVCoords(numNodesPerCell, numNodesPerSubCV, spaceDim);
for (int isubcv = 0; isubcv < numNodesPerCell; isubcv++) {
for (int inode = 0; inode < numNodesPerSubCV; inode++){
for (int idim = 0; idim < spaceDim; idim++){
cellCVCoords(isubcv,inode,idim) = subCVCoords(icell,isubcv,inode,idim);
}
}
}
// calculate Jacobian at side centers
Intrepid2::FieldContainer<Scalar> subCVsideJacobian(numNodesPerCell, 1, spaceDim, spaceDim);
Intrepid2::CellTools<Scalar>::setJacobian(subCVsideJacobian, refSidePoints, cellCVCoords, *(subCVCellTopo_));
// Get subcontrol volume side normals
Intrepid2::FieldContainer<Scalar> normals(numNodesPerCell, 1, spaceDim);
Intrepid2::CellTools<Scalar>::getPhysicalSideNormals(normals,subCVsideJacobian,iside,*(subCVCellTopo_));
for (int inode = 0; inode < numNodesPerCell; inode++) {
for (int idim = 0; idim < spaceDim; idim++){
cubWeights(icell,inode,idim) = normals(inode,0,idim)*pow(2,spaceDim-1);
}
}
if (primaryCellTopo_->getKey()==shards::Hexahedron<8>::key)
{
// first set of side midpoints and normals (above) associated with
// primary cell edges 0-7 are obtained from side 1 of the
// eight control volumes
// second set of side midpoints and normals associated with
// primary cell edges 8-11 are obtained from side 5 of the
// first four control volumes.
iside = 5;
for (int i=0; i<numNodesPerSide; i++){
sideNodes(i) = subCVCellTopo_->getNodeMap(spaceDim-1,iside,i);
}
int numExtraSides = numEdgesPerCell - numNodesPerCell;
for (int icount=0; icount < numExtraSides; icount++){
int iedge = icount + numNodesPerCell;
for(int idim=0; idim < spaceDim; idim++){
Scalar midpt = 0.0;
for (int i=0; i<numNodesPerSide; i++){
midpt += subCVCoords(icell,icount,sideNodes(i),idim)/numNodesPerSide;
}
cubPoints(icell,iedge,idim) = midpt;
}
}
// Map side center to reference subcell
iside = 5;
Intrepid2::CellTools<Scalar>::mapToReferenceSubcell(refSidePoints,
sideCenterLocal,
spaceDim-1, iside, *(subCVCellTopo_));
// calculate Jacobian at side centers
Intrepid2::CellTools<Scalar>::setJacobian(subCVsideJacobian, refSidePoints, cellCVCoords, *(subCVCellTopo_));
// Get subcontrol volume side normals
Intrepid2::CellTools<Scalar>::getPhysicalSideNormals(normals,subCVsideJacobian,iside,*(subCVCellTopo_));
for (int icount = 0; icount < numExtraSides; icount++) {
int iedge = icount + numNodesPerCell;
for (int idim = 0; idim < spaceDim; idim++){
cubWeights(icell,iedge,idim) = normals(icount,0,idim)*pow(2,spaceDim-1);
}
}
} // end if Hex
if (primaryCellTopo_->getKey()==shards::Tetrahedron<4>::key)
{
// first set of side midpoints and normals associated with
// primary cell edges 0-2 are obtained from side 1 of the
// eight control volumes (above)
// second set of side midpoints and normals associated with
// primary cell edges 3-5 are obtained from side 5 of the
// first three control volumes.
iside = 5;
for (int i=0; i<numNodesPerSide; i++){
sideNodes(i) = subCVCellTopo_->getNodeMap(spaceDim-1,iside,i);
}
for (int icount=0; icount < 3; icount++){
int iedge = icount + 3;
for(int idim=0; idim < spaceDim; idim++){
Scalar midpt = 0.0;
for (int i=0; i<numNodesPerSide; i++){
midpt += subCVCoords(icell,icount,sideNodes(i),idim)/numNodesPerSide;
}
cubPoints(icell,iedge,idim) = midpt;
}
}
// Map side center to reference subcell
iside = 5;
Intrepid2::CellTools<Scalar>::mapToReferenceSubcell(refSidePoints,
sideCenterLocal,
spaceDim-1, iside, *(subCVCellTopo_));
// calculate Jacobian at side centers
Intrepid2::CellTools<Scalar>::setJacobian(subCVsideJacobian, refSidePoints, cellCVCoords, *(subCVCellTopo_));
// Get subcontrol volume side normals
Intrepid2::CellTools<Scalar>::getPhysicalSideNormals(normals,subCVsideJacobian,iside,*(subCVCellTopo_));
for (int icount = 0; icount < 3; icount++) {
int iedge = icount + 3;
for (int idim = 0; idim < spaceDim; idim++){
cubWeights(icell,iedge,idim) = normals(icount,0,idim)*pow(2,spaceDim-1);
}
}
}// if tetrahedron
} // end loop over cells
} // end getCubature
int volumetric_knt_cuda(int argc, char **argv)
{
Timer timer;
int vol_size = vx_count * vx_size;
float half_vol_size = vol_size * 0.5f;
Eigen::Vector3i voxel_size(vx_size, vx_size, vx_size);
Eigen::Vector3i volume_size(vol_size, vol_size, vol_size);
Eigen::Vector3i voxel_count(vx_count, vx_count, vx_count);
int total_voxels = voxel_count.x() * voxel_count.y() * voxel_count.z();
std::cout << std::fixed
<< "Voxel Count : " << voxel_count.transpose() << std::endl
<< "Voxel Size : " << voxel_size.transpose() << std::endl
<< "Volume Size : " << volume_size.transpose() << std::endl
<< "Total Voxels : " << total_voxels << std::endl
<< std::endl;
timer.start();
KinectFrame knt(filepath);
timer.print_interval("Importing knt frame : ");
Eigen::Affine3f grid_affine = Eigen::Affine3f::Identity();
grid_affine.translate(Eigen::Vector3f(0, 0, half_vol_size));
grid_affine.scale(Eigen::Vector3f(1, 1, 1)); // z is negative inside of screen
Eigen::Matrix4f grid_matrix = grid_affine.matrix();
float knt_near_plane = 0.1f;
float knt_far_plane = 10240.0f;
Eigen::Matrix4f projection = perspective_matrix<float>(KINECT_V2_FOVY, KINECT_V2_DEPTH_ASPECT_RATIO, knt_near_plane, knt_far_plane);
Eigen::Matrix4f projection_inverse = projection.inverse();
Eigen::Matrix4f view_matrix = Eigen::Matrix4f::Identity();
std::vector<float4> vertices(knt.depth.size(), make_float4(0, 0, 0, 1));
std::vector<float4> normals(knt.depth.size(), make_float4(0, 0, 1, 1));
std::vector<Eigen::Vector2f> grid_voxels_params(total_voxels);
//
// setup image parameters
//
unsigned short image_width = KINECT_V2_DEPTH_WIDTH;
unsigned short image_height = image_width / aspect_ratio;
QImage img(image_width, image_height, QImage::Format::Format_RGBA8888);
img.fill(Qt::GlobalColor::gray);
uchar4* image_data = (uchar4*)img.bits();
//float4* debug_buffer = new float4[image_width * image_height];
//memset(debug_buffer, 0, image_width * image_height * sizeof(float4));
knt_cuda_setup(
vx_count, vx_size,
grid_matrix.data(),
projection.data(),
projection_inverse.data(),
*grid_voxels_params.data()->data(),
KINECT_V2_DEPTH_WIDTH,
KINECT_V2_DEPTH_HEIGHT,
KINECT_V2_DEPTH_MIN,
KINECT_V2_DEPTH_MAX,
vertices.data()[0],
normals.data()[0],
image_width,
image_height
);
timer.start();
knt_cuda_allocate();
knt_cuda_init_grid();
timer.print_interval("Allocating gpu : ");
timer.start();
knt_cuda_copy_host_to_device();
knt_cuda_copy_depth_buffer_to_device(knt.depth.data());
timer.print_interval("Copy host to device : ");
timer.start();
knt_cuda_normal_estimation();
timer.print_interval("Normal estimation : ");
timer.start();
knt_cuda_update_grid(view_matrix.data());
timer.print_interval("Update grid : ");
timer.start();
knt_cuda_grid_params_copy_device_to_host();
knt_cuda_copy_device_to_host();
timer.print_interval("Copy device to host : ");
//
// setup camera parameters
//
timer.start();
Eigen::Affine3f camera_to_world = Eigen::Affine3f::Identity();
float cam_z = -half_vol_size;
camera_to_world.scale(Eigen::Vector3f(1, 1, -1));
camera_to_world.translate(Eigen::Vector3f(half_vol_size, half_vol_size, cam_z));
Eigen::Matrix4f camera_to_world_matrix = camera_to_world.matrix();
knt_cuda_raycast(KINECT_V2_FOVY, KINECT_V2_DEPTH_ASPECT_RATIO, camera_to_world_matrix.data());
timer.print_interval("Raycast : ");
timer.start();
knt_cuda_copy_image_device_to_host(*(uchar4*)img.bits());
timer.print_interval("Copy Img to host : ");
timer.start();
knt_cuda_free();
timer.print_interval("Cleanup gpu : ");
#if 0
//memset(image_data, 0, image_width * image_height * sizeof(uchar4));
//memset(debug_buffer, 0, image_width * image_height * sizeof(float4));
Eigen::Vector3f camera_pos = camera_to_world_matrix.col(3).head<3>();
float fov_scale = (float)tan(DegToRad(KINECT_V2_FOVY * 0.5f));
float aspect_ratio = KINECT_V2_DEPTH_ASPECT_RATIO;
//
// for each pixel, trace a ray
//
timer.start();
for (int y = 0; y < image_height; ++y)
{
for (int x = 0; x < image_width; ++x)
{
// Convert from image space (in pixels) to screen space
// Screen Space along X axis = [-aspect ratio, aspect ratio]
// Screen Space along Y axis = [-1, 1]
float x_norm = (2.f * float(x) + 0.5f) / (float)image_width;
float y_norm = (2.f * float(y) + 0.5f) / (float)image_height;
Eigen::Vector3f screen_coord(
(x_norm - 1.f) * aspect_ratio * fov_scale,
(1.f - y_norm) * fov_scale,
1.0f);
Eigen::Vector3f direction;
multDirMatrix(screen_coord, camera_to_world_matrix, direction);
direction.normalize();
long voxels_zero_crossing[2] = { -1, -1 };
int hit_count = raycast_tsdf_volume<float>(
camera_pos,
direction,
voxel_count.cast<int>(),
voxel_size.cast<int>(),
grid_voxels_params,
voxels_zero_crossing);
if (hit_count > 0)
{
if (hit_count == 2)
{
float4 n = normals[y * image_width + x];
//image_data[y * image_width + x].x = 0;
//image_data[y * image_width + x].y = 128;
//image_data[y * image_width + x].z = 128;
//image_data[y * image_width + x].w = 255;
image_data[y * image_width + x].x = uchar((n.x * 0.5f + 0.5f) * 255);
image_data[y * image_width + x].y = uchar((n.y * 0.5f + 0.5f) * 255);
image_data[y * image_width + x].z = uchar((n.z * 0.5f + 0.5f) * 255);
image_data[y * image_width + x].w = 255;
}
else
{
image_data[y * image_width + x].x = 128;
image_data[y * image_width + x].y = 128;
image_data[y * image_width + x].z = 0;
image_data[y * image_width + x].w = 255;
}
}
else
{
image_data[y * image_width + x].x = 128;
image_data[y * image_width + x].y = 0;
image_data[y * image_width + x].z = 0;
image_data[y * image_width + x].w = 255;
}
}
}
timer.print_interval("Raycasting to image : ");
//export_debug_buffer("../../data/cpu_image_data_screen_coord_f4.txt", debug_buffer, image_width, image_height);
//export_image_buffer("../../data/cpu_image_data_screen_coord_uc.txt", image_data, image_width, image_height);
#else
//export_debug_buffer("../../data/gpu_image_data_screen_coord_f4.txt", debug_buffer, image_width, image_height);
//export_image_buffer("../../data/gpu_image_data_screen_coord_uc.txt", image_data, image_width, image_height);
#endif
QImage image(&image_data[0].x, image_width, image_height, QImage::Format_RGBA8888);
//image.fill(Qt::GlobalColor::black);
QApplication app(argc, argv);
QImageWidget widget;
widget.resize(KINECT_V2_DEPTH_WIDTH, KINECT_V2_DEPTH_HEIGHT);
widget.setImage(image);
widget.show();
return app.exec();
}
osg::ref_ptr<osg::Node> TerrainGrid::buildTerrain (osg::Group* parent, float chunkSize, const osg::Vec2f& chunkCenter)
{
if (chunkSize * mNumSplits > 1.f)
{
// keep splitting
osg::ref_ptr<osg::Group> group (new osg::Group);
if (parent)
parent->addChild(group);
float newChunkSize = chunkSize/2.f;
buildTerrain(group, newChunkSize, chunkCenter + osg::Vec2f(newChunkSize/2.f, newChunkSize/2.f));
buildTerrain(group, newChunkSize, chunkCenter + osg::Vec2f(newChunkSize/2.f, -newChunkSize/2.f));
buildTerrain(group, newChunkSize, chunkCenter + osg::Vec2f(-newChunkSize/2.f, newChunkSize/2.f));
buildTerrain(group, newChunkSize, chunkCenter + osg::Vec2f(-newChunkSize/2.f, -newChunkSize/2.f));
return group;
}
else
{
float minH, maxH;
if (!mStorage->getMinMaxHeights(chunkSize, chunkCenter, minH, maxH))
return NULL; // no terrain defined
osg::Vec2f worldCenter = chunkCenter*mStorage->getCellWorldSize();
osg::ref_ptr<SceneUtil::PositionAttitudeTransform> transform (new SceneUtil::PositionAttitudeTransform);
transform->setPosition(osg::Vec3f(worldCenter.x(), worldCenter.y(), 0.f));
if (parent)
parent->addChild(transform);
osg::ref_ptr<osg::Vec3Array> positions (new osg::Vec3Array);
osg::ref_ptr<osg::Vec3Array> normals (new osg::Vec3Array);
osg::ref_ptr<osg::Vec4Array> colors (new osg::Vec4Array);
osg::ref_ptr<osg::VertexBufferObject> vbo (new osg::VertexBufferObject);
positions->setVertexBufferObject(vbo);
normals->setVertexBufferObject(vbo);
colors->setVertexBufferObject(vbo);
mStorage->fillVertexBuffers(0, chunkSize, chunkCenter, positions, normals, colors);
osg::ref_ptr<osg::Geometry> geometry (new osg::Geometry);
geometry->setVertexArray(positions);
geometry->setNormalArray(normals, osg::Array::BIND_PER_VERTEX);
geometry->setColorArray(colors, osg::Array::BIND_PER_VERTEX);
geometry->setUseDisplayList(false);
geometry->setUseVertexBufferObjects(true);
geometry->addPrimitiveSet(mCache.getIndexBuffer(0));
// we already know the bounding box, so no need to let OSG compute it.
osg::Vec3f min(-0.5f*mStorage->getCellWorldSize()*chunkSize,
-0.5f*mStorage->getCellWorldSize()*chunkSize,
minH);
osg::Vec3f max (0.5f*mStorage->getCellWorldSize()*chunkSize,
0.5f*mStorage->getCellWorldSize()*chunkSize,
maxH);
osg::BoundingBox bounds(min, max);
geometry->setComputeBoundingBoxCallback(new StaticBoundingBoxCallback(bounds));
std::vector<LayerInfo> layerList;
std::vector<osg::ref_ptr<osg::Image> > blendmaps;
mStorage->getBlendmaps(chunkSize, chunkCenter, false, blendmaps, layerList);
// For compiling textures, I don't think the osgFX::Effect does it correctly
osg::ref_ptr<osg::Node> textureCompileDummy (new osg::Node);
unsigned int dummyTextureCounter = 0;
bool useShaders = mResourceSystem->getSceneManager()->getForceShaders();
if (!mResourceSystem->getSceneManager()->getClampLighting())
useShaders = true; // always use shaders when lighting is unclamped, this is to avoid lighting seams between a terrain chunk with normal maps and one without normal maps
std::vector<TextureLayer> layers;
{
OpenThreads::ScopedLock<OpenThreads::Mutex> lock(mTextureCacheMutex);
for (std::vector<LayerInfo>::const_iterator it = layerList.begin(); it != layerList.end(); ++it)
{
TextureLayer textureLayer;
textureLayer.mParallax = it->mParallax;
textureLayer.mSpecular = it->mSpecular;
osg::ref_ptr<osg::Texture2D> texture = mTextureCache[it->mDiffuseMap];
if (!texture)
{
texture = new osg::Texture2D(mResourceSystem->getImageManager()->getImage(it->mDiffuseMap));
texture->setWrap(osg::Texture::WRAP_S, osg::Texture::REPEAT);
texture->setWrap(osg::Texture::WRAP_T, osg::Texture::REPEAT);
mResourceSystem->getSceneManager()->applyFilterSettings(texture);
mTextureCache[it->mDiffuseMap] = texture;
}
textureLayer.mDiffuseMap = texture;
textureCompileDummy->getOrCreateStateSet()->setTextureAttributeAndModes(dummyTextureCounter++, texture);
if (!it->mNormalMap.empty())
{
texture = mTextureCache[it->mNormalMap];
if (!texture)
{
texture = new osg::Texture2D(mResourceSystem->getImageManager()->getImage(it->mNormalMap));
texture->setWrap(osg::Texture::WRAP_S, osg::Texture::REPEAT);
texture->setWrap(osg::Texture::WRAP_T, osg::Texture::REPEAT);
mResourceSystem->getSceneManager()->applyFilterSettings(texture);
mTextureCache[it->mNormalMap] = texture;
}
textureCompileDummy->getOrCreateStateSet()->setTextureAttributeAndModes(dummyTextureCounter++, texture);
textureLayer.mNormalMap = texture;
}
if (it->requiresShaders())
useShaders = true;
layers.push_back(textureLayer);
}
}
std::vector<osg::ref_ptr<osg::Texture2D> > blendmapTextures;
for (std::vector<osg::ref_ptr<osg::Image> >::const_iterator it = blendmaps.begin(); it != blendmaps.end(); ++it)
{
osg::ref_ptr<osg::Texture2D> texture (new osg::Texture2D);
texture->setImage(*it);
texture->setWrap(osg::Texture::WRAP_S, osg::Texture::CLAMP_TO_EDGE);
texture->setWrap(osg::Texture::WRAP_T, osg::Texture::CLAMP_TO_EDGE);
texture->setResizeNonPowerOfTwoHint(false);
blendmapTextures.push_back(texture);
textureCompileDummy->getOrCreateStateSet()->setTextureAttributeAndModes(dummyTextureCounter++, blendmapTextures.back());
}
// use texture coordinates for both texture units, the layer texture and blend texture
for (unsigned int i=0; i<2; ++i)
geometry->setTexCoordArray(i, mCache.getUVBuffer());
float blendmapScale = ESM::Land::LAND_TEXTURE_SIZE*chunkSize;
osg::ref_ptr<osgFX::Effect> effect (new Terrain::Effect(mShaderManager ? useShaders : false, mResourceSystem->getSceneManager()->getForcePerPixelLighting(), mResourceSystem->getSceneManager()->getClampLighting(),
mShaderManager, layers, blendmapTextures, blendmapScale, blendmapScale));
effect->addCullCallback(new SceneUtil::LightListCallback);
transform->addChild(effect);
osg::Node* toAttach = geometry.get();
effect->addChild(toAttach);
if (mIncrementalCompileOperation)
{
mIncrementalCompileOperation->add(toAttach);
mIncrementalCompileOperation->add(textureCompileDummy);
}
return transform;
}
}
PointViewSet PoissonFilter::run(PointViewPtr input)
{
PointViewPtr output = input->makeNew();
PointViewSet viewSet;
viewSet.insert(output);
bool logOutput = log()->getLevel() > LogLevel::Debug1;
if (logOutput)
log()->floatPrecision(8);
log()->get(LogLevel::Debug2) << "Process PoissonFilter..." << std::endl;
BOX3D buffer_bounds;
input->calculateBounds(buffer_bounds);
// convert PointView to PointNormal
typedef pcl::PointCloud<pcl::PointXYZ> Cloud;
Cloud::Ptr cloud(new Cloud);
pclsupport::PDALtoPCD(input, *cloud, buffer_bounds);
int level = log()->getLevel();
switch (level)
{
case 0:
pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS);
break;
case 1:
pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
break;
case 2:
pcl::console::setVerbosityLevel(pcl::console::L_WARN);
break;
case 3:
pcl::console::setVerbosityLevel(pcl::console::L_INFO);
break;
case 4:
pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
break;
default:
pcl::console::setVerbosityLevel(pcl::console::L_VERBOSE);
break;
}
// pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree;
pcl::search::KdTree<pcl::PointNormal>::Ptr tree2;
// Create search tree
tree.reset(new pcl::search::KdTree<pcl::PointXYZ> (false));
tree->setInputCloud(cloud);
// Normal estimation
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal> ());
n.setInputCloud(cloud);
n.setSearchMethod(tree);
n.setKSearch(20);
n.compute(*normals);
// Concatenate XYZ and normal information
pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);
// Create search tree
tree2.reset(new pcl::search::KdTree<pcl::PointNormal>);
tree2->setInputCloud(cloud_with_normals);
// initial setup
pcl::Poisson<pcl::PointNormal> p;
p.setInputCloud(cloud_with_normals);
p.setSearchMethod(tree2);
p.setDepth(m_depth);
p.setPointWeight(m_point_weight);
// create PointCloud for results
pcl::PolygonMesh grid;
p.reconstruct(grid);
Cloud::Ptr cloud_f(new Cloud);
pcl::fromPCLPointCloud2(grid.cloud, *cloud_f);
if (cloud_f->points.empty())
{
log()->get(LogLevel::Debug2) << "Filtered cloud has no points!" << std::endl;
return viewSet;
}
pclsupport::PCDtoPDAL(*cloud_f, output, buffer_bounds);
log()->get(LogLevel::Debug2) << cloud->points.size() << " before, " <<
cloud_f->points.size() << " after" << std::endl;
log()->get(LogLevel::Debug2) << output->size() << std::endl;
return viewSet;
}
inline void JacobianBasis::getJacobianGeneral(int nJacNodes, const fullMatrix<double> &gSMatX,
const fullMatrix<double> &gSMatY, const fullMatrix<double> &gSMatZ,
const fullMatrix<double> &nodesX, const fullMatrix<double> &nodesY,
const fullMatrix<double> &nodesZ, fullMatrix<double> &jacobian) const
{
switch (_dim) {
case 0 : {
const int numEl = nodesX.size2();
for (int iEl = 0; iEl < numEl; iEl++)
for (int i = 0; i < nJacNodes; i++) jacobian(i,iEl) = 1.;
break;
}
case 1 : {
const int numEl = nodesX.size2();
fullMatrix<double> dxdX(nJacNodes,numEl), dydX(nJacNodes,numEl), dzdX(nJacNodes,numEl);
gSMatX.mult(nodesX, dxdX); gSMatX.mult(nodesY, dydX); gSMatX.mult(nodesZ, dzdX);
for (int iEl = 0; iEl < numEl; iEl++) {
fullMatrix<double> nodesXYZ(numPrimMapNodes,3);
for (int i = 0; i < numPrimMapNodes; i++) {
nodesXYZ(i,0) = nodesX(i,iEl);
nodesXYZ(i,1) = nodesY(i,iEl);
nodesXYZ(i,2) = nodesZ(i,iEl);
}
fullMatrix<double> normals(2,3);
const double invScale = getPrimNormals1D(nodesXYZ,normals);
if (scaling) {
const double scale = 1./invScale;
normals(0,0) *= scale; normals(0,1) *= scale; normals(0,2) *= scale; // Faster to scale 1 normal than afterwards
}
const double &dxdY = normals(0,0), &dydY = normals(0,1), &dzdY = normals(0,2);
const double &dxdZ = normals(1,0), &dydZ = normals(1,1), &dzdZ = normals(1,2);
for (int i = 0; i < nJacNodes; i++)
jacobian(i,iEl) = calcDet3D(dxdX(i,iEl),dydX(i,iEl),dzdX(i,iEl),
dxdY,dydY,dzdY,
dxdZ,dydZ,dzdZ);
}
break;
}
case 2 : {
const int numEl = nodesX.size2();
fullMatrix<double> dxdX(nJacNodes,numEl), dydX(nJacNodes,numEl), dzdX(nJacNodes,numEl);
fullMatrix<double> dxdY(nJacNodes,numEl), dydY(nJacNodes,numEl), dzdY(nJacNodes,numEl);
gSMatX.mult(nodesX, dxdX); gSMatX.mult(nodesY, dydX); gSMatX.mult(nodesZ, dzdX);
gSMatY.mult(nodesX, dxdY); gSMatY.mult(nodesY, dydY); gSMatY.mult(nodesZ, dzdY);
for (int iEl = 0; iEl < numEl; iEl++) {
fullMatrix<double> nodesXYZ(numPrimMapNodes,3);
for (int i = 0; i < numPrimMapNodes; i++) {
nodesXYZ(i,0) = nodesX(i,iEl);
nodesXYZ(i,1) = nodesY(i,iEl);
nodesXYZ(i,2) = nodesZ(i,iEl);
}
fullMatrix<double> normal(1,3);
const double invScale = getPrimNormal2D(nodesXYZ,normal);
if (scaling) {
const double scale = 1./invScale;
normal(0,0) *= scale; normal(0,1) *= scale; normal(0,2) *= scale; // Faster to scale normal than afterwards
}
const double &dxdZ = normal(0,0), &dydZ = normal(0,1), &dzdZ = normal(0,2);
for (int i = 0; i < nJacNodes; i++)
jacobian(i,iEl) = calcDet3D(dxdX(i,iEl),dydX(i,iEl),dzdX(i,iEl),
dxdY(i,iEl),dydY(i,iEl),dzdY(i,iEl),
dxdZ,dydZ,dzdZ);
}
break;
}
case 3 : {
const int numEl = nodesX.size2();
fullMatrix<double> dxdX(nJacNodes,numEl), dydX(nJacNodes,numEl), dzdX(nJacNodes,numEl);
fullMatrix<double> dxdY(nJacNodes,numEl), dydY(nJacNodes,numEl), dzdY(nJacNodes,numEl);
fullMatrix<double> dxdZ(nJacNodes,numEl), dydZ(nJacNodes,numEl), dzdZ(nJacNodes,numEl);
gSMatX.mult(nodesX, dxdX); gSMatX.mult(nodesY, dydX); gSMatX.mult(nodesZ, dzdX);
gSMatY.mult(nodesX, dxdY); gSMatY.mult(nodesY, dydY); gSMatY.mult(nodesZ, dzdY);
gSMatZ.mult(nodesX, dxdZ); gSMatZ.mult(nodesY, dydZ); gSMatZ.mult(nodesZ, dzdZ);
for (int iEl = 0; iEl < numEl; iEl++) {
for (int i = 0; i < nJacNodes; i++)
jacobian(i,iEl) = calcDet3D(dxdX(i,iEl),dydX(i,iEl),dzdX(i,iEl),
dxdY(i,iEl),dydY(i,iEl),dzdY(i,iEl),
dxdZ(i,iEl),dydZ(i,iEl),dzdZ(i,iEl));
if (scaling) {
fullMatrix<double> nodesXYZ(numPrimMapNodes,3);
for (int i = 0; i < numPrimMapNodes; i++) {
nodesXYZ(i,0) = nodesX(i,iEl);
nodesXYZ(i,1) = nodesY(i,iEl);
nodesXYZ(i,2) = nodesZ(i,iEl);
}
const double scale = 1./getPrimJac3D(nodesXYZ);
for (int i = 0; i < nJacNodes; i++) jacobian(i,iEl) *= scale;
}
}
break;
}
}
}
inline void JacobianBasis::getJacobianGeneral(int nJacNodes, const fullMatrix<double> &gSMatX,
const fullMatrix<double> &gSMatY, const fullMatrix<double> &gSMatZ,
const fullMatrix<double> &nodesXYZ, fullVector<double> &jacobian) const
{
switch (_dim) {
case 0 : {
for (int i = 0; i < nJacNodes; i++) jacobian(i) = 1.;
break;
}
case 1 : {
fullMatrix<double> normals(2,3);
const double invScale = getPrimNormals1D(nodesXYZ,normals);
if (scaling) {
const double scale = 1./invScale;
normals(0,0) *= scale; normals(0,1) *= scale; normals(0,2) *= scale; // Faster to scale 1 normal than afterwards
}
fullMatrix<double> dxyzdX(nJacNodes,3);
gSMatX.mult(nodesXYZ, dxyzdX);
for (int i = 0; i < nJacNodes; i++) {
const double &dxdX = dxyzdX(i,0), &dydX = dxyzdX(i,1), &dzdX = dxyzdX(i,2);
const double &dxdY = normals(0,0), &dydY = normals(0,1), &dzdY = normals(0,2);
const double &dxdZ = normals(1,0), &dydZ = normals(1,1), &dzdZ = normals(1,2);
jacobian(i) = calcDet3D(dxdX,dydX,dzdX,dxdY,dydY,dzdY,dxdZ,dydZ,dzdZ);
}
break;
}
case 2 : {
fullMatrix<double> normal(1,3);
const double invScale = getPrimNormal2D(nodesXYZ,normal);
if (scaling) {
const double scale = 1./invScale;
normal(0,0) *= scale; normal(0,1) *= scale; normal(0,2) *= scale; // Faster to scale normal than afterwards
}
fullMatrix<double> dxyzdX(nJacNodes,3), dxyzdY(nJacNodes,3);
gSMatX.mult(nodesXYZ, dxyzdX);
gSMatY.mult(nodesXYZ, dxyzdY);
for (int i = 0; i < nJacNodes; i++) {
const double &dxdX = dxyzdX(i,0), &dydX = dxyzdX(i,1), &dzdX = dxyzdX(i,2);
const double &dxdY = dxyzdY(i,0), &dydY = dxyzdY(i,1), &dzdY = dxyzdY(i,2);
const double &dxdZ = normal(0,0), &dydZ = normal(0,1), &dzdZ = normal(0,2);
jacobian(i) = calcDet3D(dxdX,dydX,dzdX,dxdY,dydY,dzdY,dxdZ,dydZ,dzdZ);
}
break;
}
case 3 : {
fullMatrix<double> dum;
fullMatrix<double> dxyzdX(nJacNodes,3), dxyzdY(nJacNodes,3), dxyzdZ(nJacNodes,3);
gSMatX.mult(nodesXYZ, dxyzdX);
gSMatY.mult(nodesXYZ, dxyzdY);
gSMatZ.mult(nodesXYZ, dxyzdZ);
for (int i = 0; i < nJacNodes; i++) {
const double &dxdX = dxyzdX(i,0), &dydX = dxyzdX(i,1), &dzdX = dxyzdX(i,2);
const double &dxdY = dxyzdY(i,0), &dydY = dxyzdY(i,1), &dzdY = dxyzdY(i,2);
const double &dxdZ = dxyzdZ(i,0), &dydZ = dxyzdZ(i,1), &dzdZ = dxyzdZ(i,2);
jacobian(i) = calcDet3D(dxdX,dydX,dzdX,dxdY,dydY,dzdY,dxdZ,dydZ,dzdZ);
}
if (scaling) {
const double scale = 1./getPrimJac3D(nodesXYZ);
jacobian.scale(scale);
}
break;
}
}
}
void
vtree_color_user::compute_features( const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_matrix_map,
vt::Document &full_doc )
{
typedef pcl::PointXYZRGB nx_PointT;
typedef pcl::Normal nx_Normal;
typedef pcl::PointCloud<nx_PointT> nx_PointCloud;
typedef pcl::PointCloud<nx_Normal> nx_PointCloud_normal;
typedef pcl::PointCloud<int> nx_PointCloud_int;
typedef pcl::UniformSampling<nx_PointT> nx_Sampler;
typedef pcl::search::KdTree<nx_PointT> nx_SearchMethod;
typedef pcl::NormalEstimation<nx_PointT, nx_Normal> nx_NormalEst;
#if FEATURE == 1
typedef pcl::PFHRGBSignature250 nx_FeatureType;
typedef pcl::PFHRGBEstimation<nx_PointT, nx_Normal, nx_FeatureType> nx_FeatureEst;
#elif FEATURE == 2
typedef pcl::PPFRGBSignature nx_FeatureType;
typedef pcl::PPFRGBEstimation<nx_PointT, nx_Normal, nx_FeatureType> nx_FeatureEst;
#else
#error A valid feature definition is required!
#endif
typedef pcl::PointCloud<nx_FeatureType> nx_PointCloud_feature;
// copy the matrix into a pcl cloud
nx_PointCloud::Ptr cld_ptr(cloud_matrix_map);
// ROS_INFO("Computing features for current eigen matrix");
// for( int pt = 0; pt < cloud_matrix_map.cols(); ++pt)
// {
// nx_PointT current_point(cloud_matrix_map(0,pt), //Problem might be here
// cloud_matrix_map(1,pt),
// cloud_matrix_map(2,pt));
// cld_ptr->push_back( nx_PointT( cloud_matrix_map(0,pt), //Problem might be here
// cloud_matrix_map(1,pt),
// cloud_matrix_map(2,pt) ) );
// }
ROS_INFO("[vtree_color_user] Starting keypoint extraction...");
clock_t tic = clock();
nx_PointCloud::Ptr keypoints( new nx_PointCloud);
nx_PointCloud_int::Ptr keypoint_idx(new nx_PointCloud_int);
nx_Sampler uniform_sampling;
uniform_sampling.setInputCloud ( cld_ptr );
uniform_sampling.setRadiusSearch ( keypoint_radius_ );
uniform_sampling.compute( *keypoint_idx );
pcl::copyPointCloud ( *cld_ptr, keypoint_idx->points, *keypoints);
ROS_INFO("[vtree_color_user] No of Keypoints found %d", static_cast<int>(keypoint_idx->size()) );
ROS_INFO("[vtree_color_user] Keypoint extraction took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );
if( keypoints->empty() )
{
ROS_WARN("[vtree_color_user] No keypoints were found...");
return;
}
// Compute normals for the input cloud
ROS_INFO("[vtree_color_user] Starting normal extraction...");
tic = clock();
nx_PointCloud_normal::Ptr normals (new nx_PointCloud_normal);
nx_SearchMethod::Ptr search_method_xyz (new nx_SearchMethod);
nx_NormalEst norm_est;
norm_est.setInputCloud ( cld_ptr );
norm_est.setSearchMethod ( search_method_xyz );
norm_est.setRadiusSearch ( normal_radius_ );
norm_est.compute ( *normals );
ROS_INFO("[vtree_color_user] Normal extraction took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );
// Get features at the computed keypoints
ROS_INFO("[vtree_color_user] Starting feature computation...");
tic = clock();
nx_PointCloud_feature::Ptr features(new nx_PointCloud_feature);
nx_FeatureEst feat_est;
feat_est.setInputCloud ( keypoints );
feat_est.setSearchSurface ( cld_ptr );
feat_est.setInputNormals ( normals );
search_method_xyz.reset(new nx_SearchMethod);
feat_est.setSearchMethod ( search_method_xyz );
feat_est.setRadiusSearch ( feature_radius_ );
feat_est.compute ( *features );
ROS_INFO("[vtree_color_user] No of Features found %d", static_cast<int>(features->size()) );
ROS_INFO("[vtree_color_user] Feature computation took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );
// Rectify the historgram values to ensure they are in [0,100] and create a document
for( nx_PointCloud_feature::iterator iter = features->begin();
iter != features->end(); ++iter)
{
rectify_histogram( iter->histogram );
full_doc.push_back( tree.quantize( FeatureHist( iter->histogram ) ) );
}
}
MeshGeometry ObjLoader::LoadMesh(const std::string& path) const {
// Try opening the file for starters.
std::ifstream input(path.c_str());
std::string buffer;
unsigned int numberOfTextureCoordinates = 0;
unsigned int numberOfNormals = 0;
unsigned int numberOfTriangles = 0;
unsigned int numberOfVertices = 0;
// Create the geometry cache object
MeshGeometry output;
output.vertices = NULL;
output.indices = NULL;
if(!input.is_open()) {
// Load failed (file not found)
std::cerr << "Could not open OBJ file \"" + path + "\"!" << std::endl;
}
else {
// First we'll scan to figure out how many vertices,
// texture coordinates, normals and triangles there are
// in the file.
while(!input.eof()) {
// Read each line
std::getline(input, buffer);
if(buffer.substr(0, 2) == "vn") {
// Vertex normal
numberOfNormals++;
}
else if(buffer.substr(0, 2) == "vt") {
// Vertex texture coordinate
numberOfTextureCoordinates++;
}
else if(buffer.substr(0, 1) == "v") {
// Vertex
numberOfVertices++;
}
else if(buffer.substr(0, 1) == "f") {
// Face
// Not necessarily a triangle, so now we gotta count it.
std::vector<std::string> faceParts = SplitString(buffer, ' ');
numberOfTriangles++;
if(faceParts.size() > 4) {
// We have to make another triangle for this face. It's a quad.
numberOfTriangles++;
}
}
}
// Instantiate the arrays
std::vector<ObjVertex> vertices(numberOfVertices);
std::vector<ObjNormal> normals(std::max(1u, numberOfNormals)); // if no normals, just predefine some
std::vector<ObjTextureCoordinate> textureCoordinates(std::max(1u, numberOfTextureCoordinates));
std::vector<ObjTriangle> triangles(numberOfTriangles);
// Reset the read pointer, otherwise it will just keep shouting eof.
input.clear();
input.seekg(0, std::ios::beg);
if(!input.is_open()) {
std::cerr << "Could not reopen OBJ file for second stage load" << std::endl;
}
else {
// Keep reading through line by line and break down the format now
int normal = 0;
int textureCoordinate = 0;
int vertex = 0;
int triangle = 0;
while(!input.eof()) {
// Fetch the line
std::getline(input, buffer);
// Create a stringstream for fast searching
std::istringstream line(buffer);
if(buffer.substr(0, 2) == "vn") {
std::string temp, f1, f2, f3;
// Parse out some floats and the parameters
// Format vn nx ny nz
line >> temp >> f1 >> f2 >> f3;
float x = (float)atof(f1.c_str());
float y = (float)atof(f2.c_str());
float z = (float)atof(f3.c_str());
normals[normal].x = x;
normals[normal].y = y;
normals[normal].z = z;
normal++;
}
else if(buffer.substr(0, 2) == "vt") {
// format: vt u v
std::string temp, f1, f2;
line >> temp >> f1 >> f2;
float u = (float)atof(f1.c_str());
float v = (float)atof(f2.c_str());
textureCoordinates[textureCoordinate].u = u;
textureCoordinates[textureCoordinate].v = v;
textureCoordinate++;
}
else if(buffer.substr(0, 1) == "v") {
int HDIV_WEDGE_I1_FEM_Test01(const bool verbose) {
Teuchos::RCP<std::ostream> outStream;
Teuchos::oblackholestream bhs; // outputs nothing
if (1)// (verbose)
outStream = Teuchos::rcp(&std::cout, false);
else
outStream = Teuchos::rcp(&bhs, false);
Teuchos::oblackholestream oldFormatState;
oldFormatState.copyfmt(std::cout);
typedef typename
Kokkos::Impl::is_space<DeviceSpaceType>::host_mirror_space::execution_space HostSpaceType ;
*outStream << "DeviceSpace:: "; DeviceSpaceType::print_configuration(*outStream, false);
*outStream << "HostSpace:: "; HostSpaceType::print_configuration(*outStream, false);
*outStream
<< "===============================================================================\n"
<< "| |\n"
<< "| Unit Test (Basis_HDIV_WEDGE_I1_FEM) |\n"
<< "| |\n"
<< "| 1) Conversion of Dof tags into Dof ordinals and back |\n"
<< "| 2) Basis values for VALUE and DIV operators |\n"
<< "| |\n"
<< "| Questions? Contact Pavel Bochev ([email protected]), |\n"
<< "| Denis Ridzal ([email protected]), |\n"
<< "| Kara Peterson ([email protected]). |\n"
<< "| |\n"
<< "| Intrepid's website: http://trilinos.sandia.gov/packages/intrepid |\n"
<< "| Trilinos website: http://trilinos.sandia.gov |\n"
<< "| |\n"
<< "===============================================================================\n";
typedef Kokkos::DynRankView<ValueType,DeviceSpaceType> DynRankView;
typedef Kokkos::DynRankView<ValueType,HostSpaceType> DynRankViewHost;
#define ConstructWithLabel(obj, ...) obj(#obj, __VA_ARGS__)
const ValueType tol = tolerence();
int errorFlag = 0;
// for virtual function, value and point types are declared in the class
typedef ValueType outputValueType;
typedef ValueType pointValueType;
Basis_HDIV_WEDGE_I1_FEM<DeviceSpaceType,outputValueType,pointValueType> wedgeBasis;
*outStream
<< "\n"
<< "===============================================================================\n"
<< "| TEST 1: constructors and exceptions |\n"
<< "===============================================================================\n";
try {
ordinal_type nthrow = 0, ncatch = 0;
#ifdef HAVE_INTREPID2_DEBUG
// Define array containing the 6 vertices of the reference WEDGE and 6 other points.
DynRankView ConstructWithLabel(wedgeNodes, 12, 3);
// Generic array for the output values; needs to be properly resized depending on the operator type
const auto numFields = wedgeBasis.getCardinality();
const auto numPoints = wedgeNodes.dimension(0);
const auto spaceDim = wedgeBasis.getBaseCellTopology().getDimension();
// exception #1: GRAD cannot be applied to HDIV functions
DynRankView ConstructWithLabel(vals, numFields, numPoints, spaceDim );
INTREPID2_TEST_ERROR_EXPECTED( wedgeBasis.getValues(vals, wedgeNodes, OPERATOR_GRAD));
// exception #2: CURL cannot be applied to HDIV functions
INTREPID2_TEST_ERROR_EXPECTED( wedgeBasis.getValues(vals, wedgeNodes, OPERATOR_CURL));
// Exceptions 3-7: all bf tags/bf Ids below are wrong and should cause getDofOrdinal() and
// getDofTag() to access invalid array elements thereby causing bounds check exception
// exception #3
INTREPID2_TEST_ERROR_EXPECTED( wedgeBasis.getDofOrdinal(3,0,0));
// exception #4
INTREPID2_TEST_ERROR_EXPECTED( wedgeBasis.getDofOrdinal(1,1,1));
// exception #5
INTREPID2_TEST_ERROR_EXPECTED( wedgeBasis.getDofOrdinal(0,4,1));
// exception #6
INTREPID2_TEST_ERROR_EXPECTED( wedgeBasis.getDofTag(11));
// exception #7
INTREPID2_TEST_ERROR_EXPECTED( wedgeBasis.getDofTag(-1));
// Exceptions 8-15 test exception handling with incorrectly dimensioned input/output arrays
// exception #8: input points array must be of rank-2
DynRankView ConstructWithLabel(badPoints1, 4, 5, 3);
INTREPID2_TEST_ERROR_EXPECTED( wedgeBasis.getValues(vals, badPoints1, OPERATOR_VALUE));
// exception #9 dimension 1 in the input point array must equal space dimension of the cell
DynRankView ConstructWithLabel(badPoints2, 4, 2);
INTREPID2_TEST_ERROR_EXPECTED( wedgeBasis.getValues(vals, badPoints2, OPERATOR_VALUE));
// exception #10 output values must be of rank-3 for OPERATOR_VALUE
DynRankView ConstructWithLabel(badVals1, 4, 3);
INTREPID2_TEST_ERROR_EXPECTED( wedgeBasis.getValues(badVals1, wedgeNodes, OPERATOR_VALUE));
// exception #11 output values must be of rank-2 for OPERATOR_DIV
DynRankView ConstructWithLabel(badVals2, 4, 3, 1);
INTREPID2_TEST_ERROR_EXPECTED( wedgeBasis.getValues(badVals2, wedgeNodes, OPERATOR_DIV));
// exception #12 incorrect 0th dimension of output array (must equal number of basis functions)
DynRankView ConstructWithLabel(badVals3, wedgeBasis.getCardinality() + 1, wedgeNodes.dimension(0), 3);
INTREPID2_TEST_ERROR_EXPECTED( wedgeBasis.getValues(badVals3, wedgeNodes, OPERATOR_VALUE));
// exception #13 incorrect 0th dimension of output array (must equal number of basis functions)
DynRankView ConstructWithLabel(badVals4, wedgeBasis.getCardinality() + 1, wedgeNodes.dimension(0));
INTREPID2_TEST_ERROR_EXPECTED( wedgeBasis.getValues(badVals4, wedgeNodes, OPERATOR_DIV));
// exception #14 incorrect 1st dimension of output array (must equal number of points)
DynRankView ConstructWithLabel(badVals5, wedgeBasis.getCardinality(), wedgeNodes.dimension(0) + 1, 3);
INTREPID2_TEST_ERROR_EXPECTED( wedgeBasis.getValues(badVals5, wedgeNodes, OPERATOR_VALUE));
// exception #15 incorrect 1st dimension of output array (must equal number of points)
DynRankView ConstructWithLabel(badVals6, wedgeBasis.getCardinality(), wedgeNodes.dimension(0) + 1);
INTREPID2_TEST_ERROR_EXPECTED( wedgeBasis.getValues(badVals6, wedgeNodes, OPERATOR_DIV));
// exception #16: incorrect 2nd dimension of output array (must equal the space dimension)
DynRankView ConstructWithLabel(badVals7, wedgeBasis.getCardinality(), wedgeNodes.dimension(0), 4);
INTREPID2_TEST_ERROR_EXPECTED( wedgeBasis.getValues(badVals7, wedgeNodes, OPERATOR_VALUE));
#endif
} catch (std::logic_error err) {
*outStream << "UNEXPECTED ERROR !!! ----------------------------------------------------------\n";
*outStream << err.what() << '\n';
*outStream << "-------------------------------------------------------------------------------" << "\n\n";
errorFlag = -1000;
}
*outStream
<< "\n"
<< "===============================================================================\n"
<< "| TEST 2: correctness of tag to enum and enum to tag lookups |\n"
<< "===============================================================================\n";
try{
const auto numFields = wedgeBasis.getCardinality();
const auto allTags = wedgeBasis.getAllDofTags();
// Loop over all tags, lookup the associated dof enumeration and then lookup the tag again
const auto dofTagSize = allTags.dimension(0);
// Loop over all tags, lookup the associated dof enumeration and then lookup the tag again
for (size_type i = 0; i < dofTagSize; i++) {
const auto bfOrd = wedgeBasis.getDofOrdinal(allTags(i, 0), allTags(i, 1), allTags(i, 2));
const auto myTag = wedgeBasis.getDofTag(bfOrd);
if( !( (myTag(0) == allTags(i, 0)) &&
(myTag(1) == allTags(i, 1)) &&
(myTag(2) == allTags(i, 2)) &&
(myTag(3) == allTags(i, 3)) ) ) {
errorFlag++;
*outStream << std::setw(70) << "^^^^----FAILURE!" << "\n";
*outStream << " getDofOrdinal( {"
<< allTags(i, 0) << ", "
<< allTags(i, 1) << ", "
<< allTags(i, 2) << ", "
<< allTags(i, 3) << "}) = " << bfOrd <<" but \n";
*outStream << " getDofTag(" << bfOrd << ") = { "
<< myTag(0) << ", "
<< myTag(1) << ", "
<< myTag(2) << ", "
<< myTag(3) << "}\n";
}
}
// Now do the same but loop over basis functions
for( int bfOrd = 0; bfOrd <numFields; bfOrd++) {
const auto myTag = wedgeBasis.getDofTag(bfOrd);
const auto myBfOrd = wedgeBasis.getDofOrdinal(myTag(0), myTag(1), myTag(2));
if( bfOrd != myBfOrd) {
errorFlag++;
*outStream << std::setw(70) << "^^^^----FAILURE!" << "\n";
*outStream << " getDofTag(" << bfOrd << ") = { "
<< myTag(0) << ", "
<< myTag(1) << ", "
<< myTag(2) << ", "
<< myTag(3) << "} but getDofOrdinal({"
<< myTag(0) << ", "
<< myTag(1) << ", "
<< myTag(2) << ", "
<< myTag(3) << "} ) = " << myBfOrd << "\n";
}
}
}
catch (std::logic_error err){
*outStream << err.what() << "\n\n";
errorFlag = -1000;
};
*outStream \
<< "\n"
<< "===============================================================================\n"\
<< "| TEST 3: correctness of basis function values |\n"\
<< "===============================================================================\n";
outStream -> precision(20);
// VALUE: Each row pair gives the 5x3 correct basis set values at an evaluation point
double basisValues[] = {
0, -0.500000, 0, 0, 0, 0, -0.500000, 0, 0, 0, 0, -2.00000, 0, 0, 0, \
0.500000, -0.500000, 0, 0.500000, 0, 0, 0, 0, 0, 0, 0, -2.00000, 0, \
0, 0, 0, 0, 0, 0, 0.500000, 0, -0.500000, 0.500000, 0, 0, 0, \
-2.00000, 0, 0, 0, 0, -0.500000, 0, 0, 0, 0, -0.500000, 0, 0, 0, 0, \
0, 0, 0, 2.00000, 0.500000, -0.500000, 0, 0.500000, 0, 0, 0, 0, 0, 0, \
0, 0, 0, 0, 2.00000, 0, 0, 0, 0, 0.500000, 0, -0.500000, 0.500000, 0, \
0, 0, 0, 0, 0, 2.00000, 0.125000, -0.250000, 0, 0.125000, 0.250000, \
0, -0.375000, 0.250000, 0, 0, 0, -2.00000, 0, 0, 0, 0.250000, \
-0.375000, 0, 0.250000, 0.125000, 0, -0.250000, 0.125000, 0, 0, 0, \
-1.00000, 0, 0, 1.00000, 0.125000, -0.375000, 0, 0.125000, 0.125000, \
0, -0.375000, 0.125000, 0, 0, 0, 0, 0, 0, 2.00000, 0.125000, \
-0.500000, 0, 0.125000, 0, 0, -0.375000, 0, 0, 0, 0, -0.250000, 0, 0, \
1.75000, 0, -0.250000, 0, 0, 0.250000, 0, -0.500000, 0.250000, 0, 0, \
0, -1.25000, 0, 0, 0.750000, 0.250000, -0.250000, 0, 0.250000, \
0.250000, 0, -0.250000, 0.250000, 0, 0, 0, -1.00000, 0, 0, 1.00000};
// DIV: each row pair gives the 5 correct values of the divergence of the 5 basis functions
double basisDivs[] = {
// 6 vertices
1.0, 1.0, 1.0, 1.0, 1.0,
1.0, 1.0, 1.0, 1.0, 1.0,
1.0, 1.0, 1.0, 1.0, 1.0,
1.0, 1.0, 1.0, 1.0, 1.0,
1.0, 1.0, 1.0, 1.0, 1.0,
1.0, 1.0, 1.0, 1.0, 1.0,
// 6 other points
1.0, 1.0, 1.0, 1.0, 1.0,
1.0, 1.0, 1.0, 1.0, 1.0,
1.0, 1.0, 1.0, 1.0, 1.0,
1.0, 1.0, 1.0, 1.0, 1.0,
1.0, 1.0, 1.0, 1.0, 1.0,
1.0, 1.0, 1.0, 1.0, 1.0
};
try{
DynRankViewHost ConstructWithLabel(wedgeNodesHost, 12, 3);
wedgeNodesHost(0,0) = 0.0; wedgeNodesHost(0,1) = 0.0; wedgeNodesHost(0,2) = -1.0;
wedgeNodesHost(1,0) = 1.0; wedgeNodesHost(1,1) = 0.0; wedgeNodesHost(1,2) = -1.0;
wedgeNodesHost(2,0) = 0.0; wedgeNodesHost(2,1) = 1.0; wedgeNodesHost(2,2) = -1.0;
wedgeNodesHost(3,0) = 0.0; wedgeNodesHost(3,1) = 0.0; wedgeNodesHost(3,2) = 1.0;
wedgeNodesHost(4,0) = 1.0; wedgeNodesHost(4,1) = 0.0; wedgeNodesHost(4,2) = 1.0;
wedgeNodesHost(5,0) = 0.0; wedgeNodesHost(5,1) = 1.0; wedgeNodesHost(5,2) = 1.0;
wedgeNodesHost(6,0) = 0.25; wedgeNodesHost(6,1) = 0.5; wedgeNodesHost(6,2) = -1.0;
wedgeNodesHost(7,0) = 0.5; wedgeNodesHost(7,1) = 0.25; wedgeNodesHost(7,2) = 0.0;
wedgeNodesHost(8,0) = 0.25; wedgeNodesHost(8,1) = 0.25; wedgeNodesHost(8,2) = 1.0;
wedgeNodesHost(9,0) = 0.25; wedgeNodesHost(9,1) = 0.0; wedgeNodesHost(9,2) = 0.75;
wedgeNodesHost(10,0)= 0.0; wedgeNodesHost(10,1)= 0.5; wedgeNodesHost(10,2)= -0.25;
wedgeNodesHost(11,0)= 0.5; wedgeNodesHost(11,1)= 0.5; wedgeNodesHost(11,2)= 0.0;
const auto wedgeNodes = Kokkos::create_mirror_view(typename DeviceSpaceType::memory_space(), wedgeNodesHost);
Kokkos::deep_copy(wedgeNodes, wedgeNodesHost);
// Dimensions for the output arrays:
const auto numFields = wedgeBasis.getCardinality();
const auto numPoints = wedgeNodes.dimension(0);
const auto spaceDim = wedgeBasis.getBaseCellTopology().getDimension();
// Check VALUE of basis functions: resize vals to rank-3 container:
{
DynRankView ConstructWithLabel(vals, numFields, numPoints, spaceDim);
wedgeBasis.getValues(vals, wedgeNodes, OPERATOR_VALUE);
const auto vals_host = Kokkos::create_mirror_view(typename HostSpaceType::memory_space(), vals);
Kokkos::deep_copy(vals_host, vals);
for (int i = 0; i < numFields; i++) {
for (size_type j = 0; j < numPoints; j++) {
for (size_type k = 0; k < spaceDim; k++) {
int l = k + i * spaceDim + j * spaceDim * numFields;
if (std::abs(vals_host(i,j,k) - basisValues[l]) > tol) {
errorFlag++;
*outStream << std::setw(70) << "^^^^----FAILURE!" << "\n";
// Output the multi-index of the value where the error is:
*outStream << " At multi-index { ";
*outStream << i << " ";*outStream << j << " ";*outStream << k << " ";
*outStream << "} computed value: " << vals_host(i,j,k)
<< " but reference value: " << basisValues[l] << "\n";
}
}
}
}
}
// Check DIV of basis function: resize vals to rank-2 container
{
DynRankView ConstructWithLabel(vals, numFields, numPoints);
wedgeBasis.getValues(vals, wedgeNodes, OPERATOR_DIV);
const auto vals_host = Kokkos::create_mirror_view(typename HostSpaceType::memory_space(), vals);
Kokkos::deep_copy(vals_host, vals);
for (int i = 0; i < numFields; i++) {
for (size_type j = 0; j < numPoints; j++) {
int l = i + j * numFields;
if (std::abs(vals_host(i,j) - basisDivs[l]) > tol) {
errorFlag++;
*outStream << std::setw(70) << "^^^^----FAILURE!" << "\n";
// Output the multi-index of the value where the error is:
*outStream << " At multi-index { ";
*outStream << i << " ";*outStream << j << " ";
*outStream << "} computed divergence component: " << vals_host(i,j)
<< " but reference divergence component: " << basisDivs[l] << "\n";
}
}
}
}
} catch (std::logic_error err) {
*outStream << err.what() << "\n\n";
errorFlag = -1000;
}
*outStream
<< "\n"
<< "===============================================================================\n"
<< "| TEST 4: correctness of DoF locations |\n"
<< "===============================================================================\n";
try {
const auto numFields = wedgeBasis.getCardinality();
const auto spaceDim = wedgeBasis.getBaseCellTopology().getDimension();
// Check exceptions.
ordinal_type nthrow = 0, ncatch = 0;
#ifdef HAVE_INTREPID2_DEBUG
{
DynRankView ConstructWithLabel(badVals, 1,2,3);
INTREPID2_TEST_ERROR_EXPECTED( wedgeBasis.getDofCoords(badVals) );
}
{
DynRankView ConstructWithLabel(badVals, 4,3);
INTREPID2_TEST_ERROR_EXPECTED( wedgeBasis.getDofCoords(badVals) );
}
{
DynRankView ConstructWithLabel(badVals, 5,2);
INTREPID2_TEST_ERROR_EXPECTED( wedgeBasis.getDofCoords(badVals) );
}
#endif
if (nthrow != ncatch) {
errorFlag++;
*outStream << std::setw(70) << "^^^^----FAILURE!" << "\n";
*outStream << "# of catch ("<< ncatch << ") is different from # of throw (" << ncatch << ")\n";
}
DynRankView ConstructWithLabel(bvals, numFields, numFields, spaceDim);
DynRankView ConstructWithLabel(cvals, numFields, spaceDim);
// Check mathematical correctness.
wedgeBasis.getDofCoords(cvals);
wedgeBasis.getValues(bvals, cvals, OPERATOR_VALUE);
// Check mathematical correctness
DynRankViewHost ConstructWithLabel(normals, numFields,spaceDim); // normals at each point basis point
normals(0,0) = 0.0; normals(0,1) = -2.0; normals(0,2) = 0.0;
normals(1,0) = 2.0; normals(1,1) = 2.0; normals(1,2) = 0.0;
normals(2,0) = -2.0; normals(2,1) = 0.0; normals(2,2) = 0.0;
normals(3,0) = 0.0; normals(3,1) = 0.0; normals(3,2) = -0.5;
normals(4,0) = 0.0; normals(4,1) = 0.0; normals(4,2) = 0.5;
auto cvals_host = Kokkos::create_mirror_view(typename HostSpaceType::memory_space(), cvals);
Kokkos::deep_copy(cvals_host, cvals);
auto bvals_host = Kokkos::create_mirror_view(typename HostSpaceType::memory_space(), bvals);
Kokkos::deep_copy(bvals_host, bvals);
for (ordinal_type i=0;i<numFields;++i) {
for (ordinal_type j=0;j<numFields;++j) {
ValueType normal = 0.0;
for(size_type d=0;d<spaceDim;++d) {
normal += bvals_host(i,j,d)*normals(j,d);
}
const ValueType expected_normal = (i == j);
if (std::abs(normal - expected_normal) > tol || std::isnan(normal)) {
errorFlag++;
std::stringstream ss;
ss << "\nNormal component of basis function " << i << " at (" << cvals_host(j,0) << ", " << cvals_host(j,1)<< ", " << cvals_host(j,2) << ") is " << normal << " but should be " << expected_normal << "\n";
*outStream << ss.str();
}
}
}
} catch (std::logic_error err) {
*outStream << err.what() << "\n\n";
errorFlag = -1000;
}
if (errorFlag != 0)
std::cout << "End Result: TEST FAILED\n";
else
std::cout << "End Result: TEST PASSED\n";
// reset format state of std::cout
std::cout.copyfmt(oldFormatState);
return errorFlag;
}
int MLSSmoothingUpsampling::compute()
{
//pointer to selected cloud
ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
if (!cloud)
return -1;
//get xyz in PCL format
cc2smReader converter;
converter.setInputCloud(cloud);
//take out the xyz info
PCLCloud sm_xyz = converter.getXYZ();
PCLCloud sm_cloud;
//take out the current scalar field (if any)
if (cloud->getCurrentDisplayedScalarField())
{
const char* current_sf_name = cloud->getCurrentDisplayedScalarField()->getName();
PCLCloud sm_field = converter.getFloatScalarField(current_sf_name);
//change its name
std::string new_name = "scalar";
sm_field.fields[0].name = new_name.c_str();
//put everithing together
pcl::concatenateFields(sm_xyz, sm_field, sm_cloud);
}
else
{
sm_cloud = sm_xyz;
}
//get as pcl point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>);
FROM_PCL_CLOUD(sm_cloud, *pcl_cloud);
//create storage for outcloud
pcl::PointCloud<pcl::PointNormal>::Ptr normals (new pcl::PointCloud<pcl::PointNormal>);
#ifdef LP_PCL_PATCH_ENABLED
pcl::PointIndicesPtr mapping_indices;
smooth_mls<pcl::PointXYZ, pcl::PointNormal> (pcl_cloud, *m_parameters, normals, mapping_indices);
#else
smooth_mls<pcl::PointXYZ, pcl::PointNormal> (pcl_cloud, *m_parameters, normals);
#endif
PCLCloud::Ptr sm_normals (new PCLCloud);
TO_PCL_CLOUD(*normals, *sm_normals);
ccPointCloud* new_cloud = sm2ccConverter(sm_normals).getCCloud();
if (!new_cloud)
{
//conversion failed (not enough memory?)
return -1;
}
new_cloud->setName(cloud->getName()+QString("_smoothed")); //original name + suffix
new_cloud->setDisplay(cloud->getDisplay());
#ifdef LP_PCL_PATCH_ENABLED
//copy the original scalar fields here
copyScalarFields(cloud, new_cloud, mapping_indices, true);
//copy the original colors here
copyRGBColors(cloud, new_cloud, mapping_indices, true);
#endif
//disable original cloud
cloud->setEnabled(false);
if (cloud->getParent())
cloud->getParent()->addChild(new_cloud);
emit newEntity(new_cloud);
return 1;
}
