서동현

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
This diff is collapsed. Click to expand it.
#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