i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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

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