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/DBDriver.h>
00029 #include <rtabmap/core/Rtabmap.h>
00030 #include <rtabmap/core/util3d.h>
00031 #include <rtabmap/core/util3d_filtering.h>
00032 #include <rtabmap/core/util3d_transforms.h>
00033 #include <rtabmap/core/util3d_surface.h>
00034 #include <rtabmap/utilite/UMath.h>
00035 #include <rtabmap/utilite/UTimer.h>
00036 #include <rtabmap/utilite/UFile.h>
00037 #include <pcl/filters/filter.h>
00038 #include <pcl/io/ply_io.h>
00039 #include <pcl/io/obj_io.h>
00040 #include <pcl/common/common.h>
00041 #include <pcl/surface/poisson.h>
00042 #include <stdio.h>
00043
00044 using namespace rtabmap;
00045
00046 void showUsage()
00047 {
00048 printf("\nUsage:\n"
00049 "rtabmap-exportCloud [options] database.db\n"
00050 "Options:\n"
00051 " --mesh Create a mesh.\n"
00052 " --texture Create a mesh with texture.\n"
00053 "\n");
00054 exit(1);
00055 }
00056
00057 int main(int argc, char * argv[])
00058 {
00059 ULogger::setType(ULogger::kTypeConsole);
00060 ULogger::setLevel(ULogger::kError);
00061
00062 if(argc < 2)
00063 {
00064 showUsage();
00065 }
00066
00067 bool mesh = false;
00068 bool texture = false;
00069 for(int i=1; i<argc-1; ++i)
00070 {
00071 if(std::strcmp(argv[i], "--mesh") == 0)
00072 {
00073 mesh = true;
00074 }
00075 else if(std::strcmp(argv[i], "--texture") == 0)
00076 {
00077 texture = true;
00078 }
00079 }
00080
00081 std::string dbPath = argv[argc-1];
00082
00083
00084 ParametersMap parameters;
00085 DBDriver * driver = DBDriver::create();
00086 if(driver->openConnection(dbPath))
00087 {
00088 parameters = driver->getLastParameters();
00089 driver->closeConnection(false);
00090 }
00091 else
00092 {
00093 UERROR("Cannot open database %s!", dbPath.c_str());
00094 }
00095 delete driver;
00096
00097
00098 Rtabmap rtabmap;
00099 rtabmap.init(parameters, dbPath);
00100
00101 std::map<int, Signature> nodes;
00102 std::map<int, Transform> optimizedPoses;
00103 std::multimap<int, Link> links;
00104 rtabmap.get3DMap(nodes, optimizedPoses, links, true, true);
00105
00106
00107 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00108 std::map<int, rtabmap::Transform> cameraPoses;
00109 std::map<int, std::vector<rtabmap::CameraModel> > cameraModels;
00110 std::map<int, cv::Mat> cameraDepths;
00111 for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
00112 {
00113 Signature node = nodes.find(iter->first)->second;
00114
00115
00116 node.sensorData().uncompressData();
00117 std::vector<CameraModel> models = node.sensorData().cameraModels();
00118 cv::Mat depth = node.sensorData().depthRaw();
00119
00120 pcl::IndicesPtr indices(new std::vector<int>);
00121 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
00122 node.sensorData(),
00123 4,
00124 4.0f,
00125 0.0f,
00126 indices.get());
00127
00128 pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
00129 transformedCloud = rtabmap::util3d::voxelize(cloud, indices, 0.01);
00130 transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second);
00131
00132 Eigen::Vector3f viewpoint( iter->second.x(), iter->second.y(), iter->second.z());
00133 pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(transformedCloud, 10, 0.0f, viewpoint);
00134
00135 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00136 pcl::concatenateFields(*transformedCloud, *normals, *cloudWithNormals);
00137
00138 if(mergedClouds->size() == 0)
00139 {
00140 *mergedClouds = *cloudWithNormals;
00141 }
00142 else
00143 {
00144 *mergedClouds += *cloudWithNormals;
00145 }
00146
00147 cameraPoses.insert(std::make_pair(iter->first, iter->second));
00148 if(!models.empty())
00149 {
00150 cameraModels.insert(std::make_pair(iter->first, models));
00151 }
00152 if(!depth.empty())
00153 {
00154 cameraDepths.insert(std::make_pair(iter->first, depth));
00155 }
00156 }
00157 if(mergedClouds->size())
00158 {
00159 if(!(mesh || texture))
00160 {
00161 printf("Voxel grid filtering of the assembled cloud (voxel=%f, %d points)\n", 0.01f, (int)mergedClouds->size());
00162 mergedClouds = util3d::voxelize(mergedClouds, 0.01f);
00163
00164 printf("Saving cloud.ply... (%d points)\n", (int)mergedClouds->size());
00165 pcl::io::savePLYFile("cloud.ply", *mergedClouds);
00166 printf("Saving cloud.ply... done!\n");
00167 }
00168 else
00169 {
00170 Eigen::Vector4f min,max;
00171 pcl::getMinMax3D(*mergedClouds, min, max);
00172 float mapLength = uMax3(max[0]-min[0], max[1]-min[1], max[2]-min[2]);
00173 int optimizedDepth = 12;
00174 for(int i=6; i<12; ++i)
00175 {
00176 if(mapLength/float(1<<i) < 0.03f)
00177 {
00178 optimizedDepth = i;
00179 break;
00180 }
00181 }
00182
00183
00184 printf("Mesh reconstruction...\n");
00185 pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
00186 pcl::Poisson<pcl::PointXYZRGBNormal> poisson;
00187 poisson.setDepth(optimizedDepth);
00188 poisson.setInputCloud(mergedClouds);
00189 UTimer timer;
00190 poisson.reconstruct(*mesh);
00191 printf("Mesh reconstruction... done! %fs (%d polygons)\n", timer.ticks(), (int)mesh->polygons.size());
00192
00193 if(mesh->polygons.size())
00194 {
00195 rtabmap::util3d::denseMeshPostProcessing<pcl::PointXYZRGBNormal>(
00196 mesh,
00197 0.0f,
00198 0,
00199 mergedClouds,
00200 0.05,
00201 !texture);
00202
00203 if(!texture)
00204 {
00205 printf("Saving mesh.ply...\n");
00206 pcl::io::savePLYFile("mesh.ply", *mesh);
00207 printf("Saving mesh.ply... done!\n");
00208 }
00209 else
00210 {
00211 printf("Texturing... cameraPoses=%d, cameraDepths=%d\n", (int)cameraPoses.size(), (int)cameraDepths.size());
00212 std::vector<std::map<int, pcl::PointXY> > vertexToPixels;
00213 pcl::TextureMeshPtr textureMesh = rtabmap::util3d::createTextureMesh(
00214 mesh,
00215 cameraPoses,
00216 cameraModels,
00217 cameraDepths,
00218 3,
00219 0.0f,
00220 0.0f,
00221 50,
00222 std::vector<float>(),
00223 0,
00224 &vertexToPixels);
00225 printf("Texturing... done! %fs\n", timer.ticks());
00226
00227
00228 if(textureMesh->tex_coordinates.size())
00229 {
00230 printf("Cleanup mesh...\n");
00231 rtabmap::util3d::cleanTextureMesh(*textureMesh, 0);
00232 printf("Cleanup mesh... done! %fs\n", timer.ticks());
00233 }
00234
00235 if(textureMesh->tex_materials.size())
00236 {
00237 printf("Merging %d textures...\n", (int)textureMesh->tex_materials.size());
00238 cv::Mat textures = rtabmap::util3d::mergeTextures(
00239 *textureMesh,
00240 std::map<int, cv::Mat>(),
00241 std::map<int, std::vector<rtabmap::CameraModel> >(),
00242 rtabmap.getMemory(),
00243 0,
00244 4096,
00245 1,
00246 vertexToPixels,
00247 true, 10.0f, true ,true, 0, 0, 0, false);
00248
00249
00250
00251 bool success = false;
00252 UASSERT(!textures.empty());
00253 UASSERT(textureMesh->tex_materials.size() == 1);
00254
00255 std::string filePath = "mesh.jpg";
00256 textureMesh->tex_materials[0].tex_file = filePath;
00257 printf("Saving texture to %s.\n", filePath.c_str());
00258 success = cv::imwrite(filePath, textures);
00259 if(!success)
00260 {
00261 UERROR("Failed saving %s!", filePath.c_str());
00262 }
00263 else
00264 {
00265 printf("Saved %s.\n", filePath.c_str());
00266 }
00267
00268 if(success)
00269 {
00270
00271 std::string filePath = "mesh.obj";
00272 printf("Saving obj (%d vertices) to %s.\n", (int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step, filePath.c_str());
00273 success = pcl::io::saveOBJFile(filePath, *textureMesh) == 0;
00274
00275 if(success)
00276 {
00277 printf("Saved obj to %s!\n", filePath.c_str());
00278 }
00279 else
00280 {
00281 UERROR("Failed saving obj to %s!", filePath.c_str());
00282 }
00283 }
00284 }
00285 }
00286 }
00287 }
00288 }
00289 else
00290 {
00291 printf("Export failed! The cloud is empty.\n");
00292 }
00293
00294 return 0;
00295 }