week16 source code upload

+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+
+project(Capstone)
+set(CMAKE_BUILD_TYPE Release)
+add_compile_options(-std=c++11)
+
+find_package(PCL 1.7 REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(Boost COMPONENTS program_options filesystem REQUIRED)
+
+include_directories(
+	include
+	${Boost_INCLUDE_DIRS}
+	${PCL_INCLUDE_DIRS}
+	${EIGEN3_INCLUDE_DIR}
+)
+
+set(sources
+)
+
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+add_executable(cloud src/pointcloud.cpp ${sources})
+target_link_libraries(cloud ${PCL_LIBRARIES} ${OpenCV_LIBS} ${Boost_LIBRARIES})
+
+add_executable(mesh src/mesh.cpp ${sources})
+target_link_libraries(mesh ${PCL_LIBRARIES} ${OpenCV_LIBS})
+
+add_executable(obj src/obj.cpp ${sources})
+target_link_libraries(obj ${PCL_LIBRARIES} ${OpenCV_LIBS})
+
+add_executable(densedepth src/densedepth.cpp ${sources})
+target_link_libraries(densedepth ${PCL_LIBRARIES} ${OpenCV_LIBS})

+#pragma once
+
+//common
+#include <string>
+#include <fstream>
+#include <iostream>
+#include <vector>
+#include <ctime>
+#include <cstdio>
+#include <algorithm>
+#include <stdlib.h>
+#include <sstream>
+#include <iomanip>
+
+//eigen
+#include <Eigen/Dense>
+
+//boost
+#include <boost/format.hpp>
+#include <boost/filesystem.hpp>
+#include <boost/thread/thread.hpp>
+#include <boost/algorithm/string.hpp>
+
+//opencv
+#include <opencv2/core.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+
+//pcl
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/io.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/vtk_io.h>
+#include <pcl/io/vtk_lib_io.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/filters/filter.h>
+#include <pcl/filters/radius_outlier_removal.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/surface/mls.h>
+#include <pcl/surface/poisson.h>
+#include <pcl/surface/marching_cubes_hoppe.h>
+#include <pcl/surface/gp3.h>
+
+using namespace boost::filesystem;
+
+struct camInfo
+{
+    double fx;
+    double fy;
+    double cx;
+    double cy;
+    double k1;
+    double k2;
+    double p1;
+    double p2;
+    int width;
+    int height;
+};
+
+void realsenseSetUp(camInfo& cam)
+{
+    cam.fx = 612.1810302745375;
+    cam.fy = 612.27685546875;
+    cam.cx = 323.64208984375;
+    cam.cy = 234.55838012695312;
+    cam.k1 = 0;
+    cam.k2 = 0;
+    cam.p1 = 0;
+    cam.p2 = 0;
+    cam.width = 640;
+    cam.height = 480;
+}
+
+Eigen::Vector3d coordinateRotation(double x, double y, double z)
+{
+    Eigen::Vector3d coord;
+    coord << x, y, z;
+
+    Eigen::Matrix3d rot;
+    rot << 1, 0, 0, 0, 0, 1, 0, -1, 0;
+
+    coord = rot * coord;
+    return coord;
+}
\ No newline at end of file

+#pragma once
+#include "common.h"
+
+void readFolder(std::string folderPath, std::vector<cv::Mat>& depths)
+{
+    path p(folderPath);
+    try
+    {
+        if(exists(p) && is_directory(p))
+        {
+            std::vector<std::string> imgs;
+            for(directory_entry& x : directory_iterator(p))
+            {
+                std::string img = x.path().string();
+                imgs.push_back(img);
+            }
+            std::sort(imgs.begin(), imgs.end());
+
+            for(auto img : imgs)
+            {
+                cv::Mat depth = cv::imread(img, cv::IMREAD_ANYDEPTH); //16UC1
+                depths.push_back(depth);
+            }
+        }
+        else
+        {
+            std::cout << "[Error] Check Depth Map Folder Path" << std::endl;
+            exit(0);
+        }
+    }
+    catch(const filesystem_error& ex)
+    {
+        std::cout << ex.what() << std::endl;
+    }
+}
+
+std::string type2str(int type)
+{
+    std::string r;
+
+    uchar depth = type & CV_MAT_DEPTH_MASK;
+    uchar chans = 1 + (type >> CV_CN_SHIFT);
+
+    switch ( depth )
+    {
+    case CV_8U:  r = "8U"; break;
+    case CV_8S:  r = "8S"; break;
+    case CV_16U: r = "16U"; break;
+    case CV_16S: r = "16S"; break;
+    case CV_32S: r = "32S"; break;
+    case CV_32F: r = "32F"; break;
+    case CV_64F: r = "64F"; break;
+    default:     r = "User"; break;
+    }
+
+    r += "C";
+    r += (chans+'0');
+
+    return r;
+}
+
+cv::Mat getMeanImg(std::vector<cv::Mat> depths)
+{
+    if(depths.empty())
+    {
+        std::cout << "[Error] no input images" << std::endl;
+        exit(0);
+    }
+    
+    cv::Mat m(depths[0].rows, depths[0].cols, CV_64FC1);
+    m.setTo(0);
+
+    cv::Mat tmp;
+    for(auto depth : depths)
+    {
+        depth.convertTo(tmp, CV_64FC1);
+        m += tmp;
+    }
+
+    m.convertTo(m, CV_16UC1, 1.0 / depths.size());
+    return m;
+}
+
+cv::Mat medianFilter(cv::Mat img)
+{
+    cv::Mat filtered;
+    cv::medianBlur(img, filtered, 5);
+    return filtered;
+}
+
+void reconstruction3D(camInfo cam, cv::Mat img, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
+{
+    for(int i = 0; i < img.cols; i++)
+    {
+        for(int j = 0; j < img.rows; j++)
+        {
+            auto d = img.at<u_int16_t>(j, i) / 1000.0;
+            if(d < 0.105 || d > 3.0)
+            {   //realsense min dist && max dist
+                continue;
+            }
+            double x = (i - cam.cx) * d / cam.fx;
+            double y = (j - cam.cy) * d / cam.fy;
+            double z = d;
+
+            pcl::PointXYZ pt;
+            pt.x = x;
+            pt.y = y;
+            pt.z = z;
+            cloud->push_back(pt);
+        }
+    }
+}
\ No newline at end of file

+#include "common.h"
+#include "cv_func.h"
+#include "pcl_func.h"
+
+/*
+[input]
+argc    = 2
+argv[0] = program
+argv[1] = densedepth.png (320 x 240)
+
+[output]
+interpolated_densedepth.png (640 x 480)
+densedepth.pcd
+*/
+
+int main(int argc, char** argv)
+{
+    if(argc != 2)
+    {
+        std::cout << "[Error] Program Usage: [./densedepth] [densedepth.png]" << std::endl;
+        return -1;
+    }
+
+    //intrinsic setup
+    camInfo cam;
+    realsenseSetUp(cam);
+    
+    cv::Mat img, interpolated_img;
+    img = cv::imread(argv[1], cv::IMREAD_ANYDEPTH);
+    //std::cout << type2str(img.type()) << std::endl; -> 16UC1
+    
+    //interpolation to (320, 240) -> (640, 480)
+    cv::resize(img, interpolated_img, cv::Size(640, 480), cv::INTER_LINEAR);
+    std::string inputPath = argv[1];
+    std::size_t div = inputPath.find_last_of("/\\");
+    std::string outputPath = inputPath.substr(0, div + 1);
+    cv::imwrite(outputPath + "interpolated_densedepth.png", interpolated_img);
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+    for(int i = 0; i < interpolated_img.cols; i++)
+    {
+        for(int j = 0; j < interpolated_img.rows; j++)
+        {
+            auto d = (-interpolated_img.at<u_int16_t>(j, i)) / 1000.0; // closer - darker reversed
+            double x = (i - cam.cx) * d / cam.fx;
+            double y = (j - cam.cy) * d / cam.fy;
+            double z = d;
+
+            pcl::PointXYZ pt;
+            pt.x = x;
+            pt.y = y;
+            pt.z = z;
+            cloud->push_back(pt);
+        }
+    }
+    pcl::io::savePCDFile(outputPath + "densedepth.pcd", *cloud);
+
+    return 0;
+}
\ No newline at end of file

+#include "common.h"
+#include "cv_func.h"
+#include "pcl_func.h"
+
+/*
+[input]
+argc    = 3
+argv[0] = program
+argv[1] = original pointcloud
+argv[2] = filtered pointcloud
+
+[output]
+original mesh -> vtk format
+filtered mesh -> vtk format
+*/
+
+int main(int argc, char** argv)
+{
+    if(argc != 3)
+    {
+        std::cout << "[Error] Program Usage: [./mesh] [original_cloud.pcd] [filtered_cloud.pcd]" << std::endl;
+        return -1;
+    }
+    
+    //save path
+    std::string inputPath = argv[1];
+    std::size_t div = inputPath.find_last_of("/\\");
+    std::string outputPath = inputPath.substr(0, div + 1);
+
+    //load pointcloud data
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_original(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::io::loadPCDFile(argv[1], *cloud_original);
+    pcl::io::loadPCDFile(argv[2], *cloud_filtered);
+    std::cout << "[Processing] reading pointcloud done..." << std::endl;
+
+    //origianl cloud needs upsampling
+    //(triangulation process impossible for noisy normal estimation)
+    //upsampling(cloud_original);
+    //std::cout << "original cloud upsampling done..." << std::endl;
+
+    //normal estimation
+    pcl::PointCloud<pcl::PointNormal>::Ptr original_with_normals(new pcl::PointCloud<pcl::PointNormal>);
+    normal_estimation(cloud_original, original_with_normals);
+
+    pcl::PointCloud<pcl::PointNormal>::Ptr filtered_with_normals(new pcl::PointCloud<pcl::PointNormal>);
+    normal_estimation(cloud_filtered, filtered_with_normals);
+    std::cout << "[Processing] normal estimation done..." << std::endl;
+    
+    //triangulation
+    pcl::PolygonMesh triangles_original, triangles_filtered;
+    triangulation(original_with_normals, triangles_original);
+    triangulation(filtered_with_normals, triangles_filtered);
+    std::cout << "[Processing] triangulation done..." << std::endl;
+
+    //save output
+    pcl::io::saveVTKFile(outputPath + "original_mesh.vtk", triangles_original);
+    pcl::io::saveVTKFile(outputPath + "filtered_mesh.vtk", triangles_filtered);
+
+    return 0;
+}
\ No newline at end of file

+#include "common.h"
+#include "cv_func.h"
+#include "pcl_func.h"
+
+/*
+[input]
+argc    = 3
+argv[0] = program
+argv[1] = vtk file
+artv[2] = color img
+
+[output]
+obj file
+mtl file
+*/
+
+int main(int argc, char** argv)
+{
+    if(argc != 3)
+    {
+        std::cout << "[Error] Program Usage: [./obj] [mesh.vtk] [color_img.png]" << std::endl;
+        return -1;
+    }
+
+    //read vtk file
+    pcl::PolygonMesh vtk_mesh;
+    pcl::io::loadPolygonFileVTK(argv[1], vtk_mesh);
+
+    //copy pointcloud from vtk file (vertices)
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::fromPCLPointCloud2(vtk_mesh.cloud, *cloud);
+    
+    //mesh initialization
+    pcl::TextureMesh mesh;
+    mesh.cloud = vtk_mesh.cloud;
+    std::vector<pcl::Vertices> vertices;
+
+    //copy faces from vtk file
+    vertices.resize(vtk_mesh.polygons.size());
+    for(std::size_t i = 0; i < vtk_mesh.polygons.size(); i++)
+    {
+        vertices[i] = vtk_mesh.polygons[i];
+    }
+    mesh.tex_polygons.push_back(vertices);
+    PCL_INFO("Input mesh contains %zu faces and %zu vertices\n",
+            mesh.tex_polygons[0].size(),
+            static_cast<std::size_t>(cloud->size()));
+    PCL_INFO("...Done.\n");
+
+    //intrinsic setup
+    camInfo cam;
+    realsenseSetUp(cam);
+    
+    //one material
+    mesh.tex_materials.resize(1);
+    
+    pcl::TexMaterial mesh_material;
+    mesh_material.tex_name = "texture image";
+    std::string color_img = argv[2];
+    color_img = color_img.substr(color_img.find_last_of("/\\") + 1);
+    mesh_material.tex_file = color_img;
+
+    mesh_material.tex_Ka.r = 0.2f;
+    mesh_material.tex_Ka.g = 0.2f;
+    mesh_material.tex_Ka.b = 0.2f;
+
+    mesh_material.tex_Kd.r = 0.8f;
+    mesh_material.tex_Kd.g = 0.8f;
+    mesh_material.tex_Kd.b = 0.8f;
+
+    mesh_material.tex_Ks.r = 1.0f;
+    mesh_material.tex_Ks.g = 1.0f;
+    mesh_material.tex_Ks.b = 1.0f;
+
+    mesh_material.tex_d = 1.0f;
+    mesh_material.tex_Ns = 75.0f;
+    mesh_material.tex_illum = 2;
+
+    mesh.tex_materials[0] = mesh_material;
+
+    //normal estimation
+    PCL_INFO ("\nEstimating normals...\n");
+    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
+    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
+    tree->setInputCloud(cloud);
+    n.setInputCloud(cloud);
+    n.setSearchMethod(tree);
+    n.setKSearch(20);
+    n.compute(*normals);
+
+    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
+    pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);
+    pcl::toPCLPointCloud2(*cloud_with_normals, mesh.cloud);
+    PCL_INFO("...Done.\n");
+
+    //obj file creating
+    PCL_INFO("\nSaving mesh to textured_mesh.obj\n");
+    saveOBJFile("mesh.obj", mesh, 5, cloud, cam);
+
+    return 0;
+}
\ No newline at end of file

+#include "common.h"
+#include "cv_func.h"
+#include "pcl_func.h"
+
+/*
+[input]
+argc    = 2
+argv[0] = program
+argv[1] = depth folder (containing depth images)
+
+[output]
+single frame depth image & point cloud
+average depth image & point cloud
+*/
+
+int main(int argc, char** argv)
+{
+  if(argc != 2)
+  {
+    std::cout << "[Error] Program Usage: [./cloud] [depth image folder]" << std::endl;
+    return -1;
+  }
+
+  //intrinsic setup
+  camInfo cam;
+  realsenseSetUp(cam);
+  std::cout << "[Processing] Intrinsic parameters for Realsense setting done..." << std::endl;
+
+  //get raw, filtered img
+  std::string folderPath = argv[1];
+  std::vector<cv::Mat> depths;
+  readFolder(folderPath, depths);
+  cv::Mat meanImg = getMeanImg(depths);
+  cv::Mat filtered = medianFilter(meanImg);
+  cv::Mat raw = depths[0];
+  std::cout << "[Processing] image filtering done..." << std::endl;
+  
+  //3D reconstruction
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_original(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
+
+  reconstruction3D(cam, raw, cloud_original);
+  reconstruction3D(cam, filtered, cloud_filtered);
+  std::cout << "[Processing] 3d reconstruction done..." << std::endl;
+
+  //create output folder
+  std::size_t div = folderPath.find_last_of("/\\");
+  std::string outputPath = folderPath.substr(0, div + 1) + "result";
+  path p(outputPath);
+  create_directory(p);
+  
+  //save output
+  pcl::io::savePCDFile(outputPath + "/cloud_original.pcd", *cloud_original);
+  pcl::io::savePCDFile(outputPath + "/cloud_filtered.pcd", *cloud_filtered);
+  cv::imwrite(outputPath + "/img_original.png", raw);
+  cv::imwrite(outputPath + "/img_filtered.png", filtered);
+
+  return 0;
+}
\ No newline at end of file