i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +08:00
Code Issues Packages Projects Releases Wiki Activity

Export dialog: hiding not used options, exporting can be now canceled on texturing phase

Browse Source
This commit is contained in:
matlabbe
2017-02-26 18:17:00 -05:00
parent 8ab609471a
commit 2565239a1b
11 changed files with 863 additions and 436 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
corelib/include/rtabmap/core/ProgressState.h Normal file
View File

@@ -0,0 +1,43 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef CORELIB_INCLUDE_RTABMAP_CORE_PROGRESSSTATE_H_
#define CORELIB_INCLUDE_RTABMAP_CORE_PROGRESSSTATE_H_
class ProgressState
{
public:
virtual bool callback(const std::string & msg) const
{
return true;
}
virtual ~ProgressState(){}
};
#endif /* CORELIB_INCLUDE_RTABMAP_CORE_PROGRESSSTATE_H_ */

4
corelib/include/rtabmap/core/util3d_surface.h
View File

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/pcl_base.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/CameraModel.h>
#include <rtabmap/core/ProgressState.h>
#include <set>
#include <list>
@@ -136,7 +137,8 @@ pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh(
const pcl::PolygonMesh::Ptr & mesh,
const std::map<int, Transform> & poses,
const std::map<int, CameraModel> & cameraModels,
float maxDistance = 0.0f); // max camera distance to polygon to apply texture
float maxDistance = 0.0f, // max camera distance to polygon to apply texture
const ProgressState * state = 0);
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,

4
corelib/src/GainCompensator.cpp
View File

@@ -305,11 +305,11 @@ void feedImpl(
gains = cv::Mat_<double>();
cv::solve(A, b, gains);
//if(ULogger::kDebug)
if(ULogger::kInfo)
{
for(int i=0; i<gains.rows; ++i)
{
UWARN("Gain index=%d (id=%d) = %f", i, indexToId[i], gains(i, 0));
UINFO("Gain index=%d (id=%d) = %f", i, indexToId[i], gains(i, 0));
}
}
}

142
corelib/src/pcl18/surface/impl/texture_mapping.hpp
View File

