00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "rtabmap/core/util3d_surface.h"
00029 #include "rtabmap/core/util3d_filtering.h"
00030 #include "rtabmap/core/util3d.h"
00031 #include "rtabmap/core/util2d.h"
00032 #include "rtabmap/core/Memory.h"
00033 #include "rtabmap/core/DBDriver.h"
00034 #include "rtabmap/core/Compression.h"
00035 #include "rtabmap/utilite/ULogger.h"
00036 #include "rtabmap/utilite/UDirectory.h"
00037 #include "rtabmap/utilite/UConversion.h"
00038 #include "rtabmap/utilite/UMath.h"
00039 #include "rtabmap/utilite/UTimer.h"
00040 #include <opencv2/core/core_c.h>
00041 #include <opencv2/imgproc/types_c.h>
00042 #include <pcl/search/kdtree.h>
00043 #include <pcl/surface/gp3.h>
00044 #include <pcl/features/normal_3d_omp.h>
00045 #include <pcl/surface/mls.h>
00046 #include <pcl18/surface/texture_mapping.h>
00047 #include <pcl/features/integral_image_normal.h>
00048
00049 #ifndef DISABLE_VTK
00050 #include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>
00051 #endif
00052
00053 #if PCL_VERSION_COMPARE(<, 1, 8, 0)
00054 #include "pcl18/surface/organized_fast_mesh.h"
00055 #else
00056 #include <pcl/surface/organized_fast_mesh.h>
00057 #include <pcl/surface/impl/marching_cubes.hpp>
00058 #include <pcl/surface/impl/organized_fast_mesh.hpp>
00059 #include <pcl/impl/instantiate.hpp>
00060 #include <pcl/point_types.h>
00061
00062
00063 PCL_INSTANTIATE(OrganizedFastMesh, (pcl::PointXYZRGBNormal))
00064
00065 #include <pcl/features/impl/normal_3d_omp.hpp>
00066 #if PCL_VERSION_COMPARE(<=, 1, 8, 0)
00067 #ifdef PCL_ONLY_CORE_POINT_TYPES
00068 PCL_INSTANTIATE_PRODUCT(NormalEstimationOMP, ((pcl::PointXYZRGB))((pcl::Normal)))
00069 #endif
00070 #endif
00071 #endif
00072
00073 namespace rtabmap
00074 {
00075
00076 namespace util3d
00077 {
00078
00079 void createPolygonIndexes(
00080 const std::vector<pcl::Vertices> & polygons,
00081 int cloudSize,
00082 std::vector<std::set<int> > & neighbors,
00083 std::vector<std::set<int> > & vertexToPolygons)
00084 {
00085 vertexToPolygons = std::vector<std::set<int> >(cloudSize);
00086 neighbors = std::vector<std::set<int> >(polygons.size());
00087
00088 for(unsigned int i=0; i<polygons.size(); ++i)
00089 {
00090 std::set<int> vertices(polygons[i].vertices.begin(), polygons[i].vertices.end());
00091
00092 for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
00093 {
00094 int v = polygons[i].vertices.at(j);
00095 for(std::set<int>::iterator iter=vertexToPolygons[v].begin(); iter!=vertexToPolygons[v].end(); ++iter)
00096 {
00097 int numSharedVertices = 0;
00098 for(unsigned int k=0; k<polygons.at(*iter).vertices.size() && numSharedVertices<2; ++k)
00099 {
00100 if(vertices.find(polygons.at(*iter).vertices.at(k)) != vertices.end())
00101 {
00102 ++numSharedVertices;
00103 }
00104 }
00105 if(numSharedVertices >= 2)
00106 {
00107 neighbors[*iter].insert(i);
00108 neighbors[i].insert(*iter);
00109 }
00110 }
00111 vertexToPolygons[v].insert(i);
00112 }
00113 }
00114 }
00115
00116 std::list<std::list<int> > clusterPolygons(
00117 const std::vector<std::set<int> > & neighborPolygons,
00118 int minClusterSize)
00119 {
00120 std::set<int> polygonsChecked;
00121
00122 std::list<std::list<int> > clusters;
00123
00124 for(unsigned int i=0; i<neighborPolygons.size(); ++i)
00125 {
00126 if(polygonsChecked.find(i) == polygonsChecked.end())
00127 {
00128 std::list<int> currentCluster;
00129 currentCluster.push_back(i);
00130 polygonsChecked.insert(i);
00131
00132 for(std::list<int>::iterator iter=currentCluster.begin(); iter!=currentCluster.end(); ++iter)
00133 {
00134
00135 std::set<int> neighbors = neighborPolygons[*iter];
00136 for(std::set<int>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
00137 {
00138 if(polygonsChecked.insert(*jter).second)
00139 {
00140 currentCluster.push_back(*jter);
00141 }
00142 }
00143 }
00144 if((int)currentCluster.size() > minClusterSize)
00145 {
00146 clusters.push_back(currentCluster);
00147 }
00148 }
00149 }
00150 return clusters;
00151 }
00152
00153 std::vector<pcl::Vertices> organizedFastMesh(
00154 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
00155 double angleTolerance,
00156 bool quad,
00157 int trianglePixelSize,
00158 const Eigen::Vector3f & viewpoint)
00159 {
00160 UDEBUG("size=%d angle=%f quad=%d triangleSize=%d", (int)cloud->size(), angleTolerance, quad?1:0, trianglePixelSize);
00161 UASSERT(cloud->is_dense == false);
00162 UASSERT(cloud->width > 1 && cloud->height > 1);
00163
00164 pcl::OrganizedFastMesh<pcl::PointXYZ> ofm;
00165 ofm.setTrianglePixelSize (trianglePixelSize);
00166 ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZ>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZ>::TRIANGLE_RIGHT_CUT);
00167 ofm.setInputCloud (cloud);
00168 ofm.setAngleTolerance(angleTolerance);
00169 ofm.setViewpoint(viewpoint);
00170
00171 std::vector<pcl::Vertices> vertices;
00172 ofm.reconstruct (vertices);
00173
00174 if(quad)
00175 {
00176
00177 std::vector<pcl::Vertices> output(vertices.size());
00178 for(unsigned int i=0; i<vertices.size(); ++i)
00179 {
00180 output[i].vertices.resize(4);
00181 output[i].vertices[0] = vertices[i].vertices[0];
00182 output[i].vertices[3] = vertices[i].vertices[1];
00183 output[i].vertices[2] = vertices[i].vertices[2];
00184 output[i].vertices[1] = vertices[i].vertices[3];
00185 }
00186 return output;
00187 }
00188
00189 return vertices;
00190 }
00191 std::vector<pcl::Vertices> organizedFastMesh(
00192 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
00193 double angleTolerance,
00194 bool quad,
00195 int trianglePixelSize,
00196 const Eigen::Vector3f & viewpoint)
00197 {
00198 UDEBUG("size=%d angle=%f quad=%d triangleSize=%d", (int)cloud->size(), angleTolerance, quad?1:0, trianglePixelSize);
00199 UASSERT(cloud->is_dense == false);
00200 UASSERT(cloud->width > 1 && cloud->height > 1);
00201
00202 pcl::OrganizedFastMesh<pcl::PointXYZRGB> ofm;
00203 ofm.setTrianglePixelSize (trianglePixelSize);
00204 ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZRGB>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZRGB>::TRIANGLE_RIGHT_CUT);
00205 ofm.setInputCloud (cloud);
00206 ofm.setAngleTolerance(angleTolerance);
00207 ofm.setViewpoint(viewpoint);
00208
00209 std::vector<pcl::Vertices> vertices;
00210 ofm.reconstruct (vertices);
00211
00212 if(quad)
00213 {
00214
00215 std::vector<pcl::Vertices> output(vertices.size());
00216 for(unsigned int i=0; i<vertices.size(); ++i)
00217 {
00218 output[i].vertices.resize(4);
00219 output[i].vertices[0] = vertices[i].vertices[0];
00220 output[i].vertices[3] = vertices[i].vertices[1];
00221 output[i].vertices[2] = vertices[i].vertices[2];
00222 output[i].vertices[1] = vertices[i].vertices[3];
00223 }
00224 return output;
00225 }
00226
00227 return vertices;
00228 }
00229 std::vector<pcl::Vertices> organizedFastMesh(
00230 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
00231 double angleTolerance,
00232 bool quad,
00233 int trianglePixelSize,
00234 const Eigen::Vector3f & viewpoint)
00235 {
00236 UDEBUG("size=%d angle=%f quad=%d triangleSize=%d", (int)cloud->size(), angleTolerance, quad?1:0, trianglePixelSize);
00237 UASSERT(cloud->is_dense == false);
00238 UASSERT(cloud->width > 1 && cloud->height > 1);
00239
00240 pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal> ofm;
00241 ofm.setTrianglePixelSize (trianglePixelSize);
00242 ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal>::TRIANGLE_RIGHT_CUT);
00243 ofm.setInputCloud (cloud);
00244 ofm.setAngleTolerance(angleTolerance);
00245 ofm.setViewpoint(viewpoint);
00246
00247 std::vector<pcl::Vertices> vertices;
00248 ofm.reconstruct (vertices);
00249
00250 if(quad)
00251 {
00252
00253 std::vector<pcl::Vertices> output(vertices.size());
00254 for(unsigned int i=0; i<vertices.size(); ++i)
00255 {
00256 output[i].vertices.resize(4);
00257 output[i].vertices[0] = vertices[i].vertices[0];
00258 output[i].vertices[3] = vertices[i].vertices[1];
00259 output[i].vertices[2] = vertices[i].vertices[2];
00260 output[i].vertices[1] = vertices[i].vertices[3];
00261 }
00262 return output;
00263 }
00264
00265 return vertices;
00266 }
00267
00268 void appendMesh(
00269 pcl::PointCloud<pcl::PointXYZRGBNormal> & cloudA,
00270 std::vector<pcl::Vertices> & polygonsA,
00271 const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloudB,
00272 const std::vector<pcl::Vertices> & polygonsB)
00273 {
00274 UDEBUG("cloudA=%d polygonsA=%d cloudB=%d polygonsB=%d", (int)cloudA.size(), (int)polygonsA.size(), (int)cloudB.size(), (int)polygonsB.size());
00275 UASSERT(!cloudA.isOrganized() && !cloudB.isOrganized());
00276
00277 int sizeA = (int)cloudA.size();
00278 cloudA += cloudB;
00279
00280 int sizePolygonsA = (int)polygonsA.size();
00281 polygonsA.resize(sizePolygonsA+polygonsB.size());
00282
00283 for(unsigned int i=0; i<polygonsB.size(); ++i)
00284 {
00285 pcl::Vertices vertices = polygonsB[i];
00286 for(unsigned int j=0; j<vertices.vertices.size(); ++j)
00287 {
00288 vertices.vertices[j] += sizeA;
00289 }
00290 polygonsA[i+sizePolygonsA] = vertices;
00291 }
00292 }
00293
00294 void appendMesh(
00295 pcl::PointCloud<pcl::PointXYZRGB> & cloudA,
00296 std::vector<pcl::Vertices> & polygonsA,
00297 const pcl::PointCloud<pcl::PointXYZRGB> & cloudB,
00298 const std::vector<pcl::Vertices> & polygonsB)
00299 {
00300 UDEBUG("cloudA=%d polygonsA=%d cloudB=%d polygonsB=%d", (int)cloudA.size(), (int)polygonsA.size(), (int)cloudB.size(), (int)polygonsB.size());
00301 UASSERT(!cloudA.isOrganized() && !cloudB.isOrganized());
00302
00303 int sizeA = (int)cloudA.size();
00304 cloudA += cloudB;
00305
00306 int sizePolygonsA = (int)polygonsA.size();
00307 polygonsA.resize(sizePolygonsA+polygonsB.size());
00308
00309 for(unsigned int i=0; i<polygonsB.size(); ++i)
00310 {
00311 pcl::Vertices vertices = polygonsB[i];
00312 for(unsigned int j=0; j<vertices.vertices.size(); ++j)
00313 {
00314 vertices.vertices[j] += sizeA;
00315 }
00316 polygonsA[i+sizePolygonsA] = vertices;
00317 }
00318 }
00319
00320 std::vector<int> filterNotUsedVerticesFromMesh(
00321 const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
00322 const std::vector<pcl::Vertices> & polygons,
00323 pcl::PointCloud<pcl::PointXYZRGBNormal> & outputCloud,
00324 std::vector<pcl::Vertices> & outputPolygons)
00325 {
00326 UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
00327 std::map<int, int> addedVertices;
00328 std::vector<int> output;
00329 output.resize(cloud.size());
00330 outputCloud.resize(cloud.size());
00331 outputCloud.is_dense = true;
00332 outputPolygons.resize(polygons.size());
00333 int oi = 0;
00334 for(unsigned int i=0; i<polygons.size(); ++i)
00335 {
00336 pcl::Vertices & v = outputPolygons[i];
00337 v.vertices.resize(polygons[i].vertices.size());
00338 for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
00339 {
00340 std::map<int, int>::iterator iter = addedVertices.find(polygons[i].vertices[j]);
00341 if(iter == addedVertices.end())
00342 {
00343 outputCloud[oi] = cloud.at(polygons[i].vertices[j]);
00344 addedVertices.insert(std::make_pair(polygons[i].vertices[j], oi));
00345 output[oi] = polygons[i].vertices[j];
00346 v.vertices[j] = oi++;
00347 }
00348 else
00349 {
00350 v.vertices[j] = iter->second;
00351 }
00352 }
00353 }
00354 outputCloud.resize(oi);
00355 output.resize(oi);
00356
00357 return output;
00358 }
00359
00360 std::vector<int> filterNotUsedVerticesFromMesh(
00361 const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
00362 const std::vector<pcl::Vertices> & polygons,
00363 pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,
00364 std::vector<pcl::Vertices> & outputPolygons)
00365 {
00366 UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
00367 std::map<int, int> addedVertices;
00368 std::vector<int> output;
00369 output.resize(cloud.size());
00370 outputCloud.resize(cloud.size());
00371 outputCloud.is_dense = true;
00372 outputPolygons.resize(polygons.size());
00373 int oi = 0;
00374 for(unsigned int i=0; i<polygons.size(); ++i)
00375 {
00376 pcl::Vertices & v = outputPolygons[i];
00377 v.vertices.resize(polygons[i].vertices.size());
00378 for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
00379 {
00380 std::map<int, int>::iterator iter = addedVertices.find(polygons[i].vertices[j]);
00381 if(iter == addedVertices.end())
00382 {
00383 outputCloud[oi] = cloud.at(polygons[i].vertices[j]);
00384 addedVertices.insert(std::make_pair(polygons[i].vertices[j], oi));
00385 output[oi] = polygons[i].vertices[j];
00386 v.vertices[j] = oi++;
00387 }
00388 else
00389 {
00390 v.vertices[j] = iter->second;
00391 }
00392 }
00393 }
00394 outputCloud.resize(oi);
00395 output.resize(oi);
00396
00397 return output;
00398 }
00399
00400 std::vector<int> filterNaNPointsFromMesh(
00401 const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
00402 const std::vector<pcl::Vertices> & polygons,
00403 pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,
00404 std::vector<pcl::Vertices> & outputPolygons)
00405 {
00406 UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
00407 std::map<int, int> addedVertices;
00408 std::vector<int> output;
00409 output.resize(cloud.size());
00410 outputCloud.resize(cloud.size());
00411 outputCloud.is_dense = true;
00412 std::vector<int> organizedToDense(cloud.size(), -1);
00413
00414 int oi = 0;
00415 for(unsigned int i=0; i<cloud.size(); ++i)
00416 {
00417 if(pcl::isFinite(cloud.at(i)))
00418 {
00419 outputCloud.at(oi) = cloud.at(i);
00420 output[oi] = i;
00421 organizedToDense[i] = oi;
00422 ++oi;
00423 }
00424 }
00425 outputCloud.resize(oi);
00426 output.resize(oi);
00427
00428
00429 outputPolygons = polygons;
00430 for(unsigned int i=0; i<outputPolygons.size(); ++i)
00431 {
00432 pcl::Vertices & v = outputPolygons[i];
00433 for(unsigned int j=0; j<v.vertices.size(); ++j)
00434 {
00435 UASSERT(organizedToDense[v.vertices[j]] >= 0);
00436 v.vertices[j] = organizedToDense[v.vertices[j]];
00437 }
00438 }
00439
00440 return output;
00441 }
00442
00443 std::vector<pcl::Vertices> filterCloseVerticesFromMesh(
00444 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud,
00445 const std::vector<pcl::Vertices> & polygons,
00446 float radius,
00447 float angle,
00448 bool keepLatestInRadius)
00449 {
00450 UDEBUG("size=%d polygons=%d radius=%f angle=%f keepLatest=%d",
00451 (int)cloud->size(), (int)polygons.size(), radius, angle, keepLatestInRadius?1:0);
00452 std::vector<pcl::Vertices> outputPolygons;
00453 pcl::KdTreeFLANN<pcl::PointXYZRGBNormal>::Ptr kdtree(new pcl::KdTreeFLANN<pcl::PointXYZRGBNormal>);
00454 kdtree->setInputCloud(cloud);
00455
00456 std::map<int, int> verticesDone;
00457 outputPolygons = polygons;
00458 for(unsigned int i=0; i<outputPolygons.size(); ++i)
00459 {
00460 pcl::Vertices & polygon = outputPolygons[i];
00461 for(unsigned int j=0; j<polygon.vertices.size(); ++j)
00462 {
00463 std::map<int, int>::iterator iter = verticesDone.find(polygon.vertices[j]);
00464 if(iter != verticesDone.end())
00465 {
00466 polygon.vertices[j] = iter->second;
00467 }
00468 else
00469 {
00470 std::vector<int> kIndices;
00471 std::vector<float> kDistances;
00472 kdtree->radiusSearch(polygon.vertices[j], radius, kIndices, kDistances);
00473 if(kIndices.size())
00474 {
00475 int reference = -1;
00476 for(unsigned int z=0; z<kIndices.size(); ++z)
00477 {
00478 if(reference == -1)
00479 {
00480 reference = kIndices[z];
00481 }
00482 else if(keepLatestInRadius)
00483 {
00484 if(kIndices[z] < reference)
00485 {
00486 reference = kIndices[z];
00487 }
00488 }
00489 else
00490 {
00491 if(kIndices[z] > reference)
00492 {
00493 reference = kIndices[z];
00494 }
00495 }
00496 }
00497 if(reference >= 0)
00498 {
00499 for(unsigned int z=0; z<kIndices.size(); ++z)
00500 {
00501 verticesDone.insert(std::make_pair(kIndices[j], reference));
00502 }
00503 polygon.vertices[j] = reference;
00504 }
00505 }
00506 else
00507 {
00508 verticesDone.insert(std::make_pair(polygon.vertices[j], polygon.vertices[j]));
00509 }
00510 }
00511 }
00512 }
00513 return outputPolygons;
00514 }
00515
00516 std::vector<pcl::Vertices> filterInvalidPolygons(const std::vector<pcl::Vertices> & polygons)
00517 {
00518 std::vector<pcl::Vertices> output(polygons.size());
00519 int oi=0;
00520 for(unsigned int i=0; i<polygons.size(); ++i)
00521 {
00522 bool valid = true;
00523 for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
00524 {
00525 if(polygons[i].vertices[j] == polygons[i].vertices[(j+1)%polygons[i].vertices.size()])
00526 {
00527 valid = false;
00528 break;
00529 }
00530 }
00531 if(valid)
00532 {
00533 output[oi++] = polygons[i];
00534 }
00535 }
00536 output.resize(oi);
00537 return output;
00538 }
00539
00540 pcl::PolygonMesh::Ptr createMesh(
00541 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloudWithNormals,
00542 float gp3SearchRadius,
00543 float gp3Mu,
00544 int gp3MaximumNearestNeighbors,
00545 float gp3MaximumSurfaceAngle,
00546 float gp3MinimumAngle,
00547 float gp3MaximumAngle,
00548 bool gp3NormalConsistency)
00549 {
00550 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormalsNoNaN = removeNaNNormalsFromPointCloud(cloudWithNormals);
00551
00552
00553 pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
00554 tree2->setInputCloud (cloudWithNormalsNoNaN);
00555
00556
00557 pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal> gp3;
00558 pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
00559
00560
00561 gp3.setSearchRadius (gp3SearchRadius);
00562
00563
00564 gp3.setMu (gp3Mu);
00565 gp3.setMaximumNearestNeighbors (gp3MaximumNearestNeighbors);
00566 gp3.setMaximumSurfaceAngle(gp3MaximumSurfaceAngle);
00567 gp3.setMinimumAngle(gp3MinimumAngle);
00568 gp3.setMaximumAngle(gp3MaximumAngle);
00569 gp3.setNormalConsistency(gp3NormalConsistency);
00570 gp3.setConsistentVertexOrdering(gp3NormalConsistency);
00571
00572
00573 gp3.setInputCloud (cloudWithNormalsNoNaN);
00574 gp3.setSearchMethod (tree2);
00575 gp3.reconstruct (*mesh);
00576
00577
00578
00579
00580 return mesh;
00581 }
00582
00583 pcl::texture_mapping::CameraVector createTextureCameras(
00584 const std::map<int, Transform> & poses,
00585 const std::map<int, std::vector<CameraModel> > & cameraModels,
00586 const std::map<int, cv::Mat> & cameraDepths,
00587 const std::vector<float> & roiRatios)
00588 {
00589 UASSERT_MSG(poses.size() == cameraModels.size(), uFormat("%d vs %d", (int)poses.size(), (int)cameraModels.size()).c_str());
00590 UASSERT(roiRatios.empty() || roiRatios.size() == 4);
00591 pcl::texture_mapping::CameraVector cameras;
00592 std::map<int, Transform>::const_iterator poseIter=poses.begin();
00593 std::map<int, std::vector<CameraModel> >::const_iterator modelIter=cameraModels.begin();
00594 for(; poseIter!=poses.end(); ++poseIter, ++modelIter)
00595 {
00596 UASSERT(poseIter->first == modelIter->first);
00597
00598 std::map<int, cv::Mat>::const_iterator depthIter = cameraDepths.find(poseIter->first);
00599
00600
00601 for(unsigned int i=0; i<modelIter->second.size(); ++i)
00602 {
00603 pcl::TextureMapping<pcl::PointXYZ>::Camera cam;
00604
00605 UASSERT(!modelIter->second[i].localTransform().isNull() && !poseIter->second.isNull());
00606 Transform t = poseIter->second*modelIter->second[i].localTransform();
00607
00608 cam.pose = t.toEigen3f();
00609
00610 if(modelIter->second[i].imageHeight() <=0 || modelIter->second[i].imageWidth() <=0)
00611 {
00612 UERROR("Should have camera models with width/height set to create texture cameras!");
00613 return pcl::texture_mapping::CameraVector();
00614 }
00615
00616 UASSERT(modelIter->second[i].fx()>0 && modelIter->second[i].imageHeight()>0 && modelIter->second[i].imageWidth()>0);
00617 cam.focal_length=modelIter->second[i].fx();
00618 cam.height=modelIter->second[i].imageHeight();
00619 cam.width=modelIter->second[i].imageWidth();
00620 if(modelIter->second.size() == 1)
00621 {
00622 cam.texture_file = uFormat("%d", poseIter->first);
00623 }
00624 else
00625 {
00626 cam.texture_file = uFormat("%d_%d", poseIter->first, (int)i);
00627 }
00628 if(!roiRatios.empty())
00629 {
00630 cam.roi.resize(4);
00631 cam.roi[0] = cam.width * roiRatios[0];
00632 cam.roi[1] = cam.height * roiRatios[2];
00633 cam.roi[2] = cam.width * (1.0 - roiRatios[1]) - cam.roi[0];
00634 cam.roi[3] = cam.height * (1.0 - roiRatios[3]) - cam.roi[1];
00635 }
00636
00637 if(depthIter != cameraDepths.end() && !depthIter->second.empty())
00638 {
00639 UASSERT(depthIter->second.type() == CV_32FC1 || depthIter->second.type() == CV_16UC1);
00640 UASSERT(depthIter->second.cols % modelIter->second.size() == 0);
00641 int subWidth = depthIter->second.cols/(modelIter->second.size());
00642 cam.depth = cv::Mat(depthIter->second, cv::Range(0, depthIter->second.rows), cv::Range(subWidth*i, subWidth*(i+1)));
00643 }
00644
00645 UDEBUG("%f", cam.focal_length);
00646 UDEBUG("%f", cam.height);
00647 UDEBUG("%f", cam.width);
00648 UDEBUG("cam.pose=%s", t.prettyPrint().c_str());
00649
00650 cameras.push_back(cam);
00651 }
00652 }
00653 return cameras;
00654 }
00655
00656 pcl::TextureMesh::Ptr createTextureMesh(
00657 const pcl::PolygonMesh::Ptr & mesh,
00658 const std::map<int, Transform> & poses,
00659 const std::map<int, CameraModel> & cameraModels,
00660 const std::map<int, cv::Mat> & cameraDepths,
00661 float maxDistance,
00662 float maxDepthError,
00663 float maxAngle,
00664 int minClusterSize,
00665 const std::vector<float> & roiRatios,
00666 const ProgressState * state,
00667 std::vector<std::map<int, pcl::PointXY> > * vertexToPixels)
00668 {
00669 std::map<int, std::vector<CameraModel> > cameraSubModels;
00670 for(std::map<int, CameraModel>::const_iterator iter=cameraModels.begin(); iter!=cameraModels.end(); ++iter)
00671 {
00672 std::vector<CameraModel> models;
00673 models.push_back(iter->second);
00674 cameraSubModels.insert(std::make_pair(iter->first, models));
00675 }
00676
00677 return createTextureMesh(
00678 mesh,
00679 poses,
00680 cameraSubModels,
00681 cameraDepths,
00682 maxDistance,
00683 maxDepthError,
00684 maxAngle,
00685 minClusterSize,
00686 roiRatios,
00687 state,
00688 vertexToPixels);
00689 }
00690
00691 pcl::TextureMesh::Ptr createTextureMesh(
00692 const pcl::PolygonMesh::Ptr & mesh,
00693 const std::map<int, Transform> & poses,
00694 const std::map<int, std::vector<CameraModel> > & cameraModels,
00695 const std::map<int, cv::Mat> & cameraDepths,
00696 float maxDistance,
00697 float maxDepthError,
00698 float maxAngle,
00699 int minClusterSize,
00700 const std::vector<float> & roiRatios,
00701 const ProgressState * state,
00702 std::vector<std::map<int, pcl::PointXY> > * vertexToPixels)
00703 {
00704 UASSERT(mesh->polygons.size());
00705 pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
00706 textureMesh->cloud = mesh->cloud;
00707 textureMesh->tex_polygons.push_back(mesh->polygons);
00708
00709
00710
00711
00712
00713
00714
00715 pcl::texture_mapping::CameraVector cameras = createTextureCameras(
00716 poses,
00717 cameraModels,
00718 cameraDepths,
00719 roiRatios);
00720
00721
00722 textureMesh->tex_materials.resize (cameras.size () + 1);
00723 for(unsigned int i = 0 ; i <= cameras.size() ; ++i)
00724 {
00725 pcl::TexMaterial mesh_material;
00726 mesh_material.tex_Ka.r = 0.2f;
00727 mesh_material.tex_Ka.g = 0.2f;
00728 mesh_material.tex_Ka.b = 0.2f;
00729
00730 mesh_material.tex_Kd.r = 0.8f;
00731 mesh_material.tex_Kd.g = 0.8f;
00732 mesh_material.tex_Kd.b = 0.8f;
00733
00734 mesh_material.tex_Ks.r = 1.0f;
00735 mesh_material.tex_Ks.g = 1.0f;
00736 mesh_material.tex_Ks.b = 1.0f;
00737
00738 mesh_material.tex_d = 1.0f;
00739 mesh_material.tex_Ns = 75.0f;
00740 mesh_material.tex_illum = 2;
00741
00742 std::stringstream tex_name;
00743 tex_name << "material_" << i;
00744 tex_name >> mesh_material.tex_name;
00745
00746 if(i < cameras.size ())
00747 {
00748 mesh_material.tex_file = cameras[i].texture_file;
00749 }
00750 else
00751 {
00752 mesh_material.tex_file = "occluded";
00753 }
00754
00755 textureMesh->tex_materials[i] = mesh_material;
00756 }
00757
00758
00759 pcl::TextureMapping<pcl::PointXYZ> tm;
00760 tm.setMaxDistance(maxDistance);
00761 tm.setMaxAngle(maxAngle);
00762 if(maxDepthError > 0.0f)
00763 {
00764 tm.setMaxDepthError(maxDepthError);
00765 }
00766 tm.setMinClusterSize(minClusterSize);
00767 if(tm.textureMeshwithMultipleCameras2(*textureMesh, cameras, state, vertexToPixels))
00768 {
00769
00770 bool hasNormals = false;
00771 bool hasColors = false;
00772 for(unsigned int i=0; i<textureMesh->cloud.fields.size(); ++i)
00773 {
00774 if(textureMesh->cloud.fields[i].name.compare("normal_x") == 0)
00775 {
00776 hasNormals = true;
00777 }
00778 else if(textureMesh->cloud.fields[i].name.compare("rgb") == 0)
00779 {
00780 hasColors = true;
00781 }
00782 }
00783 if(!hasNormals)
00784 {
00785
00786 if(hasColors)
00787 {
00788 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00789 pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
00790
00791 for(unsigned int i=0; i<mesh->polygons.size(); ++i)
00792 {
00793 pcl::Vertices & v = mesh->polygons[i];
00794 UASSERT(v.vertices.size()>2);
00795 Eigen::Vector3f v0(
00796 cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
00797 cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
00798 cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
00799 int last = v.vertices.size()-1;
00800 Eigen::Vector3f v1(
00801 cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
00802 cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
00803 cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
00804 Eigen::Vector3f normal = v0.cross(v1);
00805 normal.normalize();
00806
00807 for(unsigned int j=0; j<v.vertices.size(); ++j)
00808 {
00809 cloud->at(v.vertices[j]).normal_x = normal[0];
00810 cloud->at(v.vertices[j]).normal_y = normal[1];
00811 cloud->at(v.vertices[j]).normal_z = normal[2];
00812 }
00813 }
00814 pcl::toPCLPointCloud2 (*cloud, textureMesh->cloud);
00815 }
00816 else
00817 {
00818 pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal>);
00819 pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
00820
00821 for(unsigned int i=0; i<mesh->polygons.size(); ++i)
00822 {
00823 pcl::Vertices & v = mesh->polygons[i];
00824 UASSERT(v.vertices.size()>2);
00825 Eigen::Vector3f v0(
00826 cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
00827 cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
00828 cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
00829 int last = v.vertices.size()-1;
00830 Eigen::Vector3f v1(
00831 cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
00832 cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
00833 cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
00834 Eigen::Vector3f normal = v0.cross(v1);
00835 normal.normalize();
00836
00837 for(unsigned int j=0; j<v.vertices.size(); ++j)
00838 {
00839 cloud->at(v.vertices[j]).normal_x = normal[0];
00840 cloud->at(v.vertices[j]).normal_y = normal[1];
00841 cloud->at(v.vertices[j]).normal_z = normal[2];
00842 }
00843 }
00844 pcl::toPCLPointCloud2 (*cloud, textureMesh->cloud);
00845 }
00846 }
00847 }
00848 return textureMesh;
00849 }
00850
00851 void cleanTextureMesh(
00852 pcl::TextureMesh & textureMesh,
00853 int minClusterSize)
00854 {
00855 UDEBUG("minClusterSize=%d", minClusterSize);
00856
00857 if(textureMesh.tex_coordinates.size())
00858 {
00859
00860 textureMesh.tex_coordinates.pop_back();
00861 textureMesh.tex_polygons.pop_back();
00862 textureMesh.tex_materials.pop_back();
00863
00864 if(minClusterSize!=0)
00865 {
00866
00867 unsigned int totalSize = 0;
00868 for(unsigned int t=0; t<textureMesh.tex_polygons.size(); ++t)
00869 {
00870 totalSize+=textureMesh.tex_polygons[t].size();
00871 }
00872 std::vector<pcl::Vertices> allPolygons(totalSize);
00873 int oi=0;
00874 for(unsigned int t=0; t<textureMesh.tex_polygons.size(); ++t)
00875 {
00876 for(unsigned int i=0; i<textureMesh.tex_polygons[t].size(); ++i)
00877 {
00878 allPolygons[oi++] = textureMesh.tex_polygons[t][i];
00879 }
00880 }
00881
00882
00883 std::vector<std::set<int> > neighbors;
00884 std::vector<std::set<int> > vertexToPolygons;
00885 util3d::createPolygonIndexes(allPolygons,
00886 (int)textureMesh.cloud.data.size()/textureMesh.cloud.point_step,
00887 neighbors,
00888 vertexToPolygons);
00889
00890 std::list<std::list<int> > clusters = util3d::clusterPolygons(
00891 neighbors,
00892 minClusterSize<0?0:minClusterSize);
00893
00894 std::set<int> validPolygons;
00895 if(minClusterSize < 0)
00896 {
00897
00898 std::list<std::list<int> >::iterator biggestClusterIndex = clusters.end();
00899 unsigned int biggestClusterSize = 0;
00900 for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
00901 {
00902 if(iter->size() > biggestClusterSize)
00903 {
00904 biggestClusterIndex = iter;
00905 biggestClusterSize = iter->size();
00906 }
00907 }
00908 if(biggestClusterIndex != clusters.end())
00909 {
00910 for(std::list<int>::iterator jter=biggestClusterIndex->begin(); jter!=biggestClusterIndex->end(); ++jter)
00911 {
00912 validPolygons.insert(*jter);
00913 }
00914 }
00915 }
00916 else
00917 {
00918 for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
00919 {
00920 for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
00921 {
00922 validPolygons.insert(*jter);
00923 }
00924 }
00925 }
00926
00927 if(validPolygons.size() == 0)
00928 {
00929 UWARN("All %d polygons filtered after polygon cluster filtering. Cluster minimum size is %d.",totalSize, minClusterSize);
00930 }
00931
00932
00933 unsigned int allPolygonsIndex = 0;
00934 for(unsigned int t=0; t<textureMesh.tex_polygons.size(); ++t)
00935 {
00936 std::vector<pcl::Vertices> filteredPolygons(textureMesh.tex_polygons[t].size());
00937 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
00938 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > filteredCoordinates(textureMesh.tex_coordinates[t].size());
00939 #else
00940 std::vector<Eigen::Vector2f> filteredCoordinates(textureMesh.tex_coordinates[t].size());
00941 #endif
00942
00943 if(textureMesh.tex_polygons[t].size())
00944 {
00945 UASSERT_MSG(allPolygonsIndex < allPolygons.size(), uFormat("%d vs %d", (int)allPolygonsIndex, (int)allPolygons.size()).c_str());
00946
00947
00948 std::vector<unsigned int> polygonToCoord(textureMesh.tex_polygons[t].size());
00949 unsigned int totalCoord = 0;
00950 for(unsigned int i=0; i<textureMesh.tex_polygons[t].size(); ++i)
00951 {
00952 polygonToCoord[i] = totalCoord;
00953 totalCoord+=textureMesh.tex_polygons[t][i].vertices.size();
00954 }
00955 UASSERT_MSG(totalCoord == textureMesh.tex_coordinates[t].size(), uFormat("%d vs %d", totalCoord, (int)textureMesh.tex_coordinates[t].size()).c_str());
00956
00957 int oi=0;
00958 int ci=0;
00959 for(unsigned int i=0; i<textureMesh.tex_polygons[t].size(); ++i)
00960 {
00961 if(validPolygons.find(allPolygonsIndex) != validPolygons.end())
00962 {
00963 filteredPolygons[oi] = textureMesh.tex_polygons[t].at(i);
00964 for(unsigned int j=0; j<filteredPolygons[oi].vertices.size(); ++j)
00965 {
00966 UASSERT(polygonToCoord[i] < textureMesh.tex_coordinates[t].size());
00967 filteredCoordinates[ci] = textureMesh.tex_coordinates[t][polygonToCoord[i]+j];
00968 ++ci;
00969 }
00970 ++oi;
00971 }
00972 ++allPolygonsIndex;
00973 }
00974 filteredPolygons.resize(oi);
00975 filteredCoordinates.resize(ci);
00976 textureMesh.tex_polygons[t] = filteredPolygons;
00977 textureMesh.tex_coordinates[t] = filteredCoordinates;
00978 }
00979 }
00980 }
00981 }
00982 }
00983
00984 pcl::TextureMesh::Ptr concatenateTextureMeshes(const std::list<pcl::TextureMesh::Ptr> & meshes)
00985 {
00986 pcl::TextureMesh::Ptr output(new pcl::TextureMesh);
00987 std::map<std::string, int> addedMaterials;
00988 for(std::list<pcl::TextureMesh::Ptr>::const_iterator iter = meshes.begin(); iter!=meshes.end(); ++iter)
00989 {
00990 if((*iter)->cloud.point_step &&
00991 (*iter)->cloud.data.size()/(*iter)->cloud.point_step &&
00992 (*iter)->tex_polygons.size() &&
00993 (*iter)->tex_coordinates.size())
00994 {
00995
00996 int polygonStep = output->cloud.height * output->cloud.width;
00997 pcl::PCLPointCloud2 tmp;
00998 pcl::concatenatePointCloud(output->cloud, iter->get()->cloud, tmp);
00999 output->cloud = tmp;
01000
01001 UASSERT((*iter)->tex_polygons.size() == (*iter)->tex_coordinates.size() &&
01002 (*iter)->tex_polygons.size() == (*iter)->tex_materials.size());
01003
01004 int materialCount = (*iter)->tex_polygons.size();
01005 for(int i=0; i<materialCount; ++i)
01006 {
01007 std::map<std::string, int>::iterator jter = addedMaterials.find((*iter)->tex_materials[i].tex_file);
01008 int index;
01009 if(jter != addedMaterials.end())
01010 {
01011 index = jter->second;
01012 }
01013 else
01014 {
01015 addedMaterials.insert(std::make_pair((*iter)->tex_materials[i].tex_file, output->tex_materials.size()));
01016 index = output->tex_materials.size();
01017 output->tex_materials.push_back((*iter)->tex_materials[i]);
01018 output->tex_materials.back().tex_name = uFormat("material_%d", index);
01019 output->tex_polygons.resize(output->tex_polygons.size() + 1);
01020 output->tex_coordinates.resize(output->tex_coordinates.size() + 1);
01021 }
01022
01023
01024 int oi = output->tex_polygons[index].size();
01025 output->tex_polygons[index].resize(output->tex_polygons[index].size() + (*iter)->tex_polygons[i].size());
01026 for(unsigned int j=0; j<(*iter)->tex_polygons[i].size(); ++j)
01027 {
01028 pcl::Vertices polygon = (*iter)->tex_polygons[i][j];
01029 for(unsigned int k=0; k<polygon.vertices.size(); ++k)
01030 {
01031 polygon.vertices[k] += polygonStep;
01032 }
01033 output->tex_polygons[index][oi+j] = polygon;
01034 }
01035
01036
01037 oi = output->tex_coordinates[index].size();
01038 output->tex_coordinates[index].resize(output->tex_coordinates[index].size() + (*iter)->tex_coordinates[i].size());
01039 for(unsigned int j=0; j<(*iter)->tex_coordinates[i].size(); ++j)
01040 {
01041 output->tex_coordinates[index][oi+j] = (*iter)->tex_coordinates[i][j];
01042 }
01043 }
01044 }
01045 }
01046 return output;
01047 }
01048
01049 int gcd(int a, int b) {
01050 return b == 0 ? a : gcd(b, a % b);
01051 }
01052
01053 void concatenateTextureMaterials(pcl::TextureMesh & mesh, const cv::Size & imageSize, int textureSize, int maxTextures, float & scale, std::vector<bool> * materialsKept)
01054 {
01055 UASSERT(textureSize>0 && imageSize.width>0 && imageSize.height>0);
01056 if(maxTextures < 1)
01057 {
01058 maxTextures = 1;
01059 }
01060 int materials = 0;
01061 for(unsigned int i=0; i<mesh.tex_materials.size(); ++i)
01062 {
01063 if(mesh.tex_polygons.size())
01064 {
01065 ++materials;
01066 }
01067 }
01068 if(materials)
01069 {
01070 int w = imageSize.width;
01071 int h = imageSize.height;
01072 int g = gcd(w,h);
01073 int a = w/g;
01074 int b = h/g;
01075 UDEBUG("w=%d h=%d g=%d a=%d b=%d", w, h, g, a, b);
01076 int colCount = 0;
01077 int rowCount = 0;
01078 float factor = 0.1f;
01079 float epsilon = 0.001f;
01080 scale = 1.0f;
01081 while((colCount*rowCount)*maxTextures < materials || (factor == 0.1f || scale > 1.0f))
01082 {
01083
01084 if(factor!=0.1f)
01085 {
01086 scale = float(textureSize)/float(w*b*factor);
01087 }
01088 colCount = float(textureSize)/(scale*float(w));
01089 rowCount = float(textureSize)/(scale*float(h));
01090 factor+=epsilon;
01091 }
01092 int outputTextures = (materials / (colCount*rowCount)) + (materials % (colCount*rowCount) > 0?1:0);
01093 UDEBUG("materials=%d col=%d row=%d output textures=%d factor=%f scale=%f", materials, colCount, rowCount, outputTextures, factor-epsilon, scale);
01094
01095 UASSERT(mesh.tex_coordinates.size() == mesh.tex_materials.size() && mesh.tex_polygons.size() == mesh.tex_materials.size());
01096
01097
01098 std::vector<int> totalPolygons(outputTextures, 0);
01099 std::vector<int> totalCoordinates(outputTextures, 0);
01100 int count = 0;
01101 for(unsigned int i=0; i<mesh.tex_materials.size(); ++i)
01102 {
01103 if(mesh.tex_polygons[i].size())
01104 {
01105 int indexMaterial = count / (colCount*rowCount);
01106 UASSERT(indexMaterial < outputTextures);
01107
01108 totalPolygons[indexMaterial]+=mesh.tex_polygons[i].size();
01109 totalCoordinates[indexMaterial]+=mesh.tex_coordinates[i].size();
01110
01111 ++count;
01112 }
01113 }
01114
01115 pcl::TextureMesh outputMesh;
01116
01117 int pi = 0;
01118 int ci = 0;
01119 int ti=0;
01120 float scaledHeight = float(int(scale*float(h)))/float(textureSize);
01121 float scaledWidth = float(int(scale*float(w)))/float(textureSize);
01122 float lowerBorderSize = 1.0f - scaledHeight*float(rowCount);
01123 UDEBUG("scaledWidth=%f scaledHeight=%f lowerBorderSize=%f", scaledWidth, scaledHeight, lowerBorderSize);
01124 if(materialsKept)
01125 {
01126 materialsKept->resize(mesh.tex_materials.size(), false);
01127 }
01128 for(unsigned int t=0; t<mesh.tex_materials.size(); ++t)
01129 {
01130 if(mesh.tex_polygons[t].size())
01131 {
01132 int indexMaterial = ti / (colCount*rowCount);
01133 UASSERT(indexMaterial < outputTextures);
01134 if((int)outputMesh.tex_polygons.size() <= indexMaterial)
01135 {
01136 std::vector<pcl::Vertices> newPolygons(totalPolygons[indexMaterial]);
01137 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
01138 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > newCoordinates(totalCoordinates[indexMaterial]);
01139 #else
01140 std::vector<Eigen::Vector2f> newCoordinates(totalCoordinates[indexMaterial]);
01141 #endif
01142 outputMesh.tex_polygons.push_back(newPolygons);
01143 outputMesh.tex_coordinates.push_back(newCoordinates);
01144
01145 pi=0;
01146 ci=0;
01147 }
01148
01149 int row = (ti/colCount) % rowCount;
01150 int col = ti%colCount;
01151 float offsetU = scaledWidth * float(col);
01152 float offsetV = scaledHeight * float((rowCount - 1) - row) + lowerBorderSize;
01153
01154
01155 for(unsigned int i=0; i<mesh.tex_polygons[t].size(); ++i)
01156 {
01157 UASSERT(pi < (int)outputMesh.tex_polygons[indexMaterial].size());
01158 outputMesh.tex_polygons[indexMaterial][pi++] = mesh.tex_polygons[t].at(i);
01159 }
01160
01161 for(unsigned int i=0; i<mesh.tex_coordinates[t].size(); ++i)
01162 {
01163 const Eigen::Vector2f & v = mesh.tex_coordinates[t].at(i);
01164 if(v[0] >= 0 && v[1] >=0)
01165 {
01166 outputMesh.tex_coordinates[indexMaterial][ci][0] = v[0]*scaledWidth + offsetU;
01167 outputMesh.tex_coordinates[indexMaterial][ci][1] = v[1]*scaledHeight + offsetV;
01168 }
01169 else
01170 {
01171 outputMesh.tex_coordinates[indexMaterial][ci] = v;
01172 }
01173 ++ci;
01174 }
01175 ++ti;
01176 if(materialsKept)
01177 {
01178 materialsKept->at(t) = true;
01179 }
01180 }
01181 }
01182 pcl::TexMaterial m = mesh.tex_materials.front();
01183 mesh.tex_materials.clear();
01184 for(int i=0; i<outputTextures; ++i)
01185 {
01186 m.tex_file = "texture";
01187 m.tex_name = "material";
01188 if(outputTextures > 1)
01189 {
01190 m.tex_file += uNumber2Str(i);
01191 m.tex_name += uNumber2Str(i);
01192 }
01193
01194 mesh.tex_materials.push_back(m);
01195 }
01196 mesh.tex_coordinates = outputMesh.tex_coordinates;
01197 mesh.tex_polygons = outputMesh.tex_polygons;
01198 }
01199 }
01200
01201 std::vector<std::vector<unsigned int> > convertPolygonsFromPCL(const std::vector<pcl::Vertices> & polygons)
01202 {
01203 std::vector<std::vector<unsigned int> > polygonsOut(polygons.size());
01204 for(unsigned int p=0; p<polygons.size(); ++p)
01205 {
01206 polygonsOut[p] = polygons[p].vertices;
01207 }
01208 return polygonsOut;
01209 }
01210 std::vector<std::vector<std::vector<unsigned int> > > convertPolygonsFromPCL(const std::vector<std::vector<pcl::Vertices> > & tex_polygons)
01211 {
01212 std::vector<std::vector<std::vector<unsigned int> > > polygonsOut(tex_polygons.size());
01213 for(unsigned int t=0; t<tex_polygons.size(); ++t)
01214 {
01215 polygonsOut[t].resize(tex_polygons[t].size());
01216 for(unsigned int p=0; p<tex_polygons[t].size(); ++p)
01217 {
01218 polygonsOut[t][p] = tex_polygons[t][p].vertices;
01219 }
01220 }
01221 return polygonsOut;
01222 }
01223 std::vector<pcl::Vertices> convertPolygonsToPCL(const std::vector<std::vector<unsigned int> > & polygons)
01224 {
01225 std::vector<pcl::Vertices> polygonsOut(polygons.size());
01226 for(unsigned int p=0; p<polygons.size(); ++p)
01227 {
01228 polygonsOut[p].vertices = polygons[p];
01229 }
01230 return polygonsOut;
01231 }
01232 std::vector<std::vector<pcl::Vertices> > convertPolygonsToPCL(const std::vector<std::vector<std::vector<unsigned int> > > & tex_polygons)
01233 {
01234 std::vector<std::vector<pcl::Vertices> > polygonsOut(tex_polygons.size());
01235 for(unsigned int t=0; t<tex_polygons.size(); ++t)
01236 {
01237 polygonsOut[t].resize(tex_polygons[t].size());
01238 for(unsigned int p=0; p<tex_polygons[t].size(); ++p)
01239 {
01240 polygonsOut[t][p].vertices = tex_polygons[t][p];
01241 }
01242 }
01243 return polygonsOut;
01244 }
01245
01246 pcl::TextureMesh::Ptr assembleTextureMesh(
01247 const cv::Mat & cloudMat,
01248 const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
01249 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
01250 const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
01251 #else
01252 const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
01253 #endif
01254 cv::Mat & textures,
01255 bool mergeTextures)
01256 {
01257 pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
01258
01259 if(cloudMat.channels() <= 3)
01260 {
01261 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(cloudMat));
01262 pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
01263 }
01264 else if(cloudMat.channels() == 4)
01265 {
01266 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(cloudMat));
01267 pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
01268 }
01269 else if(cloudMat.channels() == 6)
01270 {
01271 pcl::PointCloud<pcl::PointNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudNormal(LaserScan::backwardCompatibility(cloudMat));
01272 pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
01273 }
01274 else if(cloudMat.channels() == 7)
01275 {
01276 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGBNormal(LaserScan::backwardCompatibility(cloudMat));
01277 pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
01278 }
01279
01280 if(textureMesh->cloud.data.size() && polygons.size())
01281 {
01282 textureMesh->tex_polygons.resize(polygons.size());
01283 for(unsigned int t=0; t<polygons.size(); ++t)
01284 {
01285 textureMesh->tex_polygons[t].resize(polygons[t].size());
01286 for(unsigned int p=0; p<polygons[t].size(); ++p)
01287 {
01288 textureMesh->tex_polygons[t][p].vertices = polygons[t][p];
01289 }
01290 }
01291
01292 if(!texCoords.empty() && !textures.empty())
01293 {
01294 textureMesh->tex_coordinates = texCoords;
01295
01296 textureMesh->tex_materials.resize (textureMesh->tex_coordinates.size());
01297 for(unsigned int i = 0 ; i < textureMesh->tex_coordinates.size() ; ++i)
01298 {
01299 pcl::TexMaterial mesh_material;
01300 mesh_material.tex_Ka.r = 0.2f;
01301 mesh_material.tex_Ka.g = 0.2f;
01302 mesh_material.tex_Ka.b = 0.2f;
01303
01304 mesh_material.tex_Kd.r = 0.8f;
01305 mesh_material.tex_Kd.g = 0.8f;
01306 mesh_material.tex_Kd.b = 0.8f;
01307
01308 mesh_material.tex_Ks.r = 1.0f;
01309 mesh_material.tex_Ks.g = 1.0f;
01310 mesh_material.tex_Ks.b = 1.0f;
01311
01312 mesh_material.tex_d = 1.0f;
01313 mesh_material.tex_Ns = 75.0f;
01314 mesh_material.tex_illum = 2;
01315
01316 std::stringstream tex_name;
01317 tex_name << "material_" << i;
01318 tex_name >> mesh_material.tex_name;
01319
01320 mesh_material.tex_file = uFormat("%d", i);
01321
01322 textureMesh->tex_materials[i] = mesh_material;
01323 }
01324
01325 if(mergeTextures && textures.cols/textures.rows > 1)
01326 {
01327 UASSERT(textures.cols % textures.rows == 0 && textures.cols/textures.rows == (int)textureMesh->tex_coordinates.size());
01328 std::vector<bool> materialsKept;
01329 float scale = 0.0f;
01330 cv::Size imageSize(textures.rows, textures.rows);
01331 int imageType = textures.type();
01332 rtabmap::util3d::concatenateTextureMaterials(*textureMesh, imageSize, textures.rows, 1, scale, &materialsKept);
01333 if(scale && textureMesh->tex_materials.size() == 1)
01334 {
01335 int cols = float(textures.rows)/(scale*imageSize.width);
01336 int rows = float(textures.rows)/(scale*imageSize.height);
01337
01338 cv::Mat mergedTextures = cv::Mat(textures.rows, textures.rows, imageType, cv::Scalar::all(255));
01339
01340
01341 cv::Size resizedImageSize(int(imageSize.width*scale), int(imageSize.height*scale));
01342 int oi=0;
01343 for(int i=0; i<(int)materialsKept.size(); ++i)
01344 {
01345 if(materialsKept.at(i))
01346 {
01347 int u = oi%cols * resizedImageSize.width;
01348 int v = ((oi/cols) % rows ) * resizedImageSize.height;
01349 UASSERT(u < textures.rows-resizedImageSize.width);
01350 UASSERT(v < textures.rows-resizedImageSize.height);
01351
01352 cv::Mat resizedImage;
01353 cv::resize(textures(cv::Range::all(), cv::Range(i*textures.rows, (i+1)*textures.rows)), resizedImage, resizedImageSize, 0.0f, 0.0f, cv::INTER_AREA);
01354
01355 UASSERT(resizedImage.type() == mergedTextures.type());
01356 resizedImage.copyTo(mergedTextures(cv::Rect(u, v, resizedImage.cols, resizedImage.rows)));
01357
01358 ++oi;
01359 }
01360 }
01361 textures = mergedTextures;
01362 }
01363 }
01364 }
01365 }
01366 return textureMesh;
01367 }
01368
01369 pcl::PolygonMesh::Ptr assemblePolygonMesh(
01370 const cv::Mat & cloudMat,
01371 const std::vector<std::vector<unsigned int> > & polygons)
01372 {
01373 pcl::PolygonMesh::Ptr polygonMesh(new pcl::PolygonMesh);
01374
01375 if(cloudMat.channels() <= 3)
01376 {
01377 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(cloudMat));
01378 pcl::toPCLPointCloud2(*cloud, polygonMesh->cloud);
01379 }
01380 else if(cloudMat.channels() == 4)
01381 {
01382 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(cloudMat));
01383 pcl::toPCLPointCloud2(*cloud, polygonMesh->cloud);
01384 }
01385 else if(cloudMat.channels() == 6)
01386 {
01387 pcl::PointCloud<pcl::PointNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudNormal(LaserScan::backwardCompatibility(cloudMat));
01388 pcl::toPCLPointCloud2(*cloud, polygonMesh->cloud);
01389 }
01390 else if(cloudMat.channels() == 7)
01391 {
01392 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGBNormal(LaserScan::backwardCompatibility(cloudMat));
01393 pcl::toPCLPointCloud2(*cloud, polygonMesh->cloud);
01394 }
01395
01396 if(polygonMesh->cloud.data.size() && polygons.size())
01397 {
01398 polygonMesh->polygons.resize(polygons.size());
01399 for(unsigned int p=0; p<polygons.size(); ++p)
01400 {
01401 polygonMesh->polygons[p].vertices = polygons[p];
01402 }
01403 }
01404 return polygonMesh;
01405 }
01406
01407 double sqr(uchar v)
01408 {
01409 return double(v)*double(v);
01410 }
01411
01412 cv::Mat mergeTextures(
01413 pcl::TextureMesh & mesh,
01414 const std::map<int, cv::Mat> & images,
01415 const std::map<int, CameraModel> & calibrations,
01416 const Memory * memory,
01417 const DBDriver * dbDriver,
01418 int textureSize,
01419 int textureCount,
01420 const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels,
01421 bool gainCompensation,
01422 float gainBeta,
01423 bool gainRGB,
01424 bool blending,
01425 int blendingDecimation,
01426 int brightnessContrastRatioLow,
01427 int brightnessContrastRatioHigh,
01428 bool exposureFusion,
01429 const ProgressState * state)
01430 {
01431 std::map<int, std::vector<CameraModel> > calibVectors;
01432 for(std::map<int, CameraModel>::const_iterator iter=calibrations.begin(); iter!=calibrations.end(); ++iter)
01433 {
01434 std::vector<CameraModel> m;
01435 m.push_back(iter->second);
01436 calibVectors.insert(std::make_pair(iter->first, m));
01437 }
01438 return mergeTextures(mesh,
01439 images,
01440 calibVectors,
01441 memory,
01442 dbDriver,
01443 textureSize,
01444 textureCount,
01445 vertexToPixels,
01446 gainCompensation,
01447 gainBeta,
01448 gainRGB,
01449 blending,
01450 blendingDecimation,
01451 brightnessContrastRatioLow,
01452 brightnessContrastRatioHigh,
01453 exposureFusion,
01454 state);
01455 }
01456 cv::Mat mergeTextures(
01457 pcl::TextureMesh & mesh,
01458 const std::map<int, cv::Mat> & images,
01459 const std::map<int, std::vector<CameraModel> > & calibrations,
01460 const Memory * memory,
01461 const DBDriver * dbDriver,
01462 int textureSize,
01463 int textureCount,
01464 const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels,
01465 bool gainCompensation,
01466 float gainBeta,
01467 bool gainRGB,
01468 bool blending,
01469 int blendingDecimation,
01470 int brightnessContrastRatioLow,
01471 int brightnessContrastRatioHigh,
01472 bool exposureFusion,
01473 const ProgressState * state)
01474 {
01475
01476 UASSERT(textureSize%256 == 0);
01477 UDEBUG("textureSize = %d", textureSize);
01478 cv::Mat globalTextures;
01479 if(mesh.tex_materials.size() > 1)
01480 {
01481 std::vector<std::pair<int, int> > textures(mesh.tex_materials.size(), std::pair<int, int>(-1,-1));
01482 cv::Size imageSize;
01483 const int imageType=CV_8UC3;
01484
01485 UDEBUG("");
01486 for(unsigned int i=0; i<mesh.tex_materials.size(); ++i)
01487 {
01488 std::list<std::string> texFileSplit = uSplit(mesh.tex_materials[i].tex_file, '_');
01489 if(!mesh.tex_materials[i].tex_file.empty() &&
01490 mesh.tex_polygons[i].size() &&
01491 uIsInteger(texFileSplit.front(), false))
01492 {
01493 textures[i].first = uStr2Int(texFileSplit.front());
01494 if(texFileSplit.size() == 2 &&
01495 uIsInteger(texFileSplit.back(), false) )
01496 {
01497 textures[i].second = uStr2Int(texFileSplit.back());
01498 }
01499
01500 int textureId = textures[i].first;
01501 if(imageSize.width == 0 || imageSize.height == 0)
01502 {
01503 if(images.find(textureId) != images.end() &&
01504 !images.find(textureId)->second.empty() &&
01505 calibrations.find(textureId) != calibrations.end())
01506 {
01507 const std::vector<CameraModel> & models = calibrations.find(textureId)->second;
01508 UASSERT(models.size()>=1);
01509 if( models[0].imageHeight()>0 &&
01510 models[0].imageWidth()>0)
01511 {
01512 imageSize = models[0].imageSize();
01513 }
01514 else if(images.find(textureId)!=images.end())
01515 {
01516
01517 cv::Mat image = images.find(textureId)->second;
01518 if(image.rows == 1 && image.type() == CV_8UC1)
01519 {
01520 image = uncompressImage(image);
01521 }
01522 UASSERT(!image.empty());
01523 imageSize = image.size();
01524 if(models.size()>1)
01525 {
01526 imageSize.width/=models.size();
01527 }
01528 }
01529 }
01530 else if(memory)
01531 {
01532 SensorData data = memory->getSignatureDataConst(textureId, true, false, false, false);
01533 std::vector<CameraModel> models = data.cameraModels();
01534 StereoCameraModel stereoModel = data.stereoCameraModel();
01535 if(models.size()>=1 &&
01536 models[0].imageHeight()>0 &&
01537 models[0].imageWidth()>0)
01538 {
01539 imageSize = models[0].imageSize();
01540 }
01541 else if(stereoModel.left().imageHeight() > 0 &&
01542 stereoModel.left().imageWidth() > 0)
01543 {
01544 imageSize = stereoModel.left().imageSize();
01545 }
01546 else
01547 {
01548 cv::Mat image;
01549 data.uncompressDataConst(&image, 0);
01550 UASSERT(!image.empty());
01551 imageSize = image.size();
01552 if(data.cameraModels().size()>1)
01553 {
01554 imageSize.width/=data.cameraModels().size();
01555 }
01556 }
01557 }
01558 else if(dbDriver)
01559 {
01560 std::vector<CameraModel> models;
01561 StereoCameraModel stereoModel;
01562 dbDriver->getCalibration(textureId, models, stereoModel);
01563 if(models.size()>=1 &&
01564 models[0].imageHeight()>0 &&
01565 models[0].imageWidth()>0)
01566 {
01567 imageSize = models[0].imageSize();
01568 }
01569 else if(stereoModel.left().imageHeight() > 0 &&
01570 stereoModel.left().imageWidth() > 0)
01571 {
01572 imageSize = stereoModel.left().imageSize();
01573 }
01574 else
01575 {
01576 SensorData data;
01577 dbDriver->getNodeData(textureId, data, true, false, false, false);
01578 cv::Mat image;
01579 data.uncompressDataConst(&image, 0);
01580 UASSERT(!image.empty());
01581 imageSize = image.size();
01582 if(data.cameraModels().size()>1)
01583 {
01584 imageSize.width/=data.cameraModels().size();
01585 }
01586 }
01587 }
01588 }
01589 }
01590 else if(mesh.tex_polygons[i].size() && mesh.tex_materials[i].tex_file.compare("occluded")!=0)
01591 {
01592 UWARN("Failed parsing texture file name: %s", mesh.tex_materials[i].tex_file.c_str());
01593 }
01594 }
01595 UDEBUG("textures=%d imageSize=%dx%d", (int)textures.size(), imageSize.height, imageSize.width);
01596 if(textures.size() && imageSize.height>0 && imageSize.width>0)
01597 {
01598 float scale = 0.0f;
01599 UDEBUG("");
01600 std::vector<bool> materialsKept;
01601 util3d::concatenateTextureMaterials(mesh, imageSize, textureSize, textureCount, scale, &materialsKept);
01602 if(scale && mesh.tex_materials.size())
01603 {
01604 int materials = (int)mesh.tex_materials.size();
01605 int cols = float(textureSize)/(scale*imageSize.width);
01606 int rows = float(textureSize)/(scale*imageSize.height);
01607
01608 globalTextures = cv::Mat(textureSize, materials*textureSize, imageType, cv::Scalar::all(255));
01609 cv::Mat globalTextureMasks = cv::Mat(textureSize, materials*textureSize, CV_8UC1, cv::Scalar::all(0));
01610
01611
01612 cv::Mat previousImage;
01613 int previousTextureId = 0;
01614 std::vector<CameraModel> previousCameraModels;
01615
01616
01617 cv::Mat emptyImage(int(imageSize.height*scale), int(imageSize.width*scale), imageType, cv::Scalar::all(255));
01618 cv::Mat emptyImageMask(int(imageSize.height*scale), int(imageSize.width*scale), CV_8UC1, cv::Scalar::all(255));
01619 int oi=0;
01620 std::vector<cv::Point2i> imageOrigin(textures.size());
01621 std::vector<int> newCamIndex(textures.size(), -1);
01622 for(int t=0; t<(int)textures.size(); ++t)
01623 {
01624 if(materialsKept.at(t))
01625 {
01626 int indexMaterial = oi / (cols*rows);
01627 UASSERT(indexMaterial < materials);
01628
01629 newCamIndex[t] = oi;
01630 int u = oi%cols * emptyImage.cols;
01631 int v = ((oi/cols) % rows ) * emptyImage.rows;
01632 UASSERT_MSG(u < textureSize-emptyImage.cols, uFormat("u=%d textureSize=%d emptyImage.cols=%d", u, textureSize, emptyImage.cols).c_str());
01633 UASSERT_MSG(v < textureSize-emptyImage.rows, uFormat("v=%d textureSize=%d emptyImage.rows=%d", v, textureSize, emptyImage.rows).c_str());
01634 imageOrigin[t].x = u;
01635 imageOrigin[t].y = v;
01636 if(textures[t].first>=0)
01637 {
01638 cv::Mat image;
01639 std::vector<CameraModel> models;
01640
01641 if(textures[t].first == previousTextureId)
01642 {
01643 image = previousImage;
01644 models = previousCameraModels;
01645 }
01646 else
01647 {
01648 if(images.find(textures[t].first) != images.end() &&
01649 !images.find(textures[t].first)->second.empty() &&
01650 calibrations.find(textures[t].first) != calibrations.end())
01651 {
01652 image = images.find(textures[t].first)->second;
01653 if(image.rows == 1 && image.type() == CV_8UC1)
01654 {
01655 image = uncompressImage(image);
01656 }
01657 models = calibrations.find(textures[t].first)->second;
01658 }
01659 else if(memory)
01660 {
01661 SensorData data = memory->getSignatureDataConst(textures[t].first, true, false, false, false);
01662 models = data.cameraModels();
01663 data.uncompressDataConst(&image, 0);
01664 }
01665 else if(dbDriver)
01666 {
01667 SensorData data;
01668 dbDriver->getNodeData(textures[t].first, data, true, false, false, false);
01669 data.uncompressDataConst(&image, 0);
01670 StereoCameraModel stereoModel;
01671 dbDriver->getCalibration(textures[t].first, models, stereoModel);
01672 }
01673
01674 previousImage = image;
01675 previousCameraModels = models;
01676 previousTextureId = textures[t].first;
01677 }
01678
01679 UASSERT(!image.empty());
01680
01681 if(textures[t].second>=0)
01682 {
01683 UASSERT(textures[t].second < (int)models.size());
01684 int width = image.cols/models.size();
01685 image = image.colRange(width*textures[t].second, width*(textures[t].second+1));
01686 }
01687
01688 cv::Mat resizedImage;
01689 cv::resize(image, resizedImage, emptyImage.size(), 0.0f, 0.0f, cv::INTER_AREA);
01690 UASSERT(resizedImage.type() == CV_8UC1 || resizedImage.type() == CV_8UC3);
01691 if(resizedImage.type() == CV_8UC1)
01692 {
01693 cv::Mat resizedImageColor;
01694 cv::cvtColor(resizedImage, resizedImageColor, CV_GRAY2BGR);
01695 resizedImage = resizedImageColor;
01696 }
01697 UASSERT(resizedImage.type() == globalTextures.type());
01698 resizedImage.copyTo(globalTextures(cv::Rect(u+indexMaterial*globalTextures.rows, v, resizedImage.cols, resizedImage.rows)));
01699 emptyImageMask.copyTo(globalTextureMasks(cv::Rect(u+indexMaterial*globalTextureMasks.rows, v, resizedImage.cols, resizedImage.rows)));
01700 }
01701 else
01702 {
01703 emptyImage.copyTo(globalTextures(cv::Rect(u+indexMaterial*globalTextures.rows, v, emptyImage.cols, emptyImage.rows)));
01704 }
01705 ++oi;
01706 }
01707
01708 if(state)
01709 {
01710 if(state->isCanceled())
01711 {
01712 return cv::Mat();
01713 }
01714 state->callback(uFormat("Assembled texture %d/%d.", t+1, (int)textures.size()));
01715 }
01716 }
01717
01718 UTimer timer;
01719 if(vertexToPixels.size())
01720 {
01721
01722
01723
01724 if(gainCompensation)
01725 {
01730 const int num_images = static_cast<int>(oi);
01731 cv::Mat_<int> N(num_images, num_images); N.setTo(0);
01732 cv::Mat_<double> I(num_images, num_images); I.setTo(0);
01733
01734 cv::Mat_<double> IR(num_images, num_images); IR.setTo(0);
01735 cv::Mat_<double> IG(num_images, num_images); IG.setTo(0);
01736 cv::Mat_<double> IB(num_images, num_images); IB.setTo(0);
01737
01738
01739 for(unsigned int p=0; p<vertexToPixels.size(); ++p)
01740 {
01741 for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[p].begin(); iter!=vertexToPixels[p].end(); ++iter)
01742 {
01743 if(materialsKept.at(iter->first))
01744 {
01745 N(newCamIndex[iter->first], newCamIndex[iter->first]) +=1;
01746
01747 std::map<int, pcl::PointXY>::const_iterator jter=iter;
01748 ++jter;
01749 int k = 1;
01750 for(; jter!=vertexToPixels[p].end(); ++jter, ++k)
01751 {
01752 if(materialsKept.at(jter->first))
01753 {
01754 int i = newCamIndex[iter->first];
01755 int j = newCamIndex[jter->first];
01756
01757 N(i, j) += 1;
01758 N(j, i) += 1;
01759
01760 int indexMaterial = i / (cols*rows);
01761
01762
01763 int ui = iter->second.x*emptyImage.cols + imageOrigin[iter->first].x;
01764 int vi = (1.0-iter->second.y)*emptyImage.rows + imageOrigin[iter->first].y;
01765 int uj = jter->second.x*emptyImage.cols + imageOrigin[jter->first].x;
01766 int vj = (1.0-jter->second.y)*emptyImage.rows + imageOrigin[jter->first].y;
01767 cv::Vec3b * pt1 = globalTextures.ptr<cv::Vec3b>(vi,ui+indexMaterial*globalTextures.rows);
01768 cv::Vec3b * pt2 = globalTextures.ptr<cv::Vec3b>(vj,uj+indexMaterial*globalTextures.rows);
01769
01770 I(i, j) += std::sqrt(static_cast<double>(sqr(pt1->val[0]) + sqr(pt1->val[1]) + sqr(pt1->val[2])));
01771 I(j, i) += std::sqrt(static_cast<double>(sqr(pt2->val[0]) + sqr(pt2->val[1]) + sqr(pt2->val[2])));
01772
01773 IR(i, j) += static_cast<double>(pt1->val[2]);
01774 IR(j, i) += static_cast<double>(pt2->val[2]);
01775 IG(i, j) += static_cast<double>(pt1->val[1]);
01776 IG(j, i) += static_cast<double>(pt2->val[1]);
01777 IB(i, j) += static_cast<double>(pt1->val[0]);
01778 IB(j, i) += static_cast<double>(pt2->val[0]);
01779 }
01780 }
01781 }
01782 }
01783 }
01784
01785 for(int i=0; i<num_images; ++i)
01786 {
01787 for(int j=i; j<num_images; ++j)
01788 {
01789 if(i == j)
01790 {
01791 if(N(i,j) == 0)
01792 {
01793 N(i,j) = 1;
01794 }
01795 }
01796 else if(N(i, j))
01797 {
01798 I(i, j) /= N(i, j);
01799 I(j, i) /= N(j, i);
01800
01801 IR(i, j) /= N(i, j);
01802 IR(j, i) /= N(j, i);
01803 IG(i, j) /= N(i, j);
01804 IG(j, i) /= N(j, i);
01805 IB(i, j) /= N(i, j);
01806 IB(j, i) /= N(j, i);
01807 }
01808 }
01809 }
01810
01811 cv::Mat_<double> A(num_images, num_images); A.setTo(0);
01812 cv::Mat_<double> b(num_images, 1); b.setTo(0);
01813 cv::Mat_<double> AR(num_images, num_images); AR.setTo(0);
01814 cv::Mat_<double> AG(num_images, num_images); AG.setTo(0);
01815 cv::Mat_<double> AB(num_images, num_images); AB.setTo(0);
01816 double alpha = 0.01;
01817 double beta = gainBeta;
01818 for (int i = 0; i < num_images; ++i)
01819 {
01820 for (int j = 0; j < num_images; ++j)
01821 {
01822 b(i, 0) += beta * N(i, j);
01823 A(i, i) += beta * N(i, j);
01824 AR(i, i) += beta * N(i, j);
01825 AG(i, i) += beta * N(i, j);
01826 AB(i, i) += beta * N(i, j);
01827 if (j == i) continue;
01828 A(i, i) += 2 * alpha * I(i, j) * I(i, j) * N(i, j);
01829 A(i, j) -= 2 * alpha * I(i, j) * I(j, i) * N(i, j);
01830
01831 AR(i, i) += 2 * alpha * IR(i, j) * IR(i, j) * N(i, j);
01832 AR(i, j) -= 2 * alpha * IR(i, j) * IR(j, i) * N(i, j);
01833
01834 AG(i, i) += 2 * alpha * IG(i, j) * IG(i, j) * N(i, j);
01835 AG(i, j) -= 2 * alpha * IG(i, j) * IG(j, i) * N(i, j);
01836
01837 AB(i, i) += 2 * alpha * IB(i, j) * IB(i, j) * N(i, j);
01838 AB(i, j) -= 2 * alpha * IB(i, j) * IB(j, i) * N(i, j);
01839 }
01840 }
01841
01842 cv::Mat_<double> gainsGray, gainsR, gainsG, gainsB;
01843 cv::solve(A, b, gainsGray);
01844
01845 cv::solve(AR, b, gainsR);
01846 cv::solve(AG, b, gainsG);
01847 cv::solve(AB, b, gainsB);
01848
01849 cv::Mat_<double> gains(gainsGray.rows, 4);
01850 gainsGray.copyTo(gains.col(0));
01851 gainsR.copyTo(gains.col(1));
01852 gainsG.copyTo(gains.col(2));
01853 gainsB.copyTo(gains.col(3));
01854
01855 for(int t=0; t<(int)textures.size(); ++t)
01856 {
01857
01858 if(materialsKept.at(t))
01859 {
01860 int u = imageOrigin[t].x;
01861 int v = imageOrigin[t].y;
01862
01863 UDEBUG("Gain cam%d = %f", newCamIndex[t], gainsGray(newCamIndex[t], 0));
01864
01865 int indexMaterial = newCamIndex[t] / (cols*rows);
01866 cv::Mat roi = globalTextures(cv::Rect(u+indexMaterial*globalTextures.rows, v, emptyImage.cols, emptyImage.rows));
01867
01868 std::vector<cv::Mat> channels;
01869 cv::split(roi, channels);
01870
01871
01872 cv::multiply(channels[0], gains(newCamIndex[t], gainRGB?3:0), channels[0]);
01873 cv::multiply(channels[1], gains(newCamIndex[t], gainRGB?2:0), channels[1]);
01874 cv::multiply(channels[2], gains(newCamIndex[t], gainRGB?1:0), channels[2]);
01875
01876 cv::merge(channels, roi);
01877 }
01878 }
01879
01880
01881 if(state) state->callback(uFormat("Gain compensation %fs", timer.ticks()));
01882 }
01883
01884 if(blending)
01885 {
01886
01887 int decimation = 1;
01888 if(blendingDecimation <= 0)
01889 {
01890
01891 std::vector<float> edgeLengths;
01892 if(mesh.tex_coordinates.size() && mesh.tex_coordinates[0].size())
01893 {
01894 UASSERT(mesh.tex_polygons.size() && mesh.tex_polygons[0].size() && mesh.tex_polygons[0][0].vertices.size());
01895 int polygonSize = mesh.tex_polygons[0][0].vertices.size();
01896 UDEBUG("polygon size=%d", polygonSize);
01897
01898 for(unsigned int k=0; k<mesh.tex_coordinates.size(); ++k)
01899 {
01900 for(unsigned int i=0; i<mesh.tex_coordinates[k].size(); i+=polygonSize)
01901 {
01902 for(int j=0; j<polygonSize; ++j)
01903 {
01904 const Eigen::Vector2f & uc1 = mesh.tex_coordinates[k][i + j];
01905 const Eigen::Vector2f & uc2 = mesh.tex_coordinates[k][i + (j+1)%polygonSize];
01906 Eigen::Vector2f edge = (uc1-uc2)*textureSize;
01907 edgeLengths.push_back(fabs(edge[0]));
01908 edgeLengths.push_back(fabs(edge[1]));
01909 }
01910 }
01911 }
01912 float edgeLength = 0.0f;
01913 if(edgeLengths.size())
01914 {
01915 std::sort(edgeLengths.begin(), edgeLengths.end());
01916 float m = uMean(edgeLengths.data(), edgeLengths.size());
01917 float stddev = std::sqrt(uVariance(edgeLengths.data(), edgeLengths.size(), m));
01918 edgeLength = m+stddev;
01919 decimation = 1 << 6;
01920 for(int i=1; i<=6; ++i)
01921 {
01922 if(float(1 << i) >= edgeLength)
01923 {
01924 decimation = 1 << i;
01925 break;
01926 }
01927 }
01928 }
01929
01930 UDEBUG("edge length=%f decimation=%d", edgeLength, decimation);
01931 }
01932 }
01933 else
01934 {
01935 if(blendingDecimation > 1)
01936 {
01937 UASSERT(textureSize % blendingDecimation == 0);
01938 }
01939 decimation = blendingDecimation;
01940 UDEBUG("decimation=%d", decimation);
01941 }
01942
01943 std::vector<cv::Mat> blendGains(materials);
01944 for(int i=0; i<materials;++i)
01945 {
01946 blendGains[i] = cv::Mat(globalTextures.rows/decimation, globalTextures.rows/decimation, CV_32FC3, cv::Scalar::all(1.0f));
01947 }
01948
01949 for(unsigned int p=0; p<vertexToPixels.size(); ++p)
01950 {
01951 if(vertexToPixels[p].size() > 1)
01952 {
01953 std::vector<float> gainsB(vertexToPixels[p].size());
01954 std::vector<float> gainsG(vertexToPixels[p].size());
01955 std::vector<float> gainsR(vertexToPixels[p].size());
01956 float sumWeight = 0.0f;
01957 int k=0;
01958 for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[p].begin(); iter!=vertexToPixels[p].end(); ++iter)
01959 {
01960 if(materialsKept.at(iter->first))
01961 {
01962 int u = iter->second.x*emptyImage.cols + imageOrigin[iter->first].x;
01963 int v = (1.0-iter->second.y)*emptyImage.rows + imageOrigin[iter->first].y;
01964 float x = iter->second.x - 0.5f;
01965 float y = iter->second.y - 0.5f;
01966 float weight = 0.7f - sqrt(x*x+y*y);
01967 if(weight<0.0f)
01968 {
01969 weight = 0.0f;
01970 }
01971 int indexMaterial = newCamIndex[iter->first] / (cols*rows);
01972 cv::Vec3b * pt = globalTextures.ptr<cv::Vec3b>(v,u+indexMaterial*globalTextures.rows);
01973 gainsB[k] = static_cast<double>(pt->val[0]) * weight;
01974 gainsG[k] = static_cast<double>(pt->val[1]) * weight;
01975 gainsR[k] = static_cast<double>(pt->val[2]) * weight;
01976 sumWeight += weight;
01977 ++k;
01978 }
01979 }
01980 gainsB.resize(k);
01981 gainsG.resize(k);
01982 gainsR.resize(k);
01983
01984 if(sumWeight > 0)
01985 {
01986 float targetColor[3];
01987 targetColor[0] = uSum(gainsB.data(), gainsB.size()) / sumWeight;
01988 targetColor[1] = uSum(gainsG.data(), gainsG.size()) / sumWeight;
01989 targetColor[2] = uSum(gainsR.data(), gainsR.size()) / sumWeight;
01990 for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[p].begin(); iter!=vertexToPixels[p].end(); ++iter)
01991 {
01992 if(materialsKept.at(iter->first))
01993 {
01994 int u = iter->second.x*emptyImage.cols + imageOrigin[iter->first].x;
01995 int v = (1.0-iter->second.y)*emptyImage.rows + imageOrigin[iter->first].y;
01996 int indexMaterial = newCamIndex[iter->first] / (cols*rows);
01997 cv::Vec3b * pt = globalTextures.ptr<cv::Vec3b>(v,u+indexMaterial*globalTextures.rows);
01998 float gB = targetColor[0]/(pt->val[0]==0?1.0f:pt->val[0]);
01999 float gG = targetColor[1]/(pt->val[1]==0?1.0f:pt->val[1]);
02000 float gR = targetColor[2]/(pt->val[2]==0?1.0f:pt->val[2]);
02001 cv::Vec3f * ptr = blendGains[indexMaterial].ptr<cv::Vec3f>(v/decimation, u/decimation);
02002 ptr->val[0] = (gB>1.3f)?1.3f:(gB<0.7f)?0.7f:gB;
02003 ptr->val[1] = (gG>1.3f)?1.3f:(gG<0.7f)?0.7f:gG;
02004 ptr->val[2] = (gR>1.3f)?1.3f:(gR<0.7f)?0.7f:gR;
02005 }
02006 }
02007 }
02008 }
02009 }
02010
02011 for(int i=0; i<materials; ++i)
02012 {
02013
02014
02015
02016
02017
02018
02019
02020
02021
02022
02023 cv::Mat globalTexturesROI = globalTextures(cv::Range::all(), cv::Range(i*globalTextures.rows, (i+1)*globalTextures.rows));
02024 cv::Mat dst;
02025 cv::blur(blendGains[i], dst, cv::Size(3,3));
02026 cv::resize(dst, blendGains[i], globalTexturesROI.size(), 0, 0, cv::INTER_LINEAR);
02027
02028
02029
02030
02031
02032
02033
02034
02035
02036 cv::multiply(globalTexturesROI, blendGains[i], globalTexturesROI, 1.0, CV_8UC3);
02037
02038
02039
02040 }
02041
02042 if(state) state->callback(uFormat("Blending (decimation=%d) %fs", decimation, timer.ticks()));
02043 }
02044 }
02045
02046 if(brightnessContrastRatioLow > 0 || brightnessContrastRatioHigh > 0)
02047 {
02048 for(int i=0; i<materials; ++i)
02049 {
02050 cv::Mat globalTexturesROI = globalTextures(cv::Range::all(), cv::Range(i*globalTextures.rows, (i+1)*globalTextures.rows));
02051 cv::Mat globalTextureMasksROI = globalTextureMasks(cv::Range::all(), cv::Range(i*globalTextureMasks.rows, (i+1)*globalTextureMasks.rows));
02052 if(exposureFusion)
02053 {
02054 std::vector<cv::Mat> images;
02055 images.push_back(globalTexturesROI);
02056 if (brightnessContrastRatioLow > 0)
02057 {
02058 images.push_back(util2d::brightnessAndContrastAuto(
02059 globalTexturesROI,
02060 globalTextureMasksROI,
02061 (float)brightnessContrastRatioLow,
02062 0.0f));
02063 }
02064 if (brightnessContrastRatioHigh > 0)
02065 {
02066 images.push_back(util2d::brightnessAndContrastAuto(
02067 globalTexturesROI,
02068 globalTextureMasksROI,
02069 0.0f,
02070 (float)brightnessContrastRatioHigh));
02071 }
02072
02073 util2d::exposureFusion(images).copyTo(globalTexturesROI);
02074 }
02075 else
02076 {
02077 util2d::brightnessAndContrastAuto(
02078 globalTexturesROI,
02079 globalTextureMasksROI,
02080 (float)brightnessContrastRatioLow,
02081 (float)brightnessContrastRatioHigh).copyTo(globalTexturesROI);
02082 }
02083 }
02084 if(state) state->callback(uFormat("Brightness and contrast auto %fs", timer.ticks()));
02085 }
02086 }
02087 }
02088 }
02089 UDEBUG("globalTextures=%d", globalTextures.cols?globalTextures.cols / globalTextures.rows:0);
02090 return globalTextures;
02091 }
02092
02093 void fixTextureMeshForVisualization(pcl::TextureMesh & textureMesh)
02094 {
02095
02096
02097
02098
02099 for (unsigned int t = 0; t < textureMesh.tex_coordinates.size(); ++t)
02100 {
02101 if(textureMesh.tex_polygons[t].size())
02102 {
02103 pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
02104 pcl::fromPCLPointCloud2(textureMesh.cloud, *originalCloud);
02105
02106
02107 unsigned int nPoints = textureMesh.tex_coordinates[t].size();
02108 UASSERT(nPoints == textureMesh.tex_polygons[t].size()*textureMesh.tex_polygons[t][0].vertices.size());
02109
02110 pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud(new pcl::PointCloud<pcl::PointXYZ>);
02111 newCloud->resize(nPoints);
02112
02113 unsigned int oi = 0;
02114 for (unsigned int i = 0; i < textureMesh.tex_polygons[t].size(); ++i)
02115 {
02116 pcl::Vertices & vertices = textureMesh.tex_polygons[t][i];
02117
02118 for(unsigned int j=0; j<vertices.vertices.size(); ++j)
02119 {
02120 UASSERT(oi < newCloud->size());
02121 UASSERT_MSG(vertices.vertices[j] < originalCloud->size(), uFormat("%d vs %d", vertices.vertices[j], (int)originalCloud->size()).c_str());
02122 newCloud->at(oi) = originalCloud->at(vertices.vertices[j]);
02123 vertices.vertices[j] = oi;
02124 ++oi;
02125 }
02126 }
02127 pcl::toPCLPointCloud2(*newCloud, textureMesh.cloud);
02128 }
02129 }
02130 }
02131
02132 LaserScan computeNormals(
02133 const LaserScan & laserScan,
02134 int searchK,
02135 float searchRadius)
02136 {
02137 if(laserScan.isEmpty())
02138 {
02139 return laserScan;
02140 }
02141
02142 pcl::PointCloud<pcl::Normal>::Ptr normals;
02143
02144 if(laserScan.hasRGB())
02145 {
02146 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = laserScanToPointCloudRGB(laserScan);
02147 if(cloud->size())
02148 {
02149 UASSERT(!laserScan.is2d());
02150 pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
02151 return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
02152 }
02153 }
02154 else if(laserScan.hasIntensity())
02155 {
02156 pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = laserScanToPointCloudI(laserScan);
02157 if(cloud->size())
02158 {
02159 if(laserScan.is2d())
02160 {
02161 pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
02162 return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
02163 }
02164 else
02165 {
02166 pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
02167 return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
02168 }
02169 }
02170 }
02171 else
02172 {
02173 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = laserScanToPointCloud(laserScan);
02174 if(cloud->size())
02175 {
02176 if(laserScan.is2d())
02177 {
02178 pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
02179 return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
02180 }
02181 else
02182 {
02183 pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
02184 return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
02185 }
02186 }
02187 }
02188 return LaserScan();
02189 }
02190
02191 template<typename PointT>
02192 pcl::PointCloud<pcl::Normal>::Ptr computeNormalsImpl(
02193 const typename pcl::PointCloud<PointT>::Ptr & cloud,
02194 const pcl::IndicesPtr & indices,
02195 int searchK,
02196 float searchRadius,
02197 const Eigen::Vector3f & viewPoint)
02198 {
02199 typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
02200 if(indices->size())
02201 {
02202 tree->setInputCloud(cloud, indices);
02203 }
02204 else
02205 {
02206 tree->setInputCloud (cloud);
02207 }
02208
02209
02210 #ifdef PCL_OMP
02211 pcl::NormalEstimationOMP<PointT, pcl::Normal> n;
02212 #else
02213 pcl::NormalEstimation<PointT, pcl::Normal> n;
02214 #endif
02215 pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
02216 n.setInputCloud (cloud);
02217
02218
02219
02220
02221
02222 n.setSearchMethod (tree);
02223 n.setKSearch (searchK);
02224 n.setRadiusSearch (searchRadius);
02225 n.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
02226 n.compute (*normals);
02227
02228 return normals;
02229 }
02230 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
02231 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
02232 int searchK,
02233 float searchRadius,
02234 const Eigen::Vector3f & viewPoint)
02235 {
02236 pcl::IndicesPtr indices(new std::vector<int>);
02237 return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
02238 }
02239 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
02240 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
02241 int searchK,
02242 float searchRadius,
02243 const Eigen::Vector3f & viewPoint)
02244 {
02245 pcl::IndicesPtr indices(new std::vector<int>);
02246 return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
02247 }
02248 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
02249 const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
02250 int searchK,
02251 float searchRadius,
02252 const Eigen::Vector3f & viewPoint)
02253 {
02254 pcl::IndicesPtr indices(new std::vector<int>);
02255 return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
02256 }
02257 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
02258 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
02259 const pcl::IndicesPtr & indices,
02260 int searchK,
02261 float searchRadius,
02262 const Eigen::Vector3f & viewPoint)
02263 {
02264 return computeNormalsImpl<pcl::PointXYZ>(cloud, indices, searchK, searchRadius, viewPoint);
02265 }
02266 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
02267 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
02268 const pcl::IndicesPtr & indices,
02269 int searchK,
02270 float searchRadius,
02271 const Eigen::Vector3f & viewPoint)
02272 {
02273 return computeNormalsImpl<pcl::PointXYZRGB>(cloud, indices, searchK, searchRadius, viewPoint);
02274 }
02275 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
02276 const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
02277 const pcl::IndicesPtr & indices,
02278 int searchK,
02279 float searchRadius,
02280 const Eigen::Vector3f & viewPoint)
02281 {
02282 return computeNormalsImpl<pcl::PointXYZI>(cloud, indices, searchK, searchRadius, viewPoint);
02283 }
02284
02285 template<typename PointT>
02286 pcl::PointCloud<pcl::Normal>::Ptr computeNormals2DImpl(
02287 const typename pcl::PointCloud<PointT>::Ptr & cloud,
02288 int searchK,
02289 float searchRadius,
02290 const Eigen::Vector3f & viewPoint)
02291 {
02292 UASSERT(searchK>0 || searchRadius>0.0f);
02293 pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
02294
02295 typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
02296 tree->setInputCloud (cloud);
02297
02298 normals->resize(cloud->size());
02299
02300 float bad_point = std::numeric_limits<float>::quiet_NaN ();
02301
02302
02303 for(unsigned int i=0; i<cloud->size(); ++i)
02304 {
02305 const PointT & pt = cloud->at(i);
02306 std::vector<Eigen::Vector3f> neighborNormals;
02307 Eigen::Vector3f direction;
02308 direction[0] = viewPoint[0] - pt.x;
02309 direction[1] = viewPoint[1] - pt.y;
02310 direction[2] = viewPoint[2] - pt.z;
02311
02312 std::vector<int> k_indices;
02313 std::vector<float> k_sqr_distances;
02314 if(searchRadius>0.0f)
02315 {
02316 tree->radiusSearch(cloud->at(i), searchRadius, k_indices, k_sqr_distances, searchK);
02317 }
02318 else
02319 {
02320 tree->nearestKSearch(cloud->at(i), searchK, k_indices, k_sqr_distances);
02321 }
02322
02323 for(unsigned int j=0; j<k_indices.size(); ++j)
02324 {
02325 if(k_indices.at(j) != (int)i)
02326 {
02327 const PointT & pt2 = cloud->at(k_indices.at(j));
02328 Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
02329 Eigen::Vector3f up = v.cross(direction);
02330 Eigen::Vector3f n = up.cross(v);
02331 n.normalize();
02332 neighborNormals.push_back(n);
02333 }
02334 }
02335
02336 if(neighborNormals.empty())
02337 {
02338 normals->at(i).normal_x = bad_point;
02339 normals->at(i).normal_y = bad_point;
02340 normals->at(i).normal_z = bad_point;
02341 }
02342 else
02343 {
02344 Eigen::Vector3f meanNormal(0,0,0);
02345 for(unsigned int j=0; j<neighborNormals.size(); ++j)
02346 {
02347 meanNormal+=neighborNormals[j];
02348 }
02349 meanNormal /= (float)neighborNormals.size();
02350 meanNormal.normalize();
02351 normals->at(i).normal_x = meanNormal[0];
02352 normals->at(i).normal_y = meanNormal[1];
02353 normals->at(i).normal_z = meanNormal[2];
02354 }
02355 }
02356
02357 return normals;
02358 }
02359 pcl::PointCloud<pcl::Normal>::Ptr computeNormals2D(
02360 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
02361 int searchK,
02362 float searchRadius,
02363 const Eigen::Vector3f & viewPoint)
02364 {
02365 return computeNormals2DImpl<pcl::PointXYZ>(cloud, searchK, searchRadius, viewPoint);
02366 }
02367 pcl::PointCloud<pcl::Normal>::Ptr computeNormals2D(
02368 const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
02369 int searchK,
02370 float searchRadius,
02371 const Eigen::Vector3f & viewPoint)
02372 {
02373 return computeNormals2DImpl<pcl::PointXYZI>(cloud, searchK, searchRadius, viewPoint);
02374 }
02375
02376 template<typename PointT>
02377 pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2DImpl(
02378 const typename pcl::PointCloud<PointT>::Ptr & cloud,
02379 int searchK,
02380 float searchRadius,
02381 const Eigen::Vector3f & viewPoint)
02382 {
02383 UASSERT(searchK>0);
02384 pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
02385
02386 normals->resize(cloud->size());
02387 searchRadius *= searchRadius;
02388
02389 float bad_point = std::numeric_limits<float>::quiet_NaN ();
02390
02391
02392 for(int i=0; i<(int)cloud->size(); ++i)
02393 {
02394 int li = i-searchK;
02395 if(li<0)
02396 {
02397 li=0;
02398 }
02399 int hi = i+searchK;
02400 if(hi>=(int)cloud->size())
02401 {
02402 hi=(int)cloud->size()-1;
02403 }
02404
02405
02406 const PointT & pt = cloud->at(i);
02407 std::vector<Eigen::Vector3f> neighborNormals;
02408 Eigen::Vector3f direction;
02409 direction[0] = viewPoint[0] - cloud->at(i).x;
02410 direction[1] = viewPoint[1] - cloud->at(i).y;
02411 direction[2] = viewPoint[2] - cloud->at(i).z;
02412 for(int j=i-1; j>=li; --j)
02413 {
02414 const PointT & pt2 = cloud->at(j);
02415 Eigen::Vector3f vd(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
02416 if(searchRadius<=0.0f || (vd[0]*vd[0] + vd[1]*vd[1] + vd[2]*vd[2]) < searchRadius)
02417 {
02418 Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
02419 Eigen::Vector3f up = v.cross(direction);
02420 Eigen::Vector3f n = up.cross(v);
02421 n.normalize();
02422 neighborNormals.push_back(n);
02423 }
02424 else
02425 {
02426 break;
02427 }
02428 }
02429 for(int j=i+1; j<=hi; ++j)
02430 {
02431 const PointT & pt2 = cloud->at(j);
02432 Eigen::Vector3f vd(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
02433 if(searchRadius<=0.0f || (vd[0]*vd[0] + vd[1]*vd[1] + vd[2]*vd[2]) < searchRadius)
02434 {
02435 Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
02436 Eigen::Vector3f up = v[2]==0.0f?Eigen::Vector3f(0,0,1):v.cross(direction);
02437 Eigen::Vector3f n = up.cross(v);
02438 n.normalize();
02439 neighborNormals.push_back(n);
02440 }
02441 else
02442 {
02443 break;
02444 }
02445 }
02446
02447 if(neighborNormals.empty())
02448 {
02449 normals->at(i).normal_x = bad_point;
02450 normals->at(i).normal_y = bad_point;
02451 normals->at(i).normal_z = bad_point;
02452 }
02453 else
02454 {
02455 Eigen::Vector3f meanNormal(0,0,0);
02456 for(unsigned int j=0; j<neighborNormals.size(); ++j)
02457 {
02458 meanNormal+=neighborNormals[j];
02459 }
02460 meanNormal /= (float)neighborNormals.size();
02461 meanNormal.normalize();
02462 normals->at(i).normal_x = meanNormal[0];
02463 normals->at(i).normal_y = meanNormal[1];
02464 normals->at(i).normal_z = meanNormal[2];
02465 }
02466 }
02467
02468 return normals;
02469 }
02470
02471 pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
02472 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
02473 int searchK,
02474 float searchRadius,
02475 const Eigen::Vector3f & viewPoint)
02476 {
02477 return computeFastOrganizedNormals2DImpl<pcl::PointXYZ>(cloud, searchK, searchRadius, viewPoint);
02478 }
02479 pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
02480 const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
02481 int searchK,
02482 float searchRadius,
02483 const Eigen::Vector3f & viewPoint)
02484 {
02485 return computeFastOrganizedNormals2DImpl<pcl::PointXYZI>(cloud, searchK, searchRadius, viewPoint);
02486 }
02487
02488 pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
02489 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
02490 float maxDepthChangeFactor,
02491 float normalSmoothingSize,
02492 const Eigen::Vector3f & viewPoint)
02493 {
02494 pcl::IndicesPtr indices(new std::vector<int>);
02495 return computeFastOrganizedNormals(cloud, indices, maxDepthChangeFactor, normalSmoothingSize, viewPoint);
02496 }
02497 pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
02498 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
02499 const pcl::IndicesPtr & indices,
02500 float maxDepthChangeFactor,
02501 float normalSmoothingSize,
02502 const Eigen::Vector3f & viewPoint)
02503 {
02504 UASSERT(cloud->isOrganized());
02505
02506 pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
02507 if(indices->size())
02508 {
02509 tree->setInputCloud(cloud, indices);
02510 }
02511 else
02512 {
02513 tree->setInputCloud (cloud);
02514 }
02515
02516
02517 pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
02518 pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
02519 ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
02520 ne.setMaxDepthChangeFactor(maxDepthChangeFactor);
02521 ne.setNormalSmoothingSize(normalSmoothingSize);
02522 ne.setBorderPolicy(ne.BORDER_POLICY_MIRROR);
02523 ne.setInputCloud(cloud);
02524
02525
02526
02527
02528
02529 ne.setSearchMethod(tree);
02530 ne.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
02531 ne.compute(*normals);
02532
02533 return normals;
02534 }
02535
02536 float computeNormalsComplexity(
02537 const LaserScan & scan,
02538 cv::Mat * pcaEigenVectors,
02539 cv::Mat * pcaEigenValues)
02540 {
02541 if(!scan.isEmpty() && (scan.hasNormals()))
02542 {
02543
02544 int sz = static_cast<int>(scan.size()*2);
02545 bool is2d = scan.is2d();
02546 cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
02547 int oi = 0;
02548 int nOffset = 0;
02549 if(!scan.is2d())
02550 {
02551 nOffset+=1;
02552 }
02553 if(scan.hasIntensity() || scan.hasRGB())
02554 {
02555 nOffset+=1;
02556 }
02557 for (int i = 0; i < scan.size(); ++i)
02558 {
02559 const float * ptrScan = scan.data().ptr<float>(0, i);
02560
02561 if(is2d)
02562 {
02563 if(uIsFinite(ptrScan[nOffset+2]) && uIsFinite(ptrScan[nOffset+3]))
02564 {
02565 float * ptr = data_normals.ptr<float>(oi++, 0);
02566 ptr[0] = ptrScan[2];
02567 ptr[1] = ptrScan[3];
02568 }
02569 }
02570 else
02571 {
02572 if(uIsFinite(ptrScan[nOffset+2]) && uIsFinite(ptrScan[nOffset+3]) && uIsFinite(ptrScan[nOffset+4]))
02573 {
02574 float * ptr = data_normals.ptr<float>(oi++, 0);
02575 ptr[0] = ptrScan[3];
02576 ptr[1] = ptrScan[4];
02577 ptr[2] = ptrScan[5];
02578 }
02579 }
02580 }
02581 if(oi>1)
02582 {
02583 cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
02584
02585 if(pcaEigenVectors)
02586 {
02587 *pcaEigenVectors = pca_analysis.eigenvectors;
02588 }
02589 if(pcaEigenValues)
02590 {
02591 *pcaEigenValues = pca_analysis.eigenvalues;
02592 }
02593
02594
02595 return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
02596 }
02597 }
02598 else if(!scan.isEmpty())
02599 {
02600 UERROR("Scan doesn't have normals!");
02601 }
02602 return 0.0f;
02603 }
02604
02605 float computeNormalsComplexity(
02606 const pcl::PointCloud<pcl::PointNormal> & cloud,
02607 bool is2d,
02608 cv::Mat * pcaEigenVectors,
02609 cv::Mat * pcaEigenValues)
02610 {
02611
02612 int sz = static_cast<int>(cloud.size()*2);
02613 cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
02614 int oi = 0;
02615 for (unsigned int i = 0; i < cloud.size(); ++i)
02616 {
02617 const pcl::PointNormal & pt = cloud.at(i);
02618 if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
02619 {
02620 float * ptr = data_normals.ptr<float>(oi++, 0);
02621 ptr[0] = pt.normal_x;
02622 ptr[1] = pt.normal_y;
02623 if(!is2d)
02624 {
02625 ptr[2] = pt.normal_z;
02626 }
02627 }
02628 }
02629 if(oi>1)
02630 {
02631 cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
02632
02633 if(pcaEigenVectors)
02634 {
02635 *pcaEigenVectors = pca_analysis.eigenvectors;
02636 }
02637 if(pcaEigenValues)
02638 {
02639 *pcaEigenValues = pca_analysis.eigenvalues;
02640 }
02641
02642
02643 return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
02644 }
02645 return 0.0f;
02646 }
02647
02648 float computeNormalsComplexity(
02649 const pcl::PointCloud<pcl::Normal> & normals,
02650 bool is2d,
02651 cv::Mat * pcaEigenVectors,
02652 cv::Mat * pcaEigenValues)
02653 {
02654
02655 int sz = static_cast<int>(normals.size()*2);
02656 cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
02657 int oi = 0;
02658 for (unsigned int i = 0; i < normals.size(); ++i)
02659 {
02660 const pcl::Normal & pt = normals.at(i);
02661 if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
02662 {
02663 float * ptr = data_normals.ptr<float>(oi++, 0);
02664 ptr[0] = pt.normal_x;
02665 ptr[1] = pt.normal_y;
02666 if(!is2d)
02667 {
02668 ptr[2] = pt.normal_z;
02669 }
02670 }
02671 }
02672 if(oi>1)
02673 {
02674 cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
02675
02676 if(pcaEigenVectors)
02677 {
02678 *pcaEigenVectors = pca_analysis.eigenvectors;
02679 }
02680 if(pcaEigenValues)
02681 {
02682 *pcaEigenValues = pca_analysis.eigenvalues;
02683 }
02684
02685
02686 return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
02687 }
02688 return 0.0f;
02689 }
02690
02691 float computeNormalsComplexity(
02692 const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
02693 bool is2d,
02694 cv::Mat * pcaEigenVectors,
02695 cv::Mat * pcaEigenValues)
02696 {
02697
02698 int sz = static_cast<int>(cloud.size()*2);
02699 cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
02700 int oi = 0;
02701 for (unsigned int i = 0; i < cloud.size(); ++i)
02702 {
02703 const pcl::PointXYZRGBNormal & pt = cloud.at(i);
02704 if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
02705 {
02706 float * ptr = data_normals.ptr<float>(oi++, 0);
02707 ptr[0] = pt.normal_x;
02708 ptr[1] = pt.normal_y;
02709 if(!is2d)
02710 {
02711 ptr[2] = pt.normal_z;
02712 }
02713 }
02714 }
02715 if(oi>1)
02716 {
02717 cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
02718
02719 if(pcaEigenVectors)
02720 {
02721 *pcaEigenVectors = pca_analysis.eigenvectors;
02722 }
02723 if(pcaEigenValues)
02724 {
02725 *pcaEigenValues = pca_analysis.eigenvalues;
02726 }
02727
02728
02729 return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
02730 }
02731 return 0.0f;
02732 }
02733
02734 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
02735 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
02736 float searchRadius,
02737 int polygonialOrder,
02738 int upsamplingMethod,
02739 float upsamplingRadius,
02740 float upsamplingStep,
02741 int pointDensity,
02742 float dilationVoxelSize,
02743 int dilationIterations)
02744 {
02745 pcl::IndicesPtr indices(new std::vector<int>);
02746 return mls(cloud,
02747 indices,
02748 searchRadius,
02749 polygonialOrder,
02750 upsamplingMethod,
02751 upsamplingRadius,
02752 upsamplingStep,
02753 pointDensity,
02754 dilationVoxelSize,
02755 dilationIterations);
02756 }
02757
02758 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
02759 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
02760 const pcl::IndicesPtr & indices,
02761 float searchRadius,
02762 int polygonialOrder,
02763 int upsamplingMethod,
02764 float upsamplingRadius,
02765 float upsamplingStep,
02766 int pointDensity,
02767 float dilationVoxelSize,
02768 int dilationIterations)
02769 {
02770 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
02771 pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
02772 if(indices->size())
02773 {
02774 tree->setInputCloud (cloud, indices);
02775 }
02776 else
02777 {
02778 tree->setInputCloud (cloud);
02779 }
02780
02781
02782 pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> mls;
02783
02784
02785 mls.setComputeNormals (true);
02786 if(polygonialOrder > 0)
02787 {
02788 mls.setPolynomialFit (true);
02789 mls.setPolynomialOrder(polygonialOrder);
02790 }
02791 else
02792 {
02793 mls.setPolynomialFit (false);
02794 }
02795 UASSERT(upsamplingMethod >= mls.NONE &&
02796 upsamplingMethod <= mls.VOXEL_GRID_DILATION);
02797 mls.setUpsamplingMethod((pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal>::UpsamplingMethod)upsamplingMethod);
02798 mls.setSearchRadius(searchRadius);
02799 mls.setUpsamplingRadius(upsamplingRadius);
02800 mls.setUpsamplingStepSize(upsamplingStep);
02801 mls.setPointDensity(pointDensity);
02802 mls.setDilationVoxelSize(dilationVoxelSize);
02803 mls.setDilationIterations(dilationIterations);
02804
02805
02806 mls.setInputCloud (cloud);
02807 if(indices->size())
02808 {
02809 mls.setIndices(indices);
02810 }
02811 mls.setSearchMethod (tree);
02812 mls.process (*cloud_with_normals);
02813
02814
02815 for(unsigned int i=0; i<cloud_with_normals->size(); ++i)
02816 {
02817 Eigen::Vector3f normal(cloud_with_normals->at(i).normal_x, cloud_with_normals->at(i).normal_y, cloud_with_normals->at(i).normal_z);
02818 normal.normalize();
02819 cloud_with_normals->at(i).normal_x = normal[0];
02820 cloud_with_normals->at(i).normal_y = normal[1];
02821 cloud_with_normals->at(i).normal_z = normal[2];
02822 }
02823
02824 return cloud_with_normals;
02825 }
02826
02827 LaserScan adjustNormalsToViewPoint(
02828 const LaserScan & scan,
02829 const Eigen::Vector3f & viewpoint,
02830 bool forceGroundNormalsUp)
02831 {
02832 if(scan.size() && !scan.is2d() && scan.hasNormals())
02833 {
02834 int nx = scan.getNormalsOffset();
02835 int ny = nx+1;
02836 int nz = ny+1;
02837 cv::Mat output = scan.data().clone();
02838 for(int i=0; i<scan.size(); ++i)
02839 {
02840 float * ptr = output.ptr<float>(0, i);
02841 if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz]))
02842 {
02843 Eigen::Vector3f v = viewpoint - Eigen::Vector3f(ptr[0], ptr[1], ptr[2]);
02844 Eigen::Vector3f n(ptr[nx], ptr[ny], ptr[nz]);
02845
02846 float result = v.dot(n);
02847 if(result < 0
02848 || (forceGroundNormalsUp && ptr[nz] < -0.8 && ptr[2] < viewpoint[3]))
02849 {
02850
02851 ptr[nx] *= -1.0f;
02852 ptr[ny] *= -1.0f;
02853 ptr[nz] *= -1.0f;
02854 }
02855 }
02856 }
02857 return LaserScan(output, scan.maxPoints(), scan.maxRange(), scan.format(), scan.localTransform());
02858 }
02859 return scan;
02860 }
02861
02862 void adjustNormalsToViewPoint(
02863 pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
02864 const Eigen::Vector3f & viewpoint,
02865 bool forceGroundNormalsUp)
02866 {
02867 for(unsigned int i=0; i<cloud->size(); ++i)
02868 {
02869 pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
02870 if(pcl::isFinite(normal))
02871 {
02872 Eigen::Vector3f v = viewpoint - cloud->points[i].getVector3fMap();
02873 Eigen::Vector3f n(normal.x, normal.y, normal.z);
02874
02875 float result = v.dot(n);
02876 if(result < 0
02877 || (forceGroundNormalsUp && normal.z < -0.8 && cloud->points[i].z < viewpoint[3]))
02878 {
02879
02880 cloud->points[i].normal_x *= -1.0f;
02881 cloud->points[i].normal_y *= -1.0f;
02882 cloud->points[i].normal_z *= -1.0f;
02883 }
02884 }
02885 }
02886 }
02887
02888 void adjustNormalsToViewPoint(
02889 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
02890 const Eigen::Vector3f & viewpoint,
02891 bool forceGroundNormalsUp)
02892 {
02893 for(unsigned int i=0; i<cloud->size(); ++i)
02894 {
02895 pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
02896 if(pcl::isFinite(normal))
02897 {
02898 Eigen::Vector3f v = viewpoint - cloud->points[i].getVector3fMap();
02899 Eigen::Vector3f n(normal.x, normal.y, normal.z);
02900
02901 float result = v.dot(n);
02902 if(result < 0
02903 || (forceGroundNormalsUp && normal.z < -0.8 && cloud->points[i].z < viewpoint[3]))
02904 {
02905
02906 cloud->points[i].normal_x *= -1.0f;
02907 cloud->points[i].normal_y *= -1.0f;
02908 cloud->points[i].normal_z *= -1.0f;
02909 }
02910 }
02911 }
02912 }
02913
02914 void adjustNormalsToViewPoints(
02915 const std::map<int, Transform> & poses,
02916 const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
02917 const std::vector<int> & rawCameraIndices,
02918 pcl::PointCloud<pcl::PointNormal>::Ptr & cloud)
02919 {
02920 if(poses.size() && rawCloud->size() && rawCloud->size() == rawCameraIndices.size() && cloud->size())
02921 {
02922 pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
02923 rawTree->setInputCloud (rawCloud);
02924
02925 for(unsigned int i=0; i<cloud->size(); ++i)
02926 {
02927 pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
02928 if(pcl::isFinite(normal))
02929 {
02930 std::vector<int> indices;
02931 std::vector<float> dist;
02932 rawTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, dist);
02933 UASSERT(indices.size() == 1);
02934 if(indices.size() && indices[0]>=0)
02935 {
02936 Transform p = poses.at(rawCameraIndices[indices[0]]);
02937 pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
02938 Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();
02939
02940 Eigen::Vector3f n(normal.x, normal.y, normal.z);
02941
02942 float result = v.dot(n);
02943 if(result < 0)
02944 {
02945
02946 cloud->points[i].normal_x *= -1.0f;
02947 cloud->points[i].normal_y *= -1.0f;
02948 cloud->points[i].normal_z *= -1.0f;
02949 }
02950 }
02951 else
02952 {
02953 UWARN("Not found camera viewpoint for point %d", i);
02954 }
02955 }
02956 }
02957 }
02958 }
02959
02960 void adjustNormalsToViewPoints(
02961 const std::map<int, Transform> & poses,
02962 const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
02963 const std::vector<int> & rawCameraIndices,
02964 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud)
02965 {
02966 UASSERT(rawCloud.get() && cloud.get());
02967 UDEBUG("poses=%d, rawCloud=%d, rawCameraIndices=%d, cloud=%d", (int)poses.size(), (int)rawCloud->size(), (int)rawCameraIndices.size(), (int)cloud->size());
02968 if(poses.size() && rawCloud->size() && rawCloud->size() == rawCameraIndices.size() && cloud->size())
02969 {
02970 pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
02971 rawTree->setInputCloud (rawCloud);
02972 for(unsigned int i=0; i<cloud->size(); ++i)
02973 {
02974 pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
02975 if(pcl::isFinite(normal))
02976 {
02977 std::vector<int> indices;
02978 std::vector<float> dist;
02979 rawTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, dist);
02980 if(indices.size() && indices[0]>=0)
02981 {
02982 UASSERT_MSG(indices[0]<(int)rawCameraIndices.size(), uFormat("indices[0]=%d rawCameraIndices.size()=%d", indices[0], (int)rawCameraIndices.size()).c_str());
02983 UASSERT(uContains(poses, rawCameraIndices[indices[0]]));
02984 Transform p = poses.at(rawCameraIndices[indices[0]]);
02985 pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
02986 Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();
02987
02988 Eigen::Vector3f n(normal.x, normal.y, normal.z);
02989
02990 float result = v.dot(n);
02991 if(result < 0)
02992 {
02993
02994 cloud->points[i].normal_x *= -1.0f;
02995 cloud->points[i].normal_y *= -1.0f;
02996 cloud->points[i].normal_z *= -1.0f;
02997 }
02998 }
02999 else
03000 {
03001 UWARN("Not found camera viewpoint for point %d!?", i);
03002 }
03003 }
03004 }
03005 }
03006 }
03007
03008 pcl::PolygonMesh::Ptr meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor)
03009 {
03010 pcl::PolygonMesh::Ptr output(new pcl::PolygonMesh);
03011 #ifndef DISABLE_VTK
03012 pcl::MeshQuadricDecimationVTK mqd;
03013 mqd.setTargetReductionFactor(factor);
03014 mqd.setInputMesh(mesh);
03015 mqd.process (*output);
03016 #else
03017 UWARN("RTAB-Map is not built with VTK module so mesh decimation cannot be used!");
03018 *output = *mesh;
03019 #endif
03020 return output;
03021 }
03022
03023 }
03024
03025 }