mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 01:20:25 +08:00
GUI: Added export mesh with textures, refactored Export dialog (more MLS options)
This commit is contained in:
matlabbe
@@ -28,10 +28,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include "rtabmap/core/util3d_surface.h"
|
||||
#include "rtabmap/core/util3d_filtering.h"
|
||||
#include "rtabmap/utilite/ULogger.h"
|
||||
#include "rtabmap/utilite/UDirectory.h"
|
||||
#include "rtabmap/utilite/UConversion.h"
|
||||
#include <pcl/search/kdtree.h>
|
||||
#include <pcl/surface/gp3.h>
|
||||
#include <pcl/features/normal_3d_omp.h>
|
||||
#include <pcl/features/normal_3d.h>
|
||||
#include <pcl/surface/mls.h>
|
||||
#include <pcl/surface/texture_mapping.h>
|
||||
|
||||
#include <pcl/visualization/pcl_visualizer.h>
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
@@ -82,6 +88,147 @@ pcl::PolygonMesh::Ptr createMesh(
|
||||
return mesh;
|
||||
}
|
||||
|
||||
pcl::texture_mapping::CameraVector createTextureCameras(
|
||||
const std::map<int, Transform> & poses,
|
||||
const std::map<int, CameraModel> & cameraModels,
|
||||
const std::map<int, cv::Mat> & images,
|
||||
const std::string & tmpDirectory)
|
||||
{
|
||||
UASSERT(poses.size() == cameraModels.size() && poses.size() == images.size());
|
||||
UASSERT(UDirectory::exists(tmpDirectory));
|
||||
pcl::texture_mapping::CameraVector cameras(poses.size());
|
||||
std::map<int, Transform>::const_iterator poseIter=poses.begin();
|
||||
std::map<int, CameraModel>::const_iterator modelIter=cameraModels.begin();
|
||||
std::map<int, cv::Mat>::const_iterator imageIter=images.begin();
|
||||
int oi=0;
|
||||
for(; poseIter!=poses.end(); ++poseIter, ++modelIter, ++imageIter)
|
||||
{
|
||||
UASSERT(poseIter->first == modelIter->first);
|
||||
UASSERT(poseIter->first == imageIter->first);
|
||||
pcl::TextureMapping<pcl::PointXYZ>::Camera cam;
|
||||
|
||||
// transform into optical referential
|
||||
Transform rotation(0,-1,0,0,
|
||||
0,0,-1,0,
|
||||
1,0,0,0);
|
||||
|
||||
Transform t = poseIter->second*rotation.inverse();
|
||||
|
||||
cam.pose = t.toEigen3f();
|
||||
|
||||
UASSERT(modelIter->second.fx()>0 && imageIter->second.rows>0 && imageIter->second.cols>0);
|
||||
cam.focal_length=modelIter->second.fx();
|
||||
cam.height=imageIter->second.rows;
|
||||
cam.width=imageIter->second.cols;
|
||||
|
||||
|
||||
std::string fileName = uFormat("%s/%s%d.png", tmpDirectory.c_str(), "texture_", poseIter->first);
|
||||
if(!cv::imwrite(fileName, imageIter->second))
|
||||
{
|
||||
UERROR("Cannot save texture of image %d", poseIter->first);
|
||||
}
|
||||
else
|
||||
{
|
||||
UINFO("Saved temporary texture: \"%s\"", fileName.c_str());
|
||||
}
|
||||
cam.texture_file = fileName;
|
||||
cameras[oi++] = cam;
|
||||
}
|
||||
return cameras;
|
||||
}
|
||||
|
||||
pcl::TextureMesh::Ptr createTextureMesh(
|
||||
const pcl::PolygonMesh::Ptr & mesh,
|
||||
const std::map<int, Transform> & poses,
|
||||
const std::map<int, CameraModel> & cameraModels,
|
||||
const std::map<int, cv::Mat> & images,
|
||||
const std::string & tmpDirectory)
|
||||
{
|
||||
// Original from pcl/gpu/kinfu_large_scale/tools/standalone_texture_mapping.cpp:
|
||||
// Author: Raphael Favier, Technical University Eindhoven, (r.mysurname <aT> tue.nl)
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
|
||||
|
||||
// Create the texturemesh object that will contain our UV-mapped mesh
|
||||
pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
|
||||
textureMesh->cloud = mesh->cloud;
|
||||
std::vector< pcl::Vertices> polygons;
|
||||
|
||||
// push faces into the texturemesh object
|
||||
polygons.resize (mesh->polygons.size ());
|
||||
for(size_t i =0; i < mesh->polygons.size (); ++i)
|
||||
{
|
||||
polygons[i] = mesh->polygons[i];
|
||||
}
|
||||
textureMesh->tex_polygons.push_back(polygons);
|
||||
|
||||
// create cameras
|
||||
pcl::texture_mapping::CameraVector cameras = createTextureCameras(
|
||||
poses,
|
||||
cameraModels,
|
||||
images,
|
||||
tmpDirectory);
|
||||
|
||||
// Create materials for each texture (and one extra for occluded faces)
|
||||
textureMesh->tex_materials.resize (cameras.size () + 1);
|
||||
for(int i = 0 ; i <= cameras.size() ; ++i)
|
||||
{
|
||||
pcl::TexMaterial mesh_material;
|
||||
mesh_material.tex_Ka.r = 0.2f;
|
||||
mesh_material.tex_Ka.g = 0.2f;
|
||||
mesh_material.tex_Ka.b = 0.2f;
|
||||
|
||||
mesh_material.tex_Kd.r = 0.8f;
|
||||
mesh_material.tex_Kd.g = 0.8f;
|
||||
mesh_material.tex_Kd.b = 0.8f;
|
||||
|
||||
mesh_material.tex_Ks.r = 1.0f;
|
||||
mesh_material.tex_Ks.g = 1.0f;
|
||||
mesh_material.tex_Ks.b = 1.0f;
|
||||
|
||||
mesh_material.tex_d = 1.0f;
|
||||
mesh_material.tex_Ns = 75.0f;
|
||||
mesh_material.tex_illum = 2;
|
||||
|
||||
std::stringstream tex_name;
|
||||
tex_name << "material_" << i;
|
||||
tex_name >> mesh_material.tex_name;
|
||||
|
||||
if(i < cameras.size ())
|
||||
{
|
||||
mesh_material.tex_file = cameras[i].texture_file;
|
||||
}
|
||||
else
|
||||
{
|
||||
mesh_material.tex_file = tmpDirectory+UDirectory::separator()+"occluded.png";
|
||||
cv::Mat emptyImage;
|
||||
if(i>0)
|
||||
{
|
||||
emptyImage = cv::Mat::zeros(cameras[i-1].height,cameras[i-1].width, CV_8UC1);
|
||||
}
|
||||
else
|
||||
{
|
||||
emptyImage = cv::Mat::zeros(480, 640, CV_8UC1);
|
||||
}
|
||||
cv::imwrite(mesh_material.tex_file, emptyImage);
|
||||
}
|
||||
|
||||
textureMesh->tex_materials[i] = mesh_material;
|
||||
}
|
||||
|
||||
// Sort faces
|
||||
pcl::TextureMapping<pcl::PointXYZ> tm; // TextureMapping object that will perform the sort
|
||||
tm.textureMeshwithMultipleCameras(*textureMesh, cameras);
|
||||
|
||||
// compute normals for the mesh
|
||||
pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals = computeNormals(cloud, 20);
|
||||
|
||||
pcl::toPCLPointCloud2 (*cloudWithNormals, textureMesh->cloud);
|
||||
|
||||
return textureMesh;
|
||||
}
|
||||
|
||||
pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(
|
||||
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
|
||||
int normalKSearch)
|
||||
@@ -130,33 +277,48 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormals(
|
||||
return cloud_with_normals;
|
||||
}
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormalsSmoothed(
|
||||
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
|
||||
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
|
||||
float smoothingSearchRadius,
|
||||
bool smoothingPolynomialFit,
|
||||
float voxelSize)
|
||||
float searchRadius,
|
||||
int polygonialOrder,
|
||||
int upsamplingMethod, // NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION
|
||||
float upsamplingRadius, // SAMPLE_LOCAL_PLANE
|
||||
float upsamplingStep, // SAMPLE_LOCAL_PLANE
|
||||
int pointDensity, // RANDOM_UNIFORM_DENSITY
|
||||
float dilationVoxelSize, // VOXEL_GRID_DILATION
|
||||
int dilationIterations) // VOXEL_GRID_DILATION
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
|
||||
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
|
||||
tree->setInputCloud (cloud);
|
||||
|
||||
// Init object (second point type is for the normals, even if unused)
|
||||
// Init object (second point type is for the normals)
|
||||
pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> mls;
|
||||
|
||||
mls.setComputeNormals (true);
|
||||
|
||||
// Set parameters
|
||||
mls.setInputCloud (cloud);
|
||||
mls.setPolynomialFit (smoothingPolynomialFit);
|
||||
mls.setSearchMethod (tree);
|
||||
mls.setSearchRadius (smoothingSearchRadius);
|
||||
if(voxelSize > 0.0f)
|
||||
mls.setComputeNormals (true);
|
||||
if(polygonialOrder > 0)
|
||||
{
|
||||
mls.setUpsamplingMethod(pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal>::VOXEL_GRID_DILATION);
|
||||
mls.setDilationVoxelSize(voxelSize);
|
||||
mls.setPolynomialFit (true);
|
||||
mls.setPolynomialOrder(polygonialOrder);
|
||||
}
|
||||
else
|
||||
{
|
||||
mls.setPolynomialFit (false);
|
||||
}
|
||||
UASSERT(upsamplingMethod >= mls.NONE &&
|
||||
upsamplingMethod <= mls.VOXEL_GRID_DILATION);
|
||||
mls.setUpsamplingMethod((pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal>::UpsamplingMethod)upsamplingMethod);
|
||||
mls.setSearchRadius(searchRadius);
|
||||
mls.setUpsamplingRadius(upsamplingRadius);
|
||||
mls.setUpsamplingStepSize(upsamplingStep);
|
||||
mls.setPointDensity(pointDensity);
|
||||
mls.setDilationVoxelSize(dilationVoxelSize);
|
||||
mls.setDilationIterations(dilationIterations);
|
||||
|
||||
// Reconstruct
|
||||
mls.setInputCloud (cloud);
|
||||
mls.setSearchMethod (tree);
|
||||
mls.process (*cloud_with_normals);
|
||||
|
||||
return cloud_with_normals;
|
||||
@@ -164,30 +326,56 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormalsSmoothed(
|
||||
|
||||
void adjustNormalsToViewPoints(
|
||||
const pcl::PointCloud<pcl::PointXYZ>::Ptr & viewpoints,
|
||||
pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud)
|
||||
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
|
||||
int k)
|
||||
{
|
||||
if(viewpoints->size() && cloud.size())
|
||||
// FIXME: maybe better to project points in camera planes to know if they are visible from a specified viewpoint
|
||||
if(viewpoints->size() && cloud->size())
|
||||
{
|
||||
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
|
||||
tree->setInputCloud (viewpoints);
|
||||
pcl::search::KdTree<pcl::PointXYZ>::Ptr viewpointsTree (new pcl::search::KdTree<pcl::PointXYZ>);
|
||||
viewpointsTree->setInputCloud (viewpoints);
|
||||
|
||||
for(unsigned int i=0; i<cloud.size(); ++i)
|
||||
pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
|
||||
tree->setInputCloud (cloud);
|
||||
|
||||
for(unsigned int i=0; i<cloud->size(); ++i)
|
||||
{
|
||||
std::vector<int> indices;
|
||||
std::vector<float> dist;
|
||||
tree->nearestKSearch(pcl::PointXYZ(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z), 1, indices, dist);
|
||||
viewpointsTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, dist);
|
||||
UASSERT(indices.size() == 1);
|
||||
|
||||
Eigen::Vector3f v = viewpoints->at(indices[0]).getVector3fMap() - cloud.points[i].getVector3fMap();
|
||||
Eigen::Vector3f n(cloud.points[i].normal_x, cloud.points[i].normal_y, cloud.points[i].normal_z);
|
||||
|
||||
float result = v.dot(n);
|
||||
if(result < 0)
|
||||
if(indices.size() && indices[0]>=0)
|
||||
{
|
||||
//reverse normal
|
||||
cloud.points[i].normal_x *= -1.0f;
|
||||
cloud.points[i].normal_y *= -1.0f;
|
||||
cloud.points[i].normal_z *= -1.0f;
|
||||
|
||||
Eigen::Vector3f v = viewpoints->at(indices[0]).getVector3fMap() - cloud->points[i].getVector3fMap();
|
||||
|
||||
//compute point normal
|
||||
if(k >= 3)
|
||||
{
|
||||
tree->nearestKSearch(cloud->points[i], k, indices, dist);
|
||||
if(indices.size() >= 3)
|
||||
{
|
||||
Eigen::Vector4f planeParameters;
|
||||
float curvature;
|
||||
pcl::computePointNormal(*cloud, indices, planeParameters, curvature);
|
||||
|
||||
//update normal
|
||||
cloud->points[i].normal_x = planeParameters[0];
|
||||
cloud->points[i].normal_y = planeParameters[1];
|
||||
cloud->points[i].normal_z = planeParameters[2];
|
||||
}
|
||||
}
|
||||
|
||||
Eigen::Vector3f n(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
|
||||
|
||||
float result = v.dot(n);
|
||||
if(result < 0)
|
||||
{
|
||||
//reverse normal
|
||||
cloud->points[i].normal_x *= -1.0f;
|
||||
cloud->points[i].normal_y *= -1.0f;
|
||||
cloud->points[i].normal_z *= -1.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -51,6 +51,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformPointCloud(
|
||||
pcl::transformPointCloud(*cloud, *output, transform.toEigen4f());
|
||||
return output;
|
||||
}
|
||||
pcl::PointCloud<pcl::PointNormal>::Ptr transformPointCloud(
|
||||
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
|
||||
const Transform & transform)
|
||||
{
|
||||
pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
|
||||
pcl::transformPointCloudWithNormals(*cloud, *output, transform.toEigen4f());
|
||||
return output;
|
||||
}
|
||||
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformPointCloud(
|
||||
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
|
||||
const Transform & transform)
|
||||
|
||||
Reference in New Issue
Block a user