@@ -40,6 +40,7 @@
#include <pcl/common/distances.h>
#include <pcl18/surface/texture_mapping.h>
#include <pcl/search/octree.h>
///////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT> std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >
@@ -1012,17 +1013,20 @@ class FaceInfo
{
public:
FaceInfo(float d,
bool facingCam,
const pcl::PointXY & uv1,
const pcl::PointXY & uv2,
const pcl::PointXY & uv3,
const pcl::PointXY & center) :
distance(d),
facingTheCam(facingCam),
uv_coord1(uv1),
uv_coord2(uv2),
uv_coord3(uv3),
uv_center(center)
{}
float distance;
bool facingTheCam;
pcl::PointXY uv_coord1;
pcl::PointXY uv_coord2;
pcl::PointXY uv_coord3;
@@ -1039,12 +1043,15 @@ bool ptInTriangle(const pcl::PointXY & p0, const pcl::PointXY & p1, const pcl::P
}
///////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT> void
pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (pcl::TextureMesh &mesh, const pcl::texture_mapping::CameraVector &cameras)
template<typename PointInT> bool
pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
pcl::TextureMesh &mesh,
const pcl::texture_mapping::CameraVector &cameras,
const ProgressState * state)
{
if (mesh.tex_polygons.size () != 1)
return;
return false;
typename pcl::PointCloud<PointInT>::Ptr mesh_cloud (new pcl::PointCloud<PointInT>);
@@ -1061,10 +1068,10 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (pcl::TextureMesh
// pre compute all cam inverse and visibility
std::vector<std::map<int, FaceInfo > > visibleFaces(cameras.size());
std::vector<Eigen::Affine3f> invCamTransform(cameras.size());
UDEBUG("Precompute visible faces per cam");
UINFO("Precompute visible faces per cam (%d faces, %d cams)", (int)faces.size(), (int)cameras.size());
for (unsigned int current_cam = 0; current_cam < cameras.size(); ++current_cam)
{
UINFO("Processing camera %d...", current_cam);
UDEBUG("Texture camera %d...", current_cam);
typename pcl::PointCloud<PointInT>::Ptr camera_cloud (new pcl::PointCloud<PointInT>);
pcl::transformPointCloud(*mesh_cloud, *camera_cloud, cameras[current_cam].pose.inverse());
@@ -1072,36 +1079,53 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (pcl::TextureMesh
std::vector<int> visibilityIndices;
visibilityIndices.resize (faces.size ());
pcl::PointCloud<pcl::PointXY>::Ptr projections (new pcl::PointCloud<pcl::PointXY>);
projections->resize(faces.size()*3);
std::map<float, int> sortedVisibleFaces;
int oi=0;
for(unsigned int idx_face=0; idx_face<faces.size(); ++idx_face)
{
pcl::Vertices & face = faces[idx_face];
pcl::PointXY uv_coords[3];
int j=oi*3;
pcl::PointXY & uv_coords1 = projections->at(j);
pcl::PointXY & uv_coords2 = projections->at(j+1);
pcl::PointXY & uv_coords3 = projections->at(j+2);
PointInT & pt0 = camera_cloud->points[face.vertices[0]];
PointInT & pt1 = camera_cloud->points[face.vertices[1]];
PointInT & pt2 = camera_cloud->points[face.vertices[2]];
float angle;
if (isFaceProjected (cameras[current_cam],
pt0,
pt1,
pt2,
uv_coords[0],
uv_coords[1],
uv_coords[2],
angle))
uv_coords1,
uv_coords2,
uv_coords3))
{
// check if the polygon is facing the camera, assuming counterclockwise normal
Eigen::Vector3f v0(
pt1.x - pt0.x,
pt1.y - pt0.y,
pt1.z - pt0.z);
Eigen::Vector3f v1(
pt2.x - pt0.x,
pt2.y - pt0.y,
pt2.z - pt0.z);
Eigen::Vector3f normal = v0.cross(v1);
float angle = normal.dot(Eigen::Vector3f(0.0f,0.0f,-1.0f));
bool facingTheCam = angle>0.0f;
float distanceToCam = std::min(std::min(pt0.z, pt1.z), pt2.z);
pcl::PointXY center;
center.x = (uv_coords[0].x+uv_coords[1].x+uv_coords[2].x)/3.0f;
center.y = (uv_coords[0].y+uv_coords[1].y+uv_coords[2].y)/3.0f;
visibleFaces[current_cam].insert(std::make_pair(idx_face, FaceInfo(distanceToCam, uv_coords[0], uv_coords[1], uv_coords[2], center)));
visibilityIndices[oi++] = idx_face;
projections->push_back(uv_coords[0]);
projections->push_back(uv_coords[1]);
projections->push_back(uv_coords[2]);
center.x = (uv_coords1.x+uv_coords2.x+uv_coords3.x)/3.0f;
center.y = (uv_coords1.y+uv_coords2.y+uv_coords3.y)/3.0f;
visibleFaces[current_cam].insert(visibleFaces[current_cam].end(), std::make_pair(idx_face, FaceInfo(distanceToCam, facingTheCam, uv_coords1, uv_coords2, uv_coords3, center)));
sortedVisibleFaces.insert(std::make_pair(distanceToCam, idx_face));
visibilityIndices[oi] = idx_face;
++oi;
}
}
visibilityIndices.resize(oi);
projections->resize(oi*3);
UASSERT(projections->size() == visibilityIndices.size()*3);
//filter occluded polygons
@@ -1115,9 +1139,11 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (pcl::TextureMesh
// then (idx_pcam == current_cam), check for self occlusions. At this stage, we skip faces that were already marked as occluded
// project all faces
int occludedFaces = 0;
for (unsigned int idx_vis = 0; idx_vis < visibilityIndices.size(); ++idx_vis)
for (std::map<float, int>::iterator jter=sortedVisibleFaces.begin(); jter!=sortedVisibleFaces.end(); ++jter)
//for (unsigned int idx = 0; idx<visibilityIndices.size(); ++idx)
{
int idx_face = visibilityIndices[idx_vis];
int idx_face = jter->second;
//int idx_face = visibilityIndices[idx];
std::map<int, FaceInfo>::iterator iter= visibleFaces[current_cam].find(idx_face);
if(iter != visibleFaces[current_cam].end())
{
@@ -1210,13 +1236,36 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (pcl::TextureMesh
}
}
UDEBUG("Filtered %d occluded and %d spurious polygons out of %d...", occludedFaces, clusterFaces, (int)visibilityIndices.size());
std::string msg = uFormat("Processed camera %d/%d: %d occluded and %d spurious polygons out of %d", (int)current_cam+1, (int)cameras.size(), occludedFaces, clusterFaces, (int)visibilityIndices.size());
UINFO(msg.c_str());
if(state && !state->callback(msg))
{
//cancelled!
UWARN("Texturing cancelled!");
return false;
}
}
UDEBUG("Process %d polygons...", (int)faces.size());
std::string msg = uFormat("Texturing %d polygons...", (int)faces.size());
UINFO(msg.c_str());
if(state && !state->callback(msg))
{
//cancelled!
UWARN("Texturing cancelled!");
return false;
}
for(unsigned int idx_face=0; idx_face<faces.size(); ++idx_face)
{
UDEBUG("face %d", idx_face);
if((idx_face+1)%1000 == 0)
{
UDEBUG("face %d/%d", idx_face+1, (int)faces.size());
if(state && !state->callback(""))
{
//cancelled!
UWARN("Texturing cancelled!");
return false;
}
}
pcl::Vertices & face = faces[idx_face];
int cameraIndex = -1;
@@ -1225,7 +1274,7 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (pcl::TextureMesh
for (unsigned int current_cam = 0; current_cam < cameras.size(); ++current_cam)
{
std::map<int, FaceInfo>::iterator iter = visibleFaces[current_cam].find(idx_face);
if (iter != visibleFaces[current_cam].end())
if (iter != visibleFaces[current_cam].end() && iter->second.facingTheCam)
{
float distanceToCam = iter->second.distance;
@@ -1244,35 +1293,21 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (pcl::TextureMesh
if(cameraIndex >= 0)
{
if(mesh.tex_polygons[cameraIndex].capacity() < mesh.tex_polygons[cameraIndex].size()+1)
{
mesh.tex_polygons[cameraIndex].reserve(mesh.tex_polygons[cameraIndex].size()+10);
}
mesh.tex_polygons[cameraIndex].push_back(face);
if(mesh.tex_coordinates[cameraIndex].capacity() < mesh.tex_coordinates[cameraIndex].size()+3)
{
mesh.tex_coordinates[cameraIndex].reserve(mesh.tex_coordinates[cameraIndex].size()+30);
}
mesh.tex_coordinates[cameraIndex].push_back(Eigen::Vector2f(uv_coords[0].x, uv_coords[0].y));
mesh.tex_coordinates[cameraIndex].push_back(Eigen::Vector2f(uv_coords[1].x, uv_coords[1].y));
mesh.tex_coordinates[cameraIndex].push_back(Eigen::Vector2f(uv_coords[2].x, uv_coords[2].y));
}
else
{
if(mesh.tex_polygons[cameras.size()].capacity() < mesh.tex_polygons[cameras.size()].size()+1)
{
mesh.tex_polygons[cameras.size()].reserve(mesh.tex_polygons[cameras.size()].size()+10);
}
mesh.tex_polygons[cameras.size()].push_back(face);
if(mesh.tex_coordinates[cameras.size()].capacity() < mesh.tex_coordinates[cameras.size()].size()+3)
{
mesh.tex_coordinates[cameras.size()].reserve(mesh.tex_coordinates[cameras.size()].size()+30);
}
mesh.tex_coordinates[cameras.size()].push_back(Eigen::Vector2f(-1.0,-1.0));
mesh.tex_coordinates[cameras.size()].push_back(Eigen::Vector2f(-1.0,-1.0));
mesh.tex_coordinates[cameras.size()].push_back(Eigen::Vector2f(-1.0,-1.0));
}
}
UINFO("Process %d polygons...done!", (int)faces.size());
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////
@@ -1396,28 +1431,13 @@ pcl::TextureMapping<PointInT>::checkPointInsideTriangle(const pcl::PointXY &p1,
///////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT> inline bool
pcl::TextureMapping<PointInT>::isFaceProjected (const Camera &camera, const PointInT &p1, const PointInT &p2, const PointInT &p3, pcl::PointXY &proj1, pcl::PointXY &proj2, pcl::PointXY &proj3, float & angle)
pcl::TextureMapping<PointInT>::isFaceProjected (const Camera &camera, const PointInT &p1, const PointInT &p2, const PointInT &p3, pcl::PointXY &proj1, pcl::PointXY &proj2, pcl::PointXY &proj3)
{
// check if the polygon is facing the camera, assuming counterclockwise normal
Eigen::Vector3f v0(
p2.x - p1.x,
p2.y - p1.y,
p2.z - p1.z);
Eigen::Vector3f v1(
p3.x - p1.x,
p3.y - p1.y,
p3.z - p1.z);
Eigen::Vector3f normal = v0.cross(v1);
normal.normalize();
angle = normal.dot(Eigen::Vector3f(0.0f,0.0f,-1.0f));
return angle > 0.0f && // toward the camera
(getPointUVCoordinates(p1, camera, proj1)
&&
getPointUVCoordinates(p2, camera, proj2)
&&
getPointUVCoordinates(p3, camera, proj3)
);
return getPointUVCoordinates(p1, camera, proj1)
&&
getPointUVCoordinates(p2, camera, proj2)
&&
getPointUVCoordinates(p3, camera, proj3);
}
#define PCL_INSTANTIATE_TextureMapping(T) \

8
corelib/src/pcl18/surface/texture_mapping.h
View File

@@ -43,6 +43,7 @@
#include <pcl/surface/reconstruction.h>
#include <pcl/common/transforms.h>
#include <pcl/TextureMesh.h>
#include <rtabmap/core/ProgressState.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UConversion.h>
@@ -341,9 +342,10 @@ namespace pcl
void
textureMeshwithMultipleCameras (pcl::TextureMesh &mesh,
const pcl::texture_mapping::CameraVector &cameras);
void
bool
textureMeshwithMultipleCameras2 (pcl::TextureMesh &mesh,
const pcl::texture_mapping::CameraVector &cameras);
const pcl::texture_mapping::CameraVector &cameras,
const ProgressState * callback = 0);
protected:
/** \brief mesh scale control. */
@@ -414,7 +416,7 @@ namespace pcl
inline bool
isFaceProjected (const Camera &camera,
const PointInT &p1, const PointInT &p2, const PointInT &p3,
pcl::PointXY &proj1, pcl::PointXY &proj2, pcl::PointXY &proj3, float & angle);
pcl::PointXY &proj1, pcl::PointXY &proj2, pcl::PointXY &proj3);
/** \brief Returns True if a point lays within a triangle
* \details see http://www.blackpawn.com/texts/pointinpoly/default.html

153
corelib/src/util3d_surface.cpp
View File

@@ -607,7 +607,8 @@ pcl::TextureMesh::Ptr createTextureMesh(
const pcl::PolygonMesh::Ptr & mesh,
const std::map<int, Transform> & poses,
const std::map<int, CameraModel> & cameraModels,
float maxDistance)
float maxDistance,
const ProgressState * state)
{
UASSERT(mesh->polygons.size());
pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
@@ -664,87 +665,87 @@ pcl::TextureMesh::Ptr createTextureMesh(
// Texture by projection
pcl::TextureMapping<pcl::PointXYZ> tm; // TextureMapping object that will perform the sort
tm.setMaxDistance(maxDistance);
tm.textureMeshwithMultipleCameras2(*textureMesh, cameras);
// compute normals for the mesh if not already here
bool hasNormals = false;
bool hasColors = false;
for(unsigned int i=0; i<textureMesh->cloud.fields.size(); ++i)
if(tm.textureMeshwithMultipleCameras2(*textureMesh, cameras, state))
{
if(textureMesh->cloud.fields[i].name.compare("normal_x") == 0)
// compute normals for the mesh if not already here
bool hasNormals = false;
bool hasColors = false;
for(unsigned int i=0; i<textureMesh->cloud.fields.size(); ++i)
{
hasNormals = true;
if(textureMesh->cloud.fields[i].name.compare("normal_x") == 0)
{
hasNormals = true;
}
else if(textureMesh->cloud.fields[i].name.compare("rgb") == 0)
{
hasColors = true;
}
}
else if(textureMesh->cloud.fields[i].name.compare("rgb") == 0)
if(!hasNormals)
{
hasColors = true;
// use polygons
if(hasColors)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
for(unsigned int i=0; i<mesh->polygons.size(); ++i)
{
pcl::Vertices & v = mesh->polygons[i];
UASSERT(v.vertices.size()>2);
Eigen::Vector3f v0(
cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
int last = v.vertices.size()-1;
Eigen::Vector3f v1(
cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
Eigen::Vector3f normal = v0.cross(v1);
normal.normalize();
// flat normal (per face)
for(unsigned int j=0; j<v.vertices.size(); ++j)
{
cloud->at(v.vertices[j]).normal_x = normal[0];
cloud->at(v.vertices[j]).normal_y = normal[1];
cloud->at(v.vertices[j]).normal_z = normal[2];
}
}
pcl::toPCLPointCloud2 (*cloud, textureMesh->cloud);
}
else
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
for(unsigned int i=0; i<mesh->polygons.size(); ++i)
{
pcl::Vertices & v = mesh->polygons[i];
UASSERT(v.vertices.size()>2);
Eigen::Vector3f v0(
cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
int last = v.vertices.size()-1;
Eigen::Vector3f v1(
cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
Eigen::Vector3f normal = v0.cross(v1);
normal.normalize();
// flat normal (per face)
for(unsigned int j=0; j<v.vertices.size(); ++j)
{
cloud->at(v.vertices[j]).normal_x = normal[0];
cloud->at(v.vertices[j]).normal_y = normal[1];
cloud->at(v.vertices[j]).normal_z = normal[2];
}
}
pcl::toPCLPointCloud2 (*cloud, textureMesh->cloud);
}
}
}
if(!hasNormals)
{
// use polygons
if(hasColors)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
for(unsigned int i=0; i<mesh->polygons.size(); ++i)
{
pcl::Vertices & v = mesh->polygons[i];
UASSERT(v.vertices.size()>2);
Eigen::Vector3f v0(
cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
int last = v.vertices.size()-1;
Eigen::Vector3f v1(
cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
Eigen::Vector3f normal = v0.cross(v1);
normal.normalize();
// flat normal (per face)
for(unsigned int j=0; j<v.vertices.size(); ++j)
{
cloud->at(v.vertices[j]).normal_x = normal[0];
cloud->at(v.vertices[j]).normal_y = normal[1];
cloud->at(v.vertices[j]).normal_z = normal[2];
}
}
pcl::toPCLPointCloud2 (*cloud, textureMesh->cloud);
}
else
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
for(unsigned int i=0; i<mesh->polygons.size(); ++i)
{
pcl::Vertices & v = mesh->polygons[i];
UASSERT(v.vertices.size()>2);
Eigen::Vector3f v0(
cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
int last = v.vertices.size()-1;
Eigen::Vector3f v1(
cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
Eigen::Vector3f normal = v0.cross(v1);
normal.normalize();
// flat normal (per face)
for(unsigned int j=0; j<v.vertices.size(); ++j)
{
cloud->at(v.vertices[j]).normal_x = normal[0];
cloud->at(v.vertices[j]).normal_y = normal[1];
cloud->at(v.vertices[j]).normal_z = normal[2];
}
}
pcl::toPCLPointCloud2 (*cloud, textureMesh->cloud);
}
}
return textureMesh;
}