add image fetch utils and cuda parallelization

.gitignore

@@ -39,3 +39,6 @@ build_tests/
 polyscope/
 libGraphCpp/
 utils/
+datasets/
+images/
+result/

CMakeLists.txt

@@ -1,7 +1,9 @@
 cmake_minimum_required(VERSION 3.14)
 
-project(embedded_deformation)
-
+project(embedded_deformation LANGUAGES CXX)
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON) 
+set(CMAKE_CXX_EXTENSIONS OFF) 
 # get polyscope
 include(FetchContent)
 FetchContent_Declare(
@@ -48,6 +50,10 @@ if(NOT libGraphCpp_POPULATED)
     FetchContent_Populate(libGraphCpp)
     add_subdirectory(libGraphCpp)
 endif()
+find_package(CUDA REQUIRED)
+find_package(PCL REQUIRED)
+include_directories(${CUDA_INCLUDE_DIRS})
+include_directories(${PCL_INCLUDE_DIRS})
 
 add_subdirectory(embedded_deformation)
 
@@ -59,10 +65,10 @@ find_package(Eigen3 REQUIRED)
 
 # yaml-cpp if not found, run sudo apt-get install libyaml-cpp-dev
 find_package(yaml-cpp REQUIRED)
-
 find_package(Ceres REQUIRED)
 
 
+
 file(GLOB_RECURSE my_c_list RELATIVE ${CMAKE_SOURCE_DIR} "app/*")
 
 foreach(file_path ${my_c_list})
@@ -75,6 +81,7 @@ foreach(file_path ${my_c_list})
                         ${YAML_CPP_INCLUDE_DIR}
                         ${CERES_INCLUDE_DIRS}
                         ${PYTHON_INCLUDE_DIRS}
+                        ${CMAKE_SOURCE_DIR}/embedded_deformation/include
                         )
 
     target_link_libraries(${filename}

app/embedded_deformation_sample.cpp

@@ -1,13 +1,4 @@
-/**
- * Author: R. Falque
- * 
- * main for testing the embedded deformation implementation
- * by R. Falque
- **/
-
-// dependencies
-
-
+#include "imageProcessor/imageProcessor.h"
 #include "utils/IO_libIGL/readOBJ.h"
 #include "utils/IO/readPLY.h"
 #include "utils/IO/writePLY.h"
@@ -25,83 +16,254 @@
 
 #include <yaml-cpp/yaml.h>
 #include "polyscope/polyscope.h"
+#include "rigidSolver/rigidSolver.h"
+
+void vertex2image(const Eigen::MatrixXd& source, cv::Mat& result, const Intrinsic& intrinsic)
+{
+    for(int i = 0; i < source.rows(); i++) {
+        Eigen::Vector3d source_point = source.row(i);
+        int x = (int) (source_point(0)/(source_point(2) + 1e-10) * intrinsic.focal_x) + intrinsic.principal_x;
+        int y = (int) (source_point(1)/(source_point(2) + 1e-10) * intrinsic.focal_y) + intrinsic.principal_y;
+        if(x >= 0 && x < result.cols && y >= 0 && y < result.rows) {
+            result.at<cv::Vec3d>(y, x) = cv::Vec3d(source_point(0), source_point(1), source_point(2));
+        }
+    }
+}
+
+void diff(const cv::Mat& src_image, const cv::Mat& tag_image, cv::Mat& diff_image)
+{
+    for(int i = 0; i < src_image.rows; i++) {
+        for (int j = 0; j < src_image.cols; j++) {
+            cv::Vec3d src = src_image.at<cv::Vec3d>(i,j);
+            cv::Vec3d tag = tag_image.at<cv::Vec3d>(i,j);
+            double diff = sqrt(pow(src[0] - tag[0], 2) + pow(src[1] - tag[1], 2) + pow(src[2] - tag[2], 2));
+            diff_image.at<double>(i,j) = diff;
+        }
+    }
+}
+
+void visualizion(cv::Mat& diff_image1, cv::Mat& diff_image2)
+{
+    double minVal1, maxVal1, minVal2, maxVal2;
+    cv::Point minLoc1, maxLoc1, minLoc2, maxLoc2;
+    cv::minMaxLoc(diff_image1, &minVal1, &maxVal1, &minLoc1, &maxLoc1);
+    cv::minMaxLoc(diff_image2, &minVal2, &maxVal2, &minLoc2, &maxLoc2);
+    double maxVal = max(maxVal1, maxVal2);
+    double minVal = min(minVal1, minVal2);
+    // 对两个深度图进行归一化 范围是0~255
+    cv::Mat diff_image1_normalized = cv::Mat::zeros(diff_image1.rows, diff_image1.cols, CV_8UC3);
+    cv::Mat diff_image2_normalized = cv::Mat::zeros(diff_image2.rows, diff_image2.cols, CV_8UC3);
+    for( int i = 0; i < diff_image1.rows; i++ )
+    {
+        for( int j = 0; j < diff_image1.cols; j++ )
+        {
+            uchar r_value1 = 255*(diff_image1.at<double>(i,j) - minVal)/(maxVal - minVal);
+            uchar b_value1 = 255*(maxVal - diff_image1.at<double>(i,j))/(maxVal - minVal);
+            diff_image1_normalized.at<cv::Vec3b>(i,j) = static_cast<cv::Vec3b>(r_value1, 
+                                                                                0,
+                                                                                b_value1);
+            diff_image2_normalized.at<cv::Vec3b>(i,j) = static_cast<cv::Vec3b>(255*(diff_image2.at<double>(i,j) - minVal)/(maxVal - minVal), 
+                                                                                0,
+                                                                                255*(maxVal - diff_image2.at<double>(i,j))/(maxVal - minVal));
+        }
+    }
+    cv::namedWindow("prev&curr", cv::WINDOW_AUTOSIZE);
+    cv::namedWindow("deformed&curr", cv::WINDOW_AUTOSIZE);
+    cv::imshow("prev&curr", diff_image1_normalized);
+    cv::imshow("deformed&curr", diff_image2_normalized);
+    cv::waitKey(0);
+}
+
+void evaluate(const Eigen::MatrixXd& previous, 
+            const Eigen::MatrixXd& previous_deformed, 
+            const Eigen::MatrixXd& current, 
+            const Intrinsic& intrinsic,
+            const unsigned width,
+            const unsigned height)
+{
+    cv::Mat previous_image = cv::Mat::zeros(height, width, CV_64FC3);
+    cv::Mat previous_deformed_image = cv::Mat::zeros(height, width, CV_64FC3);
+    cv::Mat current_image = cv::Mat::zeros(height, width, CV_64FC3);
+
+    vertex2image(previous, previous_image, intrinsic);
+    vertex2image(previous_deformed, previous_deformed_image, intrinsic);
+    vertex2image(current, current_image, intrinsic);
+
+    cv::Mat diff_image1 = cv::Mat::zeros(height, width, CV_64FC1);
+    cv::Mat diff_image2 = cv::Mat::zeros(height, width, CV_64FC1);
+
+    diff(previous_image, current_image, diff_image1);
+    diff(previous_deformed_image, current_image, diff_image2);
+
+    visualizion(diff_image1, diff_image2);
+}
+
 
 
 int main(int argc, char* argv[])
 {
+
     options opts;
     opts.loadYAML("../config.yaml");
-std::cout << "Progress: yaml loaded\n";
+    std::cout << "Progress: yaml loaded\n";
 
     polyscope::init();
-std::cout << "Progress: plyscope initialized\n";
+    std::cout << "Progress: plyscope initialized\n";
 
+    // 读取图像的类
+    std::shared_ptr<FetchInterface> image_fetcher = std::make_shared<FetchInterface>(opts.image_path);
+
+    Intrinsic raw_Intrinsic(opts.focal_x, opts.focal_y, opts.principal_x, opts.principal_y);
+
+    auto image_processor = std::make_shared<imageProcessor>(
+                                raw_Intrinsic,                                      
+                                opts.width, opts.height, 
+                                opts.start_frame,
+                                image_fetcher);
+
+    // 并行化icp刚性求解
+    // To do: 加入dense color icp
+    std::shared_ptr<RigidSolver> rigid_solver = std::make_shared<RigidSolver>(image_processor->clip_width(), 
+                                                                            image_processor->clip_height(), 
+                                                                            image_processor->clip_intrinsic());
+
+    // 图像处理 读取图像，计算法向量，计算顶点图，双边滤波，读取有效顶点与法向量
+    image_processor->test();
+    Eigen::Matrix<float, 3, 4> init_guess = rigid_solver->test(image_processor->getVertexNormalMaps(),image_processor->getVertexNormalMapsPrev());
+    std::vector<Surfel> surfels =  image_processor->getSurfelData();
+    std::vector<Surfel> surfels_prev = image_processor->getSurfelDataPrev();
+    std::vector<correspondence> correspondences = rigid_solver->getCorrespondence();
+
     /* 
      * V: vertex of the surface
      * F: faces of the surface
      * N: nodes of the deformation graph
      * E: edges of the deformation graph
      */
 
-    Eigen::MatrixXd V, N;
+    Eigen::MatrixXd V, N;                            // 当前帧顶点图和节点
+    Eigen::MatrixXd V_prev, N_prev;                  // 上一帧顶点图和节点
     Eigen::MatrixXi F, E;
+    Eigen::MatrixXd normals, normals_prev;           // 当前帧和上一帧的法向量
     EmbeddedDeformation* non_rigid_deformation;
+    readPLY("/home/maihn/dev/embeddedDeformation/datasets/breathe_pcd/ply_1.ply", V, F);
+    auto* psCloud = polyscope::registerPointCloud("Point Cloud", V);
+    psCloud->setPointColor(glm::vec3(0.0, 1.0, 0.0)); // RGB颜色，这里是绿色
+    polyscope::getPointCloud("Point Cloud")->setPointRadius(0.00125, true);
+    polyscope::show();
+    V.resize(surfels.size(), 3);
+    for(int i=0; i<surfels.size(); i++)
+    {   
+        V(i,0) = surfels[i].vertex.x;
+        V(i,1) = surfels[i].vertex.y;
+        V(i,2) = surfels[i].vertex.z;
+    }
+
+
+    V_prev.resize(surfels_prev.size(), 3);
+    normals_prev.resize(surfels_prev.size(), 3);
+    for(int i=0; i<surfels_prev.size(); i++)
+    {   
+        V_prev(i,0) = surfels_prev[i].vertex.x - 0.5;
+        V_prev(i,1) = surfels_prev[i].vertex.y;
+        V_prev(i,2) = surfels_prev[i].vertex.z;
+        normals_prev(i,0) = surfels_prev[i].normal.x;
+        normals_prev(i,1) = surfels_prev[i].normal.y;
+        normals_prev(i,2) = surfels_prev[i].normal.z;
+    }
 
-std::cout << "Progress: load file ...";
+    auto* psCloud4 = polyscope::registerPointCloud("Point Cloud 2", V_prev);
+    psCloud4->setPointColor(glm::vec3(1.0, 0.0, 0.0)); // RGB颜色，这里是绿色
+    polyscope::getPointCloud("Point Cloud 2")->setPointRadius(0.00125, true);
+    std::cout<<"using knn distance"<<std::endl;
+
+    // 输入的顶点与法向量是所有有效的顶点以及对应的法向量，是上一帧的顶点与法向量
+    non_rigid_deformation = new EmbeddedDeformation(V_prev, normals_prev, opts);
 
-    readPLY(opts.path_input_file ,V, F);
+    // 展示节点
+    // if (opts.visualization)
+    //     non_rigid_deformation->show_deformation_graph();
+
+    // read correspondences
+    // 具有对应关系的点对，相比有效的顶点与法向量，数量会少一些
+    Eigen::MatrixXd target_points(correspondences.size(), 3);
+    Eigen::MatrixXd source_points(correspondences.size(), 3);
+    Eigen::MatrixXd target_normals(correspondences.size(), 3);
+    for(int i=0; i<correspondences.size(); i++)
+    {
+        target_points(i,0) = correspondences[i].tag_vertex.x;
+        target_points(i,1) = correspondences[i].tag_vertex.y;
+        target_points(i,2) = correspondences[i].tag_vertex.z;
+        source_points(i,0) = correspondences[i].src_vertex.x;
+        source_points(i,1) = correspondences[i].src_vertex.y;
+        source_points(i,2) = correspondences[i].src_vertex.z;
+        target_normals(i,0) = correspondences[i].tag_normal.x;
+        target_normals(i,1) = correspondences[i].tag_normal.y;
+        target_normals(i,2) = correspondences[i].tag_normal.z;
+    }
 
-std::cout << " done.\n";
+    std::cout << "progress : start deformation ..." << std::endl;
+    Eigen::MatrixXd V_deformed, normal_deformed;                       // 变形后的顶点和法向量 
+    // 输入的是具有有效对应关系的点对
+    std::cout << "progress: non rigid tracking: 1 ..."<<std::endl;
+    non_rigid_deformation->deform(source_points, target_points, target_normals, V_deformed, normal_deformed, opts);
 
-    // check for error
-    if (opts.use_geodesic and F.rows() == 0)
+    for( int i =1; i<10; i++)
     {
-        std::cout << "Config file error: use_geodesic = true, but nor faces were provided." <<std::endl;
-        exit(-1);
+        std::cout<<"progress: non rigid tracking "<< i+1 <<" ..."<<std::endl;
+        // 一次非刚性求解后寻找新的匹配点
+        DeviceBufferArray<correspondence> new_correspondences;
+        int num_pixels = image_processor->clip_width() * image_processor->clip_height();
+        new_correspondences.AllocateBuffer(num_pixels);
+        rigid_solver->findCorrespondences(V_deformed, normal_deformed, new_correspondences);
+        DeviceArrayView<correspondence> new_correspondence_view = new_correspondences.ArrayView();
+        std::vector<correspondence> h_correspondences;
+        // 将新的匹配点下载到主机
+        new_correspondence_view.Download(h_correspondences);
+        Eigen::MatrixXd new_target_points(h_correspondences.size(), 3);
+        Eigen::MatrixXd new_source_points(h_correspondences.size(), 3);
+        Eigen::MatrixXd new_target_normals(h_correspondences.size(), 3);
+        for(int i=0; i<h_correspondences.size(); i++)
+        {
+            new_target_points(i,0) = h_correspondences[i].tag_vertex.x;
+            new_target_points(i,1) = h_correspondences[i].tag_vertex.y;
+            new_target_points(i,2) = h_correspondences[i].tag_vertex.z;
+            new_source_points(i,0) = h_correspondences[i].src_vertex.x;
+            new_source_points(i,1) = h_correspondences[i].src_vertex.y;
+            new_source_points(i,2) = h_correspondences[i].src_vertex.z;
+            new_target_normals(i,0) = h_correspondences[i].tag_normal.x;
+            new_target_normals(i,1) = h_correspondences[i].tag_normal.y;
+            new_target_normals(i,2) = h_correspondences[i].tag_normal.z;
+        }
+        V_deformed.resize(0,0);
+        normal_deformed.resize(0,0);
+        non_rigid_deformation->deform(new_source_points, new_target_points, new_target_normals, V_deformed, normal_deformed, opts);
     }
-
-    if (opts.visualization)
-        if (F.rows() != 0)
-            plot_mesh(V,F);
-        else
-            plot_cloud(V);
 
-    if (opts.graph_provided) /* graph is provided */
-    {
-        libgraphcpp::readGraphOBJ(opts.path_graph_obj ,N, E);
+    polyscope::init();
+    polyscope::view::upDir = polyscope::view::UpDir::NegYUp;
 
-        // create deformation object (does not use geodesic distance)
-        non_rigid_deformation = new EmbeddedDeformation(V, F, N, E, opts);
+    // 上一帧 绿色
+    auto* psCloud1 = polyscope::registerPointCloud("Point Cloud Previous", V_prev);
+    psCloud1->setPointColor(glm::vec3(0.0, 1.0, 0.0)); // RGB颜色，这里是绿色
+    // polyscope::getPointCloud("Point Cloud 2")->addVectorQuantity("normals 2", normals_prev);
+    polyscope::getPointCloud("Point Cloud Previous")->setPointRadius(0.00125, true);
 
-    }
-    else /* graph not provided */
-    {
-        if (opts.use_geodesic)
-            non_rigid_deformation = new EmbeddedDeformation(V, F, opts);
-            //non_rigid_deformation = new embedded_deformation(V, F, opts.grid_resolution, opts.graph_connectivity);
-        else /* use knn distance */
-            non_rigid_deformation = new EmbeddedDeformation(V, opts);
-            //non_rigid_deformation = new embedded_deformation(V, opts.grid_resolution, opts.graph_connectivity);
-    }
+    // 当前帧 红色
+    auto* psCloud2 = polyscope::registerPointCloud("Point Cloud Current", V);
+    // polyscope::getPointCloud("Point Cloud 1")->addVectorQuantity("normals 1", normals);
+    psCloud2->setPointColor(glm::vec3(1.0, 0.0, 0.0)); // RGB颜色，这里是红色
+    polyscope::getPointCloud("Point Cloud Current")->setPointRadius(0.00125, true);
 
-    if (opts.visualization)
-        non_rigid_deformation->show_deformation_graph();
-
-    // read correspondences
-    Eigen::MatrixXd correspondences = read_csv<Eigen::MatrixXd>(opts.path_pairwise_correspondence);
-    Eigen::MatrixXd new_points = correspondences.rightCols(3).transpose();
-    Eigen::MatrixXd old_points = correspondences.leftCols(3).transpose();
+    // 变形后的上一帧 蓝色
+    auto* psCloud3 = polyscope::registerPointCloud("Point Cloud Previous Deformed", V_deformed);
+    psCloud3->setPointColor(glm::vec3(0.0, 0.0, 1.0)); // RGB颜色，这里是蓝色
+    // polyscope::getPointCloud("Point Cloud 2")->addVectorQuantity("normals 2", normals_prev);
+    polyscope::getPointCloud("Point Cloud Previous Deformed")->setPointRadius(0.00125, true);
+    polyscope::show();
 
-    std::cout << "progress : start deformation ..." << std::endl;
-    Eigen::MatrixXd V_deformed;
-    non_rigid_deformation->deform(old_points, new_points, V_deformed);
 
-    if (opts.visualization)
-        if (F.rows() != 0)
-            plot_mesh(V_deformed,F);
-        else
-            plot_cloud(V_deformed);
+    evaluate(V_prev, V_deformed, V, image_processor->clip_intrinsic(), image_processor->clip_width(), image_processor->clip_height());
 
-    writePLY(opts.path_output_file, V_deformed, F, true);
     return 0;
 }

config.yaml

@@ -1,18 +1,27 @@
 # input output file path data
 io_files:
-  input_ply: "../data/input.ply"
-  pointwise_correspondence: "../data/FAUST_dabing.csv"
+  # input_ply: "../data/input.ply"
+  # pointwise_correspondence: "../data/FAUST_dabing.csv"
+  principal_x: 320.0
+  principal_y: 240.0
+  focal_x: 570.0
+  focal_y: 570.0
+  width: 640
+  height: 480
+  start_frame: 100
+  image_path: "../datasets/boxingMask"
+
 
   # for cases where the graph is provided
   # pointwise_correspondence: "../data/drill_rotation.csv"
-  input_obj: "../data/graphs_test/surface_to_deform.obj"
+  # input_obj: "../data/graphs_test/surface_to_deform.obj"
   #graph_obj: "../data/graphs_test/0_graph_complete.obj"
   #graph_obj: "../data/graphs_test/1_graph_complete_smaller.obj"
   #graph_obj: "../data/graphs_test/3_1_node_connectetivity.obj"
-  graph_obj: "../data/graphs_test/4_1_edge_connectetivity.obj"
+  # graph_obj: "../data/graphs_test/4_1_edge_connectetivity.obj"
   #graph_obj: "../data/graphs_test/5_2_edge_connectetivity.obj"
 
-  output_ply: "../data/output.ply"
+  # output_ply: "../data/output.ply"
 
 general_params:
   verbose: true         # enable / disable the output (true / false)