diff --git a/.gitignore b/.gitignore
index 4fa61ce..e99bc31 100644
--- a/.gitignore
+++ b/.gitignore
@@ -39,3 +39,6 @@ build_tests/
 polyscope/
 libGraphCpp/
 utils/
+datasets/
+images/
+result/
diff --git a/CMakeLists.txt b/CMakeLists.txt
index a013d88..11b9b92 100644
--- a/CMakeLists.txt
+++ b/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}
diff --git a/app/embedded_deformation_sample.cpp b/app/embedded_deformation_sample.cpp
index 70c6efe..276e6fe 100644
--- a/app/embedded_deformation_sample.cpp
+++ b/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,17 +16,124 @@
 
 #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
@@ -43,65 +141,129 @@ std::cout << "Progress: plyscope initialized\n";
      * 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;
 }
diff --git a/config.yaml b/config.yaml
index 91c799c..8683a11 100644
--- a/config.yaml
+++ b/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)
diff --git a/data/output.obj b/data/output.obj
new file mode 100644
index 0000000..9119402
--- /dev/null
+++ b/data/output.obj
@@ -0,0 +1,7 @@
+v 0 0 -7.49999958333333
+v -8.33333333449815e-08 0 7.50000008333333
+v 0 0 2.2500002
+v 0 0 -2.2499998
+l 3 2
+l 4 1
+l 4 3
diff --git a/data/output.ply b/data/output.ply
index 7263eb3..298c763 100644
--- a/data/output.ply
+++ b/data/output.ply
@@ -1,16 +1,17 @@
 ply
 format ascii 1.0
+comment generated by tinyply 2.2
 element vertex 6890
-property double x
-property double y
-property double z
+property float x
+property float y
+property float z
 element face 13776
-property list uchar int vertex_indices
+property list uchar uint vertex_indices
 end_header
 0.0319658 0.433621 0.417442 
 0.0219994 0.420917 0.414026 
 0.0353943 0.417197 0.415157 
-0.0461802 0.426588 0.416634 
+0.0461802 0.426587 0.416634 
 0.0275589 0.408196 0.41513 
 0.0155317 0.410484 0.41262 
 0.0468886 0.410553 0.413456 
@@ -145,7 +146,7 @@ end_header
 0.0591321 0.436584 0.418125 
 0.0700362 0.42435 0.412809 
 0.0353849 0.33504 0.376486 
-0.0398584 0.327525 0.369436 
+0.0398584 0.327525 0.369437 
 0.0541604 0.33372 0.371654 
 0.0556388 0.324525 0.362769 
 0.0402915 0.318332 0.361241 
@@ -154,7 +155,7 @@ end_header
 0.0562801 0.316474 0.351301 
 0.074998 0.329371 0.355057 
 0.0704166 0.335038 0.364534 
-0.0910417 0.345448 0.355245 
+0.0910417 0.345448 0.355246 
 0.0788367 0.326063 0.340032 
 0.0961028 0.34607 0.347235 
 0.0654642 0.299599 0.311366 
@@ -163,7 +164,7 @@ end_header
 0.0841837 0.303741 0.318092 
 0.0666878 0.342123 0.3733 
 0.0722062 0.396539 0.402406 
-0.0610014 0.395624 0.405157 
+0.0610014 0.395624 0.405158 
 0.0754685 0.384128 0.39777 
 0.0919455 0.456648 0.412309 
 0.0750554 0.447655 0.416213 
@@ -187,7 +188,7 @@ end_header
 0.0917596 0.508407 0.399069 
 0.155221 0.44014 0.351024 
 0.15139 0.453366 0.36459 
-0.143419 0.437076 0.371403 
+0.143419 0.437075 0.371403 
 0.14768 0.426584 0.359211 
 0.161016 0.454469 0.339253 
 0.157078 0.471587 0.355109 
@@ -225,7 +226,7 @@ end_header
 0.0473662 0.276207 0.287914 
 0.0608526 0.276314 0.293119 
 0.0546526 0.286054 0.298783 
-0.117759 0.30407 0.29856 
+0.117758 0.30407 0.29856 
 0.124439 0.316301 0.290864 
 0.140558 0.384693 0.335291 
 0.132645 0.378678 0.345409 
@@ -293,7 +294,7 @@ end_header
 0.0225325 0.29988 0.335356 
 0.0352196 0.292252 0.305426 
 0.0485958 0.294723 0.307083 
--0.0136259 0.363304 0.382408 
+-0.0136258 0.363304 0.382408 
 -0.015216 0.363883 0.381477 
 -0.0140381 0.363406 0.379588 
 -0.01196 0.362797 0.380489 
@@ -443,7 +444,7 @@ end_header
 -0.0197305 0.359478 0.373457 
 -0.0188981 0.356224 0.370858 
 0.0188936 0.388855 0.392566 
-0.0200315 0.393652 0.395363 
+0.0200315 0.393652 0.395364 
 -0.0226633 0.361818 0.380647 
 -0.0205714 0.362588 0.380342 
 -0.0248407 0.362286 0.378663 
@@ -517,7 +518,7 @@ end_header
 0.108848 0.376418 0.36626 
 0.108377 0.37853 0.365472 
 0.108228 0.381732 0.36659 
-0.108834 0.386002 0.368588 
+0.108834 0.386002 0.368587 
 0.110203 0.377148 0.364705 
 0.111681 0.374612 0.36559 
 0.10899 0.379175 0.364105 
@@ -630,7 +631,7 @@ end_header
 0.198372 0.022888 0.21474 
 0.20171 0.0421748 0.223523 
 0.0756732 0.114669 0.302626 
-0.0824231 0.127893 0.310036 
+0.0824232 0.127893 0.310036 
 0.0749432 0.135099 0.299693 
 0.0672762 0.121723 0.293129 
 0.12702 -0.0197978 0.418768 
@@ -647,7 +648,7 @@ end_header
 0.138213 0.127905 0.398631 
 0.115478 0.0985256 0.313009 
 0.11517 0.101119 0.323867 
-0.103321 0.10304 0.325735 
+0.103322 0.10304 0.325735 
 0.105102 0.0971388 0.315186 
 0.205039 0.0753631 0.244549 
 0.205749 0.0892238 0.254545 
@@ -670,7 +671,7 @@ end_header
 0.0861779 0.0382716 0.24004 
 0.069845 0.0385186 0.235841 
 0.0656853 -0.0907227 0.208295 
-0.0744524 -0.0744081 0.209353 
+0.0744523 -0.0744081 0.209353 
 0.0540772 -0.069138 0.202 
 0.0455657 -0.0862171 0.199909 
 0.0745591 0.157279 0.2926 
@@ -767,14 +768,14 @@ end_header
 0.223651 0.136203 0.279323 
 0.217828 0.129559 0.280139 
 0.222542 0.125048 0.269117 
-0.228564 0.133243 0.267369 
+0.228565 0.133243 0.267369 
 0.240021 0.110633 0.221522 
 0.238114 0.121867 0.237938 
 0.230308 0.109233 0.245947 
 0.233209 0.0957971 0.228549 
 0.166454 0.0702514 0.0621888 
 0.175193 0.0992378 0.0694943 
-0.193686 0.0923559 0.0828206 
+0.193686 0.0923558 0.0828206 
 0.183144 0.0627611 0.07446 
 0.159907 0.116245 0.318427 
 0.160904 0.0970596 0.322209 
@@ -803,13 +804,13 @@ end_header
 0.190288 -0.00552869 0.343354 
 0.22371 0.0479272 0.182673 
 0.228853 0.0629824 0.166478 
-0.169436 -0.0632333 0.11897 
+0.169436 -0.0632332 0.11897 
 0.173773 -0.0426564 0.120903 
 0.172943 -0.0497081 0.140321 
 0.169548 -0.0688987 0.139047 
 0.160597 -0.093434 0.15974 
 0.167189 -0.084351 0.117441 
-0.167452 -0.088634 0.13861 
+0.167452 -0.0886341 0.13861 
 0.136003 0.308462 0.295044 
 0.143389 0.302491 0.299734 
 0.147666 0.310149 0.281741 
@@ -825,7 +826,7 @@ end_header
 0.151352 -0.176584 0.14712 
 0.131422 -0.183859 0.170709 
 0.125619 0.293026 0.304124 
-0.130368 0.286779 0.311122 
+0.130368 0.286779 0.311123 
 0.11885 0.278237 0.298629 
 0.123173 0.271504 0.301181 
 0.110625 0.266685 0.286398 
@@ -865,10 +866,10 @@ end_header
 0.0193217 -0.101102 0.188237 
 0.0140434 -0.121571 0.182165 
 0.0328451 -0.12468 0.19156 
--0.0253116 -0.0898453 0.161205 
+-0.0253116 -0.0898454 0.161205 
 -0.0285073 -0.111708 0.149852 
 -0.0167028 -0.115257 0.162139 
--0.0120894 -0.0931409 0.172873 
+-0.0120894 -0.0931409 0.172872 
 -0.0338251 -0.238566 0.116122 
 -0.0269411 -0.215551 0.124192 
 -0.0359093 -0.213689 0.112145 
@@ -890,12 +891,12 @@ end_header
 -0.0192036 -0.305199 0.102508 
 -0.0396948 -0.296211 0.109291 
 0.123565 -0.274393 -0.0208503 
-0.14191 -0.280702 -0.00113649 
+0.14191 -0.280702 -0.0011365 
 0.134649 -0.296115 0.00160864 
 0.114605 -0.289825 -0.0139445 
 0.163894 -0.100813 0.0957451 
 0.16269 -0.0805177 0.0981788 
-0.156211 -0.0757802 0.080806 
+0.15621 -0.0757803 0.080806 
 0.159358 -0.0529468 0.0842357 
 0.165685 -0.0577834 0.100324 
 0.222702 0.095153 0.132356 
@@ -963,7 +964,7 @@ end_header
 0.040046 -0.488973 0.0155774 
 0.046548 -0.519169 0.00770366 
 0.0340939 -0.52727 0.0237835 
-0.0284597 -0.495678 0.0348092 
+0.0284598 -0.495678 0.0348092 
 0.0796639 -0.457086 0.156263 
 0.110395 -0.508354 0.124878 
 0.111543 -0.478638 0.134137 
@@ -986,7 +987,7 @@ end_header
 0.109222 -0.537308 0.114575 
 0.0794693 -0.54558 0.12249 
 0.0778799 -0.573025 0.111256 
-0.128156 -0.551585 0.0849722 
+0.128156 -0.551584 0.0849722 
 0.129223 -0.52347 0.0933568 
 0.140476 -0.539265 0.0623096 
 0.141306 -0.510837 0.0690779 
@@ -1055,7 +1056,7 @@ end_header
 0.0792891 -0.652189 0.0899488 
 0.0533036 -0.649774 0.0797198 
 0.0709363 -0.694629 0.0612417 
-0.0632864 -0.687098 0.0403931 
+0.0632863 -0.687098 0.0403931 
 0.148552 -0.654105 -0.00765306 
 0.138518 -0.652173 -0.0190142 
 0.156049 -0.658551 0.00734998 
@@ -1124,7 +1125,7 @@ end_header
 0.177548 -0.870859 -0.00121678 
 0.168984 -0.878577 0.0196723 
 0.130945 -0.926945 0.0270716 
-0.152572 -0.922688 0.0254365 
+0.152572 -0.922688 0.0254364 
 0.147785 -0.884048 0.0371596 
 0.114883 -0.930717 0.0164155 
 0.121599 -0.971776 0.012098 
@@ -1142,7 +1143,7 @@ end_header
 0.173334 -0.947136 -0.0147655 
 0.17174 -0.945015 -0.0277679 
 0.175658 -0.907865 -0.0225072 
-0.168417 -0.950806 0.00443319 
+0.168417 -0.950806 0.0044332 
 0.154935 -0.958501 0.0180373 
 0.0320504 -0.32408 0.023102 
 0.0457194 -0.328692 0.0187109 
@@ -1177,12 +1178,12 @@ end_header
 0.104808 -0.61669 0.085612 
 0.139594 -0.54718 0.0177049 
 0.121583 -0.538801 -0.00769425 
-0.134353 -0.541783 0.00239226 
+0.134353 -0.541784 0.00239226 
 0.0346878 -0.607547 0.0424398 
 0.0379754 -0.616869 0.0655471 
 0.0335947 -0.591376 0.0749838 
 0.0278746 -0.579741 0.052951 
-0.0895843 -0.76509 0.0442873 
+0.0895843 -0.76509 0.0442874 
 0.0809218 -0.75184 0.0228189 
 0.0843345 -0.782173 0.0187897 
 0.085959 -0.737896 0.050344 
@@ -1207,14 +1208,14 @@ end_header
 0.12769 0.0793533 0.270828 
 0.150918 0.126404 0.298417 
 0.149732 0.13192 0.305933 
-0.0153616 0.0396484 0.200868 
+0.0153616 0.0396485 0.200868 
 0.0116051 0.0285775 0.19744 
 0.0311054 0.0212406 0.209245 
 -0.0035223 -0.118743 0.171916 
 5.98391e-05 -0.215952 0.146058 
 -0.0151914 -0.218521 0.133879 
 -0.022899 -0.24378 0.126366 
--0.0495322 -0.291997 0.0928374 
+-0.0495322 -0.291997 0.0928373 
 -0.0477493 -0.302418 0.0754408 
 -0.0344854 -0.311063 0.0633616 
 -0.0237251 -0.31506 0.0544326 
@@ -1375,7 +1376,7 @@ end_header
 0.00673333 -0.429469 0.10997 
 0.00550717 -0.395207 0.121529 
 0.0107627 -0.465429 0.0994777 
-0.0155337 -0.497453 0.0880749 
+0.0155337 -0.497453 0.0880748 
 0.0685745 -0.689441 0.0219349 
 0.0750135 -0.720519 0.0256374 
 0.0864987 -0.816344 0.0146297 
@@ -1387,7 +1388,7 @@ end_header
 0.193127 -0.0532836 0.441466 
 0.20306 -0.0288213 0.430107 
 0.189653 -0.0203745 0.436064 
-0.164329 0.0109801 0.342325 
+0.164329 0.0109801 0.342326 
 0.162121 -0.0168863 0.353995 
 0.150004 -0.00953904 0.356439 
 0.152313 0.0175979 0.346013 
@@ -1475,7 +1476,7 @@ end_header
 0.171973 -0.822764 -0.0375629 
 0.159622 -0.821398 -0.0521882 
 0.174595 -0.782403 -0.0354212 
-0.178626 -0.751257 -0.0090449 
+0.178626 -0.751257 -0.00904489 
 0.180222 -0.7868 -0.0140429 
 0.145797 -0.846594 0.0458016 
 0.159162 -0.859839 -0.0489106 
@@ -1501,7 +1502,7 @@ end_header
 0.17401 -0.0386813 0.163997 
 0.163966 -0.0474693 0.182463 
 0.189936 0.0188955 0.0966721 
-0.201285 0.0441646 0.105228 
+0.201285 0.0441645 0.105228 
 0.205142 0.0331622 0.121523 
 0.192132 0.0100045 0.113651 
 -0.0367738 -0.259094 0.129027 
@@ -1520,7 +1521,7 @@ end_header
 0.131699 0.0893469 0.385981 
 0.0416635 -0.234175 0.184723 
 0.0471417 -0.260272 0.179833 
-0.0683088 -0.260272 0.191719 
+0.0683088 -0.260271 0.191719 
 0.0280685 -0.31771 0.019448 
 0.0335746 -0.314775 0.0140816 
 0.00120284 -0.293161 0.118663 
@@ -1553,7 +1554,7 @@ end_header
 0.147709 0.072988 0.33559 
 0.153261 0.132896 0.312181 
 0.15619 0.128912 0.314207 
-0.0424543 -0.0560875 0.477409 
+0.0424543 -0.0560875 0.47741 
 0.0251258 -0.0475659 0.476487 
 0.0254093 -0.0519426 0.488257 
 0.0427729 -0.0623471 0.488762 
@@ -1573,7 +1574,7 @@ end_header
 0.0132961 -0.119887 0.480541 
 0.096953 -0.10944 0.410784 
 0.108373 -0.102964 0.410904 
-0.107918 -0.115764 0.406584 
+0.107918 -0.115764 0.406585 
 0.0937735 -0.122401 0.409882 
 0.0786264 -0.0724297 0.479156 
 0.0610357 -0.0644572 0.478357 
@@ -1597,11 +1598,11 @@ end_header
 0.00542301 -0.0734461 0.446694 
 -0.0200448 -0.0744537 0.456176 
 0.00446475 -0.0538923 0.505918 
-0.0201153 -0.0692382 0.504775 
+0.0201153 -0.0692381 0.504775 
 0.0237584 -0.0593077 0.497895 
 0.00820482 -0.0456547 0.498347 
 0.0557851 -0.115777 0.421153 
-0.0331554 -0.11277 0.431339 
+0.0331554 -0.11277 0.431338 
 0.0383277 -0.099418 0.429292 
 0.0593392 -0.102536 0.422022 
 0.0757191 -0.069598 0.465326 
@@ -1643,7 +1644,7 @@ end_header
 0.141925 -0.115276 0.39751 
 0.146578 -0.10009 0.390487 
 0.124666 -0.0885862 0.397982 
-0.118937 -0.0785957 0.40545 
+0.118937 -0.0785958 0.40545 
 0.118682 -0.0667081 0.402687 
 0.124984 -0.0751043 0.394048 
 0.137107 -0.0627179 0.455737 
@@ -1663,7 +1664,7 @@ end_header
 0.126656 -0.0670407 0.454193 
 0.120995 -0.0692116 0.454602 
 0.129998 -0.0741629 0.467248 
-0.136792 -0.0740454 0.465998 
+0.136792 -0.0740454 0.465999 
 0.155949 -0.150903 0.461892 
 0.165686 -0.147425 0.452351 
 0.168336 -0.138058 0.46306 
@@ -1715,7 +1716,7 @@ end_header
 0.183858 -0.122098 0.442863 
 0.177347 -0.130979 0.453318 
 0.12806 -0.0573688 0.445029 
-0.124276 -0.0635814 0.440861 
+0.124276 -0.0635815 0.440861 
 0.194507 -0.0837918 0.445039 
 0.184332 -0.07777 0.452371 
 0.176343 -0.0887934 0.461858 
@@ -1758,7 +1759,7 @@ end_header
 0.118059 -0.0704971 0.416691 
 0.12098 -0.0652536 0.429063 
 -0.0275513 -0.0891697 0.475365 
-0.117538 -0.0806064 0.415988 
+0.117538 -0.0806064 0.415989 
 0.118467 -0.0956473 0.487723 
 0.163534 0.272072 0.17205 
 0.151854 0.264868 0.140592 
@@ -1789,7 +1790,7 @@ end_header
 0.136693 -0.0923978 0.0488269 
 0.139992 -0.112436 0.044363 
 0.128071 -0.109298 0.0278228 
-0.125509 -0.0895959 0.0336116 
+0.125509 -0.089596 0.0336116 
 0.115095 -0.121627 0.00592036 
 0.113155 -0.103667 0.012654 
 0.131335 -0.127938 0.0210886 
@@ -1855,7 +1856,7 @@ end_header
 0.216946 0.115127 0.32216 
 0.214874 0.113305 0.319681 
 0.215255 0.128528 0.310212 
-0.217156 0.131094 0.312711 
+0.217156 0.131095 0.312711 
 0.170108 0.23331 0.356616 
 0.209275 0.12298 0.396298 
 0.218127 0.11777 0.384805 
@@ -1865,7 +1866,7 @@ end_header
 0.223464 0.122666 0.358826 
 0.221871 0.138673 0.350046 
 0.222045 0.13712 0.364518 
-0.219487 0.119488 0.327118 
+0.219487 0.119489 0.327118 
 0.219024 0.13638 0.316834 
 0.161131 0.0984604 0.412547 
 0.154135 0.177112 0.389073 
@@ -1935,7 +1936,7 @@ end_header
 -0.0525883 -0.0130248 0.470921 
 -0.0675312 0.000145776 0.476166 
 -0.0735588 -0.00555531 0.478772 
--0.0591058 -0.0197486 0.472673 
+-0.0591058 -0.0197486 0.472672 
 -0.0664981 -0.0247113 0.47706 
 -0.0794875 -0.00987262 0.485042 
 -0.0560829 -0.0459145 0.475067 
@@ -2003,7 +2004,7 @@ end_header
 -0.0963943 0.0913958 0.475862 
 -0.0924307 0.099142 0.479138 
 -0.0847526 0.0993958 0.483637 
--0.0854237 0.0865462 0.479199 
+-0.0854238 0.0865462 0.479199 
 -0.0755875 0.0819873 0.50494 
 -0.0792652 0.09643 0.504136 
 -0.0794301 0.0939127 0.514988 
@@ -2059,12 +2060,12 @@ end_header
 -0.105505 0.0781968 0.507196 
 -0.105809 0.0752177 0.512818 
 -0.109093 0.0707733 0.523856 
--0.11153 0.070688 0.52924 
+-0.11153 0.070688 0.529239 
 -0.112056 0.0788029 0.530088 
 -0.109335 0.0824901 0.523724 
 -0.108589 0.104623 0.519421 
 -0.109676 0.10318 0.52299 
--0.10777 0.113027 0.522721 
+-0.10777 0.113027 0.522722 
 -0.106806 0.114688 0.519167 
 -0.0824492 0.00617065 0.53348 
 -0.0731495 0.010517 0.535827 
@@ -2098,7 +2099,7 @@ end_header
 -0.0942613 0.0103113 0.532414 
 -0.0900672 0.0037168 0.528166 
 -0.107606 0.0405161 0.511478 
--0.107095 0.0423923 0.508295 
+-0.107095 0.0423924 0.508295 
 -0.106723 0.0271437 0.501598 
 -0.106361 0.0256074 0.506249 
 -0.110198 0.0501744 0.490332 
@@ -2123,15 +2124,15 @@ end_header
 -0.111124 0.0614361 0.458898 
 -0.110956 0.06757 0.464749 
 -0.111472 0.0718316 0.460486 
--0.112029 0.0675815 0.454858 
+-0.112029 0.0675815 0.454857 
 -0.0989768 0.0969728 0.479007 
 -0.0971774 0.106256 0.480488 
--0.0896464 0.107985 0.481529 
+-0.0896465 0.107985 0.481529 
 -0.10866 0.0281049 0.536136 
 -0.111048 0.0344336 0.525567 
 -0.104667 0.110334 0.493966 
 -0.0883007 0.142281 0.48565 
--0.0860905 0.12697 0.486126 
+-0.0860905 0.12697 0.486127 
 -0.0906529 0.123397 0.482743 
 -0.0918787 0.140658 0.482174 
 -0.101691 0.120708 0.496071 
@@ -2152,7 +2153,7 @@ end_header
 -0.10418 0.112027 0.497946 
 -0.106547 0.050061 0.551751 
 -0.0951262 0.0527855 0.551401 
--0.0961847 0.143018 0.481969 
+-0.0961846 0.143018 0.481969 
 -0.0966505 0.147133 0.482701 
 -0.0784901 -0.000420442 0.528951 
 -0.0857187 -0.00251824 0.523483 
@@ -2187,7 +2188,7 @@ end_header
 -0.094375 0.120003 0.501544 
 -0.0891943 0.118695 0.504328 
 -0.0884115 0.119228 0.498406 
--0.10966 0.0591191 0.523792 
+-0.10966 0.0591192 0.523792 
 -0.0958995 0.138578 0.481118 
 -0.0635455 0.0372015 0.491481 
 -0.0679746 0.04303 0.490633 
@@ -2253,7 +2254,7 @@ end_header
 -0.0832609 -0.0142295 0.50232 
 -0.0437262 0.00825834 0.514401 
 -0.0987994 0.156775 0.482516 
--0.103277 0.117205 0.52249 
+-0.103277 0.117206 0.52249 
 -0.102735 0.119566 0.521176 
 -0.107613 0.0570476 0.453576 
 -0.108846 0.0644758 0.45012 
@@ -2340,9 +2341,9 @@ end_header
 -0.105175 0.18197 0.487548 
 -0.100915 0.165058 0.4835 
 -0.103151 0.164354 0.485167 
--0.105248 0.18457 0.488576 
+-0.105248 0.18457 0.488577 
 -0.104539 0.189879 0.491154 
--0.0993374 0.180898 0.49808 
+-0.0993373 0.180898 0.49808 
 -0.101444 0.186866 0.497116 
 -0.099915 0.180549 0.498505 
 -0.101853 0.18648 0.497054 
@@ -2365,7 +2366,7 @@ end_header
 -0.10538 0.18544 0.49072 
 -0.104906 0.186098 0.493032 
 -0.103211 0.155669 0.509007 
--0.102582 0.150096 0.508944 
+-0.102582 0.150096 0.508943 
 -0.104127 0.149809 0.513225 
 -0.10512 0.155735 0.514163 
 -0.0928159 0.15823 0.50943 
@@ -2414,7 +2415,7 @@ end_header
 -0.106421 0.170314 0.519696 
 -0.0980074 0.170102 0.519693 
 -0.0951291 0.16855 0.514576 
--0.0972286 0.175216 0.51481 
+-0.0972285 0.175216 0.51481 
 -0.0994251 0.175653 0.519444 
 -0.0980662 0.176053 0.509659 
 -0.101423 0.174887 0.507705 
@@ -2423,7 +2424,7 @@ end_header
 -0.103242 0.176598 0.521594 
 -0.106491 0.174035 0.519297 
 -0.10198 0.183753 0.521421 
--0.100649 0.184741 0.519842 
+-0.100649 0.184741 0.519843 
 -0.1032 0.195586 0.510615 
 -0.102571 0.196247 0.511499 
 -0.101459 0.190932 0.511422 
@@ -2458,7 +2459,7 @@ end_header
 -0.104007 0.196571 0.520843 
 -0.103496 0.190388 0.521708 
 -0.104899 0.19697 0.521344 
--0.103732 0.200735 0.519264 
+-0.103732 0.200735 0.519265 
 -0.10434 0.200904 0.519914 
 -0.105689 0.173477 0.510782 
 -0.106639 0.173659 0.51594 
@@ -2520,11 +2521,11 @@ end_header
 -0.095066 0.119788 0.534073 
 -0.109831 0.113191 0.538692 
 -0.107385 0.156026 0.530275 
--0.105131 0.156786 0.532647 
+-0.105131 0.156787 0.532647 
 -0.113284 0.139165 0.537458 
 -0.11552 0.153007 0.539845 
 -0.106234 0.155576 0.542162 
--0.103963 0.15551 0.537457 
+-0.103963 0.155511 0.537457 
 -0.105625 0.160457 0.538371 
 -0.107875 0.159785 0.542866 
 -0.106741 0.161818 0.533432 
@@ -2536,7 +2537,7 @@ end_header
 -0.112414 0.163499 0.544837 
 -0.10962 0.166206 0.543219 
 -0.112512 0.177799 0.535748 
--0.11201 0.178032 0.536787 
+-0.11201 0.178032 0.536788 
 -0.111068 0.172986 0.535529 
 -0.11096 0.173046 0.534773 
 -0.109425 0.169251 0.535787 
@@ -2550,7 +2551,7 @@ end_header
 -0.113104 0.181185 0.537798 
 -0.112796 0.181161 0.538126 
 -0.107797 0.166612 0.539126 
--0.110342 0.165693 0.53263 
+-0.110342 0.165693 0.532629 
 -0.11263 0.171971 0.533642 
 -0.114408 0.176653 0.535113 
 -0.115668 0.179549 0.537077 
@@ -2581,7 +2582,7 @@ end_header
 -0.118239 0.167905 0.542195 
 -0.117668 0.17395 0.544749 
 -0.118423 0.172832 0.543483 
--0.118659 0.173442 0.539448 
+-0.118659 0.173442 0.539447 
 -0.117863 0.176816 0.543981 
 -0.116361 0.178109 0.544936 
 -0.117915 0.178194 0.540929 
@@ -2677,7 +2678,7 @@ end_header
 -0.122593 0.140553 0.56247 
 -0.124086 0.145778 0.565036 
 -0.118591 0.137282 0.560433 
--0.120621 0.134339 0.56011 
+-0.120621 0.134339 0.560109 
 -0.124742 0.148291 0.566838 
 -0.124584 0.149802 0.570082 
 -0.122266 0.151456 0.571366 
@@ -2735,7 +2736,7 @@ end_header
 -0.110707 0.107812 0.438303 
 -0.109196 0.11487 0.44081 
 -0.108224 0.111138 0.440832 
--0.113181 0.116015 0.437415 
+-0.113181 0.116015 0.437414 
 -0.118608 0.116142 0.436199 
 -0.115478 0.120142 0.437135 
 -0.120807 0.115875 0.436641 
@@ -2833,7 +2834,7 @@ end_header
 0.0228318 -0.037698 0.195095 
 0.192335 0.000276424 0.130167 
 0.206314 0.0219452 0.13835 
-0.171293 0.0353231 0.0679148 
+0.171292 0.0353231 0.0679148 
 0.18241 0.0275875 0.0805681 
 0.145865 -0.0257133 0.22197 
 0.139157 -0.0446629 0.21495 
@@ -2903,7 +2904,7 @@ end_header
 0.193009 0.160501 0.394033 
 0.207742 0.156117 0.384218 
 0.208756 0.0446554 0.40603 
-0.211093 0.0285658 0.406947 
+0.211094 0.0285658 0.406947 
 0.215675 0.0191456 0.402982 
 0.220247 0.0127037 0.395093 
 0.222786 0.0155354 0.380546 
@@ -3051,14 +3052,14 @@ end_header
 0.0763323 0.0756932 0.272453 
 0.057017 0.0782752 0.263645 
 0.0935253 0.0755242 0.275611 
-0.0848417 0.0756117 0.275359 
+0.0848417 0.0756117 0.275358 
 0.00643238 0.299583 0.326654 
 -0.00519775 0.296839 0.336519 
 -0.0188381 0.333399 0.360757 
 -0.0178507 0.338628 0.362004 
 0.0179083 0.302399 0.306282 
 0.021088 0.294222 0.298285 
--0.0203024 0.316388 0.350225 
+-0.0203025 0.316388 0.350225 
 -0.0196113 0.303008 0.342847 
 -0.0203321 0.359723 0.372519 
 -0.0196966 0.356507 0.369887 
@@ -3129,7 +3130,7 @@ end_header
 0.119563 -0.234655 -0.0364528 
 0.124427 -0.212723 -0.0337032 
 0.158629 -0.151814 0.046534 
-0.150588 -0.114052 0.0587864 
+0.150587 -0.114052 0.0587864 
 0.155713 -0.133366 0.0538383 
 0.152208 -0.0471493 0.0695141 
 0.0489938 -0.544345 0.115715 
@@ -3148,14 +3149,14 @@ end_header
 0.1707 -0.241951 0.035595 
 0.14833 -0.259865 -0.00217591 
 0.130705 -0.254562 -0.0215658 
-0.115261 -0.251569 -0.0370006 
+0.115261 -0.251568 -0.0370006 
 -0.0468066 -0.277233 0.119785 
 -0.057529 -0.264827 0.110263 
 -0.0496314 -0.262498 0.116709 
 -0.0605746 -0.24809 0.105048 
 -0.0627503 -0.265589 0.100774 
 -0.0547132 -0.279777 0.105609 
--0.0578728 -0.278637 0.0936302 
+-0.0578728 -0.278637 0.0936301 
 -0.0530949 -0.290209 0.0858954 
 0.00961772 -0.162106 0.160988 
 0.0265074 -0.162218 0.173579 
@@ -3199,7 +3200,7 @@ end_header
 0.158565 -0.9932 0.016328 
 0.162192 -1.01998 0.0131121 
 0.142028 -1.00088 0.0207129 
-0.11766 -0.999939 -0.0102555 
+0.11766 -0.999939 -0.0102556 
 0.122638 -1.02789 -0.00585586 
 0.14924 -1.00888 -0.0588051 
 0.137516 -1.01353 -0.0535765 
@@ -3267,7 +3268,7 @@ end_header
 0.169398 -1.15753 0.160688 
 0.175371 -1.15533 0.161076 
 0.167101 -1.16609 0.159742 
-0.165204 -1.1631 0.148871 
+0.165204 -1.16309 0.148871 
 0.17018 -1.16582 0.147081 
 0.171813 -1.16872 0.1586 
 0.178365 -1.16228 0.147508 
@@ -3282,7 +3283,7 @@ end_header
 0.187385 -1.14967 0.149851 
 0.183735 -1.15475 0.150163 
 0.183927 -1.15087 0.142426 
-0.187068 -1.15818 0.138881 
+0.187068 -1.15819 0.138881 
 0.186919 -1.16168 0.147549 
 0.195011 -1.15551 0.136285 
 0.19319 -1.15978 0.1468 
@@ -3399,7 +3400,7 @@ end_header
 0.168949 -1.13445 -0.0688335 
 0.183528 -1.08384 -0.0561541 
 0.186461 -1.08558 -0.0442628 
-0.175254 -1.0855 -0.0704894 
+0.175254 -1.0855 -0.0704895 
 0.16189 -1.08934 -0.0662909 
 0.154011 -1.12143 -0.0505642 
 0.151466 -1.09257 -0.0478344 
@@ -3438,7 +3439,7 @@ end_header
 0.166158 -1.14706 -0.0515202 
 0.162473 -1.14729 -0.0409403 
 0.155954 -1.14466 -0.0121984 
-0.160284 -1.14634 -0.0282847 
+0.160285 -1.14634 -0.0282847 
 0.12753 -1.08222 -0.0145532 
 0.135982 -1.0774 -0.0310179 
 0.142333 -1.07478 -0.0386399 
@@ -3449,9 +3450,9 @@ end_header
 0.192397 -1.14788 0.0565107 
 0.207186 -1.14349 0.0539931 
 0.208402 -1.14024 0.0278932 
-0.195376 -1.1444 0.0291206 
+0.195376 -1.1444 0.0291207 
 0.179425 -1.14834 0.0313923 
-0.162916 -1.14864 0.0334586 
+0.162916 -1.14864 0.0334587 
 0.206103 -1.13987 0.00350845 
 0.196159 -1.14371 0.0060879 
 0.180361 -1.14696 0.00458385 
@@ -3477,7 +3478,7 @@ end_header
 0.178753 -1.14449 -0.0561231 
 0.184715 -1.14289 -0.0566509 
 0.177253 -1.068 0.018655 
-0.11772 0.33292 0.213946 
+0.11772 0.33292 0.213947 
 0.143347 0.285705 0.15975 
 0.075228 -0.707257 0.0426247 
 0.0821021 -0.714932 0.055021 
@@ -3563,7 +3564,7 @@ end_header
 -0.0296433 0.365282 0.335098 
 -0.0271341 0.367947 0.341134 
 -0.0278081 0.374842 0.338823 
--0.0225221 0.366899 0.357088 
+-0.0225221 0.366899 0.357087 
 -0.0275295 0.352955 0.350265 
 -0.0261866 0.35112 0.357144 
 -0.025693 0.3548 0.358327 
@@ -3577,7 +3578,7 @@ end_header
 -0.0212012 0.325432 0.309869 
 -0.0255226 0.328226 0.322146 
 -0.0263393 0.336757 0.319285 
--0.0223273 0.336211 0.308109 
+-0.0223274 0.336211 0.308109 
 -0.0245885 0.326823 0.337756 
 -0.0259783 0.31906 0.326525 
 -0.0256926 0.310949 0.332663 
@@ -3617,7 +3618,7 @@ end_header
 -0.0279255 0.362087 0.344099 
 -0.0300402 0.354826 0.329595 
 -0.0305309 0.36184 0.329502 
--0.029185 0.350367 0.327326 
+-0.029185 0.350367 0.327325 
 -0.0282345 0.345094 0.326683 
 -0.0255219 0.33254 0.332601 
 -0.0268761 0.338855 0.328504 
@@ -3671,7 +3672,7 @@ end_header
 0.0108187 0.361897 0.262089 
 0.0162923 0.324469 0.261048 
 0.0151061 0.338161 0.245557 
-0.0158093 0.329849 0.236007 
+0.0158093 0.32985 0.236007 
 0.0160714 0.316095 0.249613 
 -0.0119018 0.38822 0.285628 
 -0.0115692 0.43988 0.307698 
@@ -3679,7 +3680,7 @@ end_header
 -0.0171275 0.435673 0.317897 
 0.00633617 0.500133 0.314886 
 0.00591081 0.486909 0.300662 
--0.00365038 0.477157 0.311303 
+-0.00365038 0.477158 0.311303 
 -0.00473902 0.487234 0.327037 
 0.016803 0.520988 0.353444 
 0.0107406 0.512422 0.333392 
@@ -3707,7 +3708,7 @@ end_header
 0.0907998 0.434965 0.240765 
 0.0863138 0.415475 0.240365 
 0.0697326 0.414301 0.239083 
--0.026695 0.437025 0.342848 
+-0.026695 0.437025 0.342847 
 -0.0239191 0.43518 0.334103 
 -0.0245126 0.429727 0.339481 
 -0.0261885 0.430155 0.346102 
@@ -3749,7 +3750,7 @@ end_header
 -0.0193215 0.391789 0.356313 
 -0.013578 0.406566 0.355345 
 -0.0115778 0.406778 0.358665 
--0.0111975 0.410293 0.358578 
+-0.0111975 0.410293 0.358579 
 -0.0137706 0.410799 0.356404 
 -0.0167716 0.432661 0.384956 
 -0.0206984 0.438885 0.375435 
@@ -3793,7 +3794,7 @@ end_header
 0.0810632 0.53526 0.290741 
 0.0641868 0.523907 0.280294 
 0.036833 0.483943 0.267953 
-0.0996131 0.541878 0.30899 
+0.0996131 0.541878 0.308989 
 -0.0243663 0.359035 0.359473 
 -0.0228157 0.362343 0.362455 
 -0.0251526 0.362994 0.352086 
@@ -3869,7 +3870,7 @@ end_header
 -0.0148071 0.402034 0.355394 
 -0.0171833 0.397086 0.355565 
 -0.0246786 0.40213 0.339643 
--0.0221855 0.428023 0.333924 
+-0.0221855 0.428023 0.333925 
 -0.0212894 0.418036 0.332342 
 0.0307661 0.41634 0.248657 
 0.0359221 0.43093 0.245219 
@@ -4013,7 +4014,7 @@ end_header
 0.0271597 0.428614 0.25915 
 0.0287414 0.432813 0.258858 
 0.0139409 0.424858 0.268905 
-0.0272718 0.422603 0.251502 
+0.0272719 0.422603 0.251502 
 0.0295264 0.433018 0.249648 
 0.0161783 0.421374 0.244452 
 0.016963 0.414942 0.245847 
@@ -4022,7 +4023,7 @@ end_header
 0.0322962 0.453269 0.258461 
 0.0313593 0.44706 0.255114 
 0.0209082 0.41251 0.257931 
-0.0316334 0.441492 0.251011 
+0.0316335 0.441492 0.251011 
 0.0213114 0.431591 0.244044 
 0.0193695 0.426035 0.247189 
 0.0232921 0.438661 0.241683 
@@ -4043,7 +4044,7 @@ end_header
 0.0289135 0.440676 0.253289 
 0.0265055 0.444429 0.252994 
 0.0248404 0.443098 0.249379 
-0.0228965 0.438395 0.245885 
+0.0228965 0.438396 0.245885 
 0.023795 0.443382 0.245455 
 0.0255425 0.444182 0.242037 
 0.0253694 0.448699 0.246717 
@@ -4111,7 +4112,7 @@ end_header
 -0.0760533 0.1517 0.112685 
 -0.0531123 0.0774426 0.0788689 
 -0.0547333 0.0621397 0.0690299 
--0.054961 0.0545775 0.0880827 
+-0.054961 0.0545775 0.0880826 
 -0.0541434 0.0696706 0.0961986 
 -0.038973 0.132097 0.0245111 
 -0.0241049 0.142247 0.00344224 
@@ -4123,7 +4124,7 @@ end_header
 -0.0813361 0.191909 0.163485 
 -0.256991 0.207142 0.0311785 
 -0.248622 0.187398 0.0308779 
--0.22478 0.18784 0.0477802 
+-0.22478 0.18784 0.0477801 
 -0.235384 0.206465 0.04691 
 -0.044693 0.0130734 -0.00607422 
 -0.0267309 0.019769 -0.0155787 
@@ -4164,7 +4165,7 @@ end_header
 -0.048095 0.20138 0.177438 
 -0.032383 0.195888 0.185014 
 -0.0717887 0.12462 0.122808 
--0.0643527 0.114813 0.117659 
+-0.0643526 0.114813 0.117659 
 -0.0638198 0.108412 0.127133 
 -0.0725137 0.118865 0.131151 
 -0.0858029 0.207378 0.103293 
@@ -4185,7 +4186,7 @@ end_header
 -0.0441984 0.0898806 0.164117 
 -0.0594999 0.108464 0.109461 
 -0.055793 0.102063 0.0999901 
--0.0593902 0.101825 0.120601 
+-0.0593902 0.101825 0.1206 
 -0.0272816 0.0881729 0.181923 
 -0.0372857 0.076934 0.166381 
 -0.017325 0.189495 0.193994 
@@ -4200,7 +4201,7 @@ end_header
 0.0235831 0.273805 0.218468 
 -0.0971183 0.196313 0.112794 
 -0.0898076 0.187163 0.109863 
--0.0829586 -0.0386359 0.019104 
+-0.0829585 -0.0386359 0.019104 
 0.0135886 0.32637 0.0767697 
 0.026095 0.314838 0.0666582 
 0.000149374 0.313282 0.0655697 
@@ -4279,7 +4280,7 @@ end_header
 -0.0390677 0.338372 0.141339 
 -0.0491994 0.324606 0.154744 
 -0.0253102 0.34108 0.148285 
--0.101404 0.216236 0.050849 
+-0.101404 0.216236 0.0508489 
 -0.123197 0.204604 0.0320328 
 -0.124586 0.210248 0.0221488 
 -0.105221 0.21926 0.0395098 
@@ -4372,7 +4373,7 @@ end_header
 -0.128478 -0.330497 9.78914e-05 
 -0.141959 -0.330288 -0.0308326 
 -0.0428758 -0.297164 0.0557488 
--0.0614944 -0.274116 0.0648169 
+-0.0614944 -0.274116 0.064817 
 -0.0541492 -0.275944 0.047053 
 -0.035975 -0.299023 0.0471831 
 0.0918568 -0.263667 -0.057672 
@@ -4409,7 +4410,7 @@ end_header
 -0.0982915 -0.198262 0.0234161 
 -0.103283 -0.220256 0.0153784 
 -0.0854725 -0.234912 0.0332824 
--0.0497883 -0.05676 0.134126 
+-0.0497883 -0.0567599 0.134126 
 -0.0581547 -0.0502522 0.122295 
 -0.0675452 -0.0692482 0.101199 
 -0.0634938 -0.0456073 0.107488 
@@ -4443,7 +4444,7 @@ end_header
 -0.122298 -0.461747 -0.0164558 
 -0.126594 -0.429543 -0.013507 
 -0.0176734 -0.407723 -0.00219569 
--0.0392412 -0.409558 0.00457259 
+-0.0392412 -0.409559 0.00457259 
 -0.0387579 -0.43827 -0.00495804 
 -0.0194809 -0.437874 -0.0161508 
 -0.0225963 -0.468287 -0.0272471 
@@ -4487,7 +4488,7 @@ end_header
 -0.00299865 -0.493054 -0.0685328 
 -0.000337522 -0.490531 -0.0875864 
 0.00609894 -0.463181 -0.0796249 
-0.00304941 -0.465374 -0.0590596 
+0.00304941 -0.465373 -0.0590595 
 -0.0156573 -0.60212 -0.0911575 
 -0.0232751 -0.598532 -0.104065 
 -0.0171278 -0.570004 -0.100217 
@@ -4502,7 +4503,7 @@ end_header
 -0.0690691 -0.599752 -0.138421 
 -0.0927684 -0.605419 -0.139635 
 -0.0884945 -0.577116 -0.136584 
--0.110023 -0.614346 -0.124087 
+-0.110023 -0.614347 -0.124087 
 -0.108018 -0.585985 -0.125687 
 -0.117527 -0.596366 -0.0973748 
 -0.122278 -0.624779 -0.0965586 
@@ -4534,7 +4535,7 @@ end_header
 -0.0166607 -0.542021 -0.113574 
 -0.0116976 -0.542911 -0.0975438 
 -0.0108698 -0.545472 -0.081062 
--0.0210951 -0.553602 -0.0512739 
+-0.0210951 -0.553602 -0.0512738 
 -0.034804 -0.558391 -0.0394911 
 -0.120073 -0.62595 -0.0433361 
 -0.121617 -0.598563 -0.0370763 
@@ -4603,7 +4604,7 @@ end_header
 -0.0429687 -0.877275 -0.0539287 
 -0.0150255 -0.796621 -0.113231 
 -0.0130472 -0.796954 -0.0955969 
--0.0212667 -0.834328 -0.0922459 
+-0.0212667 -0.834328 -0.092246 
 -0.0225902 -0.834834 -0.106817 
 -0.0919332 -0.891842 -0.116819 
 -0.0966109 -0.849415 -0.128255 
@@ -4641,7 +4642,7 @@ end_header
 -0.0570768 -0.535007 -0.0232871 
 -0.0539229 -0.563942 -0.0324227 
 -0.0497802 -0.592394 -0.0427599 
--0.0503874 -0.620967 -0.0496889 
+-0.0503874 -0.620966 -0.0496889 
 -0.055758 -0.647219 -0.0534759 
 -0.0612708 -0.676472 -0.0572795 
 -0.0643174 -0.734724 -0.0566651 
@@ -4669,14 +4670,14 @@ end_header
 -0.105628 -0.546459 -0.0247448 
 -0.102297 -0.572549 -0.0273301 
 -0.109182 -0.724678 -0.0642121 
--0.0877696 -0.74374 -0.0575643 
+-0.0877696 -0.743739 -0.0575643 
 -0.0876234 -0.714038 -0.0580309 
 -0.111438 -0.696794 -0.0635202 
 -0.0410024 -0.689282 -0.145892 
 -0.0580581 -0.690453 -0.152794 
 -0.0232371 -0.723428 -0.135137 
 -0.0371692 -0.722844 -0.148082 
--0.0276087 -0.68972 -0.134295 
+-0.0276086 -0.68972 -0.134295 
 -0.0926375 -0.487801 -0.00955818 
 -0.115631 -0.491518 -0.0203332 
 -0.109081 -0.519114 -0.0239274 
@@ -4722,7 +4723,7 @@ end_header
 -0.085362 -0.26288 -0.0999083 
 0.0218406 -0.320722 -0.0891361 
 -0.224736 0.194654 -0.0455003 
--0.223824 0.206338 -0.0535014 
+-0.223824 0.206337 -0.0535014 
 -0.199345 0.202881 -0.0356025 
 -0.197906 0.190772 -0.0260538 
 0.114723 0.212484 0.0368272 
@@ -4756,7 +4757,7 @@ end_header
 -0.0189004 -0.27951 -0.116068 
 0.0023506 0.0893225 -0.0169485 
 0.0243206 0.0946263 -0.0222291 
-0.0232667 0.0704655 -0.023225 
+0.0232667 0.0704655 -0.0232249 
 0.0017935 0.0653394 -0.0184049 
 -0.00792049 0.151088 -0.00996905 
 -0.00330647 0.131756 -0.0135736 
@@ -4849,7 +4850,7 @@ end_header
 -0.103456 -0.359622 0.0145688 
 -0.0991588 -0.425723 0.00116691 
 -0.0961605 -0.458543 -0.00390484 
--0.0746277 -0.653383 -0.0530086 
+-0.0746277 -0.653382 -0.0530086 
 -0.0846764 -0.683404 -0.0562305 
 -0.0876195 -0.777129 -0.0557767 
 -0.0858981 -0.81165 -0.0541919 
@@ -5001,7 +5002,7 @@ end_header
 -0.01354 -0.549507 -0.0653736 
 -0.0121749 -0.578079 -0.070671 
 -0.0131911 -0.604834 -0.0783068 
--0.0206527 -0.632539 -0.0826875 
+-0.0206527 -0.632538 -0.0826875 
 -0.0257704 -0.664413 -0.0864576 
 -0.027315 -0.731469 -0.0717497 
 -0.0189031 -0.729745 -0.0852251 
@@ -5024,7 +5025,7 @@ end_header
 -0.0827253 0.224113 0.0976618 
 -0.411881 0.222963 -0.00587441 
 -0.412064 0.233806 -0.0151211 
--0.431286 0.227031 -0.00829252 
+-0.431286 0.227031 -0.00829251 
 -0.431884 0.216861 -0.00102968 
 -0.412678 0.210388 -0.00101238 
 -0.432931 0.205531 0.00233311 
@@ -5040,7 +5041,7 @@ end_header
 -0.430532 0.205232 -0.0729298 
 -0.411644 0.203986 -0.0801838 
 -0.410231 0.187925 -0.0753459 
--0.326562 0.181174 -0.0491689 
+-0.326562 0.181175 -0.0491689 
 -0.33125 0.177116 -0.0637457 
 -0.317242 0.182971 -0.0605257 
 -0.315162 0.188162 -0.0453594 
@@ -5053,7 +5054,7 @@ end_header
 -0.34588 0.190134 -0.0235729 
 -0.347941 0.203102 -0.0125447 
 -0.356767 0.245852 -0.0256944 
--0.357725 0.254483 -0.041704 
+-0.357726 0.254483 -0.041704 
 -0.413957 0.185166 -0.00791437 
 -0.414262 0.176875 -0.018413 
 -0.391867 0.178212 -0.019429 
@@ -5073,7 +5074,7 @@ end_header
 -0.368878 0.173697 -0.0345778 
 -0.39397 0.171819 -0.0320293 
 -0.398035 0.168832 -0.0466421 
--0.371384 0.224038 -0.00882794 
+-0.371384 0.224038 -0.00882795 
 -0.391864 0.216432 -0.00414737 
 -0.416389 0.242633 -0.0420807 
 -0.413676 0.24054 -0.0280043 
@@ -5083,7 +5084,7 @@ end_header
 -0.386085 0.228619 -0.0814793 
 -0.404093 0.22881 -0.0723699 
 -0.40075 0.238644 -0.0650939 
--0.347606 0.173358 -0.0504987 
+-0.347605 0.173358 -0.0504987 
 -0.355111 0.171985 -0.065615 
 -0.388316 0.184978 -0.0785899 
 -0.391545 0.200985 -0.0859528 
@@ -5154,7 +5155,7 @@ end_header
 -0.278564 0.191311 0.0124366 
 -0.27806 0.208089 0.0155767 
 -0.293333 0.21225 -0.000445623 
--0.291751 0.196258 -0.00583139 
+-0.291751 0.196259 -0.00583139 
 -0.277427 0.24452 -0.00186669 
 -0.275455 0.253608 -0.01676 
 -0.284658 0.262894 -0.0447404 
@@ -5238,7 +5239,7 @@ end_header
 -0.0767201 0.2262 0.0875245 
 -0.0784516 0.226408 0.0869072 
 -0.057438 0.16646 0.0721225 
-0.0359667 0.463294 0.259703 
+0.0359667 0.463293 0.259703 
 0.038281 0.45808 0.25708 
 0.0337413 0.47512 0.266144 
 -0.0338918 -0.0474291 0.144397 
@@ -5292,7 +5293,7 @@ end_header
 -0.0680772 0.329489 0.132936 
 -0.0611355 0.332612 0.112978 
 -0.0788837 0.325863 0.109516 
--0.0857248 0.324227 0.127391 
+-0.0857247 0.324227 0.127391 
 -0.0343246 0.317535 0.07253 
 -0.0229529 0.309398 0.0640352 
 -0.0413249 0.307339 0.064461 
@@ -5326,7 +5327,7 @@ end_header
 -0.0951721 0.301375 0.0600822 
 -0.0870345 0.293892 0.0548671 
 -0.100285 0.288351 0.050653 
--0.0811047 0.262366 0.0450075 
+-0.0811047 0.262367 0.0450075 
 -0.0666726 0.267553 0.0490561 
 -0.165491 0.272732 0.104745 
 -0.10971 0.307142 0.140488 
@@ -5385,9 +5386,9 @@ end_header
 -0.352775 0.232725 -0.0134388 
 -0.350415 0.217685 -0.00861951 
 -0.345265 0.179761 -0.0371958 
--0.370567 0.212963 -0.0925574 
--0.493206 0.208815 0.0109005 
--0.491824 0.219478 0.00705792 
+-0.370566 0.212963 -0.0925574 
+-0.493207 0.208815 0.0109005 
+-0.491824 0.219479 0.00705792 
 -0.512468 0.217899 0.00913608 
 -0.513924 0.207066 0.0116689 
 -0.537052 0.205156 0.0120503 
@@ -5555,16 +5556,16 @@ end_header
 -0.616459 0.27895 -0.0400039 
 -0.633538 0.196632 0.0135043 
 -0.621902 0.20277 0.014379 
--0.567654 0.247153 -0.0431126 
+-0.567654 0.247153 -0.0431127 
 -0.561908 0.240693 -0.0419285 
--0.554607 0.235825 -0.0399465 
+-0.554608 0.235825 -0.0399465 
 -0.595558 0.227421 -0.0302854 
 -0.583772 0.219395 -0.03326 
 -0.586433 0.215094 -0.0299716 
 -0.597857 0.225051 -0.0278299 
 -0.610245 0.211635 -0.0213542 
 -0.598982 0.203043 -0.0242105 
--0.61927 0.204518 -0.0156124 
+-0.619271 0.204518 -0.0156124 
 -0.574426 0.208825 -0.0285771 
 -0.562997 0.212042 -0.0329781 
 -0.56467 0.204765 -0.0241504 
@@ -5643,7 +5644,7 @@ end_header
 -0.597111 0.268731 -0.0144394 
 -0.60988 0.274503 -0.0144255 
 -0.623222 0.278892 -0.0141553 
--0.62637 0.274023 -0.00377815 
+-0.62637 0.274022 -0.00377815 
 -0.594112 0.271269 -0.0211329 
 -0.657907 0.25246 0.00770136 
 -0.655854 0.249237 0.0150115 
@@ -5670,10 +5671,10 @@ end_header
 -0.591841 0.253513 0.000612695 
 -0.57953 0.248334 0.000165961 
 -0.569718 0.243328 0.00169872 
--0.56194 0.240806 0.00179917 
+-0.56194 0.240807 0.00179917 
 -0.559353 0.246928 -0.0041115 
 -0.679394 0.248703 0.0278017 
--0.684926 0.254598 0.0245359 
+-0.684926 0.254598 0.0245358 
 -0.550059 0.245072 -0.00337179 
 -0.540062 0.242468 -0.00379509 
 -0.542262 0.235808 0.00174176 
@@ -5754,7 +5755,7 @@ end_header
 -0.698743 0.250522 0.0294967 
 -0.576403 0.254788 -0.0440321 
 -0.605105 0.234063 -0.0260537 
--0.537581 0.218878 -0.0399414 
+-0.537582 0.218878 -0.0399414 
 -0.621776 0.194052 0.0148215 
 -0.633089 0.190365 0.0154245 
 -0.654906 0.255982 -0.00741554 
@@ -5781,7 +5782,7 @@ end_header
 -0.722041 0.251147 0.0330054 
 -0.712219 0.254316 0.0318789 
 -0.717652 0.256814 0.0329781 
--0.717526 0.259857 0.0296655 
+-0.717525 0.259857 0.0296655 
 -0.7225 0.261168 0.0307427 
 -0.722786 0.258614 0.0341326 
 -0.713682 0.258047 0.0295245 
@@ -5817,7 +5818,7 @@ end_header
 -0.711321 0.258895 0.021261 
 -0.717141 0.260206 0.0231864 
 -0.717078 0.257588 0.0207563 
--0.721517 0.26071 0.0245283 
+-0.721517 0.26071 0.0245284 
 -0.721138 0.258683 0.0231089 
 -0.721643 0.254915 0.0236824 
 -0.725061 0.259734 0.025351 
@@ -5869,7 +5870,7 @@ end_header
 -0.702724 0.279201 0.00638278 
 -0.70383 0.271969 0.0128259 
 -0.659332 0.266611 0.0128418 
--0.700013 0.274883 0.0183274 
+-0.700013 0.274882 0.0183274 
 -0.702233 0.271688 0.0149255 
 -0.68939 0.277063 0.00277363 
 -0.701102 0.283031 0.00493049 
@@ -5951,7 +5952,7 @@ end_header
 -0.669776 0.283313 0.000231104 
 -0.657617 0.276672 -0.0021695 
 -0.654654 0.279638 0.000631157 
--0.678587 0.285852 -0.00238156 
+-0.678586 0.285852 -0.00238156 
 -0.674896 0.283945 -0.00267011 
 -0.66511 0.290546 -0.0124884 
 -0.666243 0.288283 -0.0131231 
@@ -5977,7 +5978,7 @@ end_header
 -0.67267 0.296835 -0.00469983 
 -0.691399 0.292198 -0.00554265 
 -0.689669 0.29484 -0.00987314 
--0.690183 0.292063 -0.00241774 
+-0.690183 0.292062 -0.00241774 
 -0.650907 0.284321 -0.00153023 
 -0.650662 0.282645 -0.0159993 
 -0.689281 0.294757 0.00101519 
@@ -6077,7 +6078,7 @@ end_header
 -0.637382 0.277866 -0.030671 
 -0.639351 0.282211 -0.0291914 
 -0.638465 0.283411 -0.0319677 
--0.639748 0.2832 -0.0245329 
+-0.639749 0.2832 -0.0245329 
 -0.641107 0.299238 -0.0332335 
 -0.639811 0.301036 -0.0287516 
 -0.627156 0.291414 -0.0301469 
@@ -6162,12 +6163,12 @@ end_header
 -0.674357 0.314178 -0.0324754 
 -0.677376 0.318809 -0.0326587 
 -0.676995 0.320178 -0.033131 
--0.678099 0.317963 -0.0315588 
+-0.6781 0.317963 -0.0315588 
 -0.676833 0.317492 -0.0324702 
 -0.677426 0.317263 -0.0317346 
 -0.626332 0.287568 -0.0345323 
 -0.647823 0.193513 0.013781 
--0.648396 0.197091 0.0103132 
+-0.648396 0.197092 0.0103132 
 -0.648011 0.189663 -0.000879365 
 -0.649522 0.194961 0.00122888 
 -0.647033 0.183258 0.00107936 
@@ -6207,7 +6208,7 @@ end_header
 -0.682317 0.179604 0.0110513 
 -0.67019 0.181244 0.00431299 
 -0.677033 0.181898 0.00659348 
--0.680801 0.183142 0.00930666 
+-0.6808 0.183142 0.00930666 
 -0.670015 0.185862 0.00351821 
 -0.676388 0.185368 0.00656798 
 -0.657756 0.187643 -0.000558959 
@@ -6227,12 +6228,12 @@ end_header
 -0.657654 0.193333 0.00174055 
 -0.669229 0.19104 0.00517269 
 -0.659704 0.196561 0.00647249 
--0.661873 0.195815 0.0123812 
+-0.661874 0.195815 0.0123812 
 -0.672076 0.193237 0.013752 
 -0.670605 0.193469 0.00914615 
 -0.67849 0.190647 0.0140387 
 -0.677051 0.190392 0.0109257 
--0.676446 0.188312 0.00807114 
+-0.676445 0.188312 0.00807114 
 -0.681708 0.187891 0.0135005 
 -0.683504 0.186726 0.0170348 
 -0.682445 0.18483 0.0117787 
@@ -6245,7 +6246,7 @@ end_header
 -0.604362 0.264193 -0.0454963 
 -0.617388 0.25842 -0.0364321 
 -0.625912 0.264691 -0.0337285 
--0.634361 0.270047 -0.0304386 
+-0.63436 0.270047 -0.0304386 
 -0.61224 0.250042 -0.0339878 
 -0.0226559 0.423039 0.348495 
 -0.022318 0.422529 0.352095 
@@ -6316,7 +6317,7 @@ end_header
 -0.0537034 0.0474005 0.108308 
 -0.0475804 0.055973 0.135475 
 -0.0530578 0.0833168 0.0281627 
--0.0257704 0.0284342 0.16546 
+-0.0257704 0.0284341 0.16546 
 0.115599 0.157416 0.0134436 
 0.138165 0.14593 0.0278065 
 0.000575597 0.11154 -0.0161211 
@@ -6368,7 +6369,7 @@ end_header
 -0.169297 0.248996 -0.00530915 
 -0.155999 0.241491 -0.00273605 
 -0.140459 0.237147 0.00578516 
--0.0921603 0.237914 0.040163 
+-0.0921604 0.237914 0.040163 
 -0.0814309 0.243412 0.0455016 
 -0.0489805 0.25401 0.0421318 
 -0.0368349 0.253215 0.0365487 
@@ -6413,7 +6414,7 @@ end_header
 -0.0691493 0.272785 0.049847 
 -0.0833803 0.268311 0.0454667 
 -0.0927055 0.280849 0.0481375 
--0.0778187 0.286429 0.0527378 
+-0.0778188 0.286429 0.0527378 
 -0.0130148 0.301169 0.0570565 
 0.0114852 0.303882 0.0576067 
 0.132098 0.254659 0.0871298 
@@ -6439,7 +6440,7 @@ end_header
 0.0879263 0.2847 0.0650977 
 0.095276 0.262422 0.0543179 
 0.0413814 0.350474 0.166054 
-0.065178 0.347652 0.166981 
+0.0651781 0.347652 0.166981 
 -0.135613 0.24959 0.0161263 
 -0.12984 0.245706 0.0170101 
 -0.13449 0.24262 0.0125659 
@@ -6462,7 +6463,7 @@ end_header
 -0.129054 0.255002 0.025147 
 -0.132606 0.261162 0.0292611 
 -0.118495 0.262982 0.0350468 
--0.103718 0.267225 0.0400553 
+-0.103718 0.267226 0.0400553 
 0.139864 0.237309 0.0775886 
 0.119004 0.25173 0.064531 
 -0.041298 0.289403 0.0543657 
@@ -6503,7 +6504,7 @@ end_header
 -0.0144657 0.302183 0.326938 
 -0.0216517 0.335012 0.357928 
 -0.0203795 0.339827 0.359259 
-0.0362721 0.246953 0.234861 
+0.036272 0.246953 0.234861 
 0.0396797 0.257798 0.238808 
 -0.00800399 0.114166 0.203649 
 -0.00340606 0.145652 0.211194 
@@ -6559,7 +6560,7 @@ end_header
 -0.109012 -0.213066 -0.0603277 
 -0.0603477 -0.22092 -0.0980558 
 -0.0317907 -0.212764 -0.10087 
--0.054834 -0.296034 0.0384296 
+-0.054834 -0.296034 0.0384297 
 -0.0346882 -0.306088 0.0386474 
 -0.120266 -0.227917 -0.0398033 
 -0.118625 -0.269798 0.00650389 
@@ -6571,7 +6572,7 @@ end_header
 0.0998309 -0.24432 -0.0552934 
 -0.0683138 -0.25971 0.0782451 
 -0.0692074 -0.252076 0.0867905 
--0.0653683 -0.258196 0.0938413 
+-0.0653683 -0.258197 0.0938413 
 -0.0628159 -0.272153 0.0872488 
 -0.0808456 -0.122455 0.044189 
 -0.0711269 -0.13076 0.0634191 
@@ -6658,7 +6659,7 @@ end_header
 -0.18048 -1.10542 0.0748885 
 -0.175507 -1.11476 0.079085 
 -0.188928 -1.11527 0.0856826 
--0.185402 -1.12328 0.0818161 
+-0.185401 -1.12328 0.081816 
 -0.173517 -1.12106 0.0752642 
 -0.179113 -1.12298 0.0696686 
 -0.189833 -1.12566 0.0765017 
@@ -6674,7 +6675,7 @@ end_header
 -0.1957 -1.13196 0.0622761 
 -0.193375 -1.12331 0.0496314 
 -0.200229 -1.1273 0.0578447 
--0.18935 -1.12691 0.0296172 
+-0.18935 -1.1269 0.0296172 
 -0.194953 -1.13145 0.0385383 
 -0.195595 -1.12503 0.0391401 
 -0.189085 -1.11955 0.0291131 
@@ -6719,13 +6720,13 @@ end_header
 -0.198895 -1.12782 0.0442795 
 -0.182683 -1.12495 0.0218758 
 -0.172177 -1.1325 0.0232759 
--0.19834 -1.12655 0.0475388 
+-0.19834 -1.12655 0.0475389 
 -0.17786 -1.11495 0.0150616 
 -0.190778 -1.12841 0.0224309 
 -0.169155 -1.12745 -0.00171771 
 -0.168105 -1.13405 0.00779368 
 -0.190784 -1.12743 0.0255007 
--0.175205 -1.11924 0.000756098 
+-0.175205 -1.11924 0.000756097 
 -0.0762263 -0.958545 -0.0379804 
 -0.0770327 -0.984168 -0.0305321 
 -0.0879876 -0.965506 -0.0412564 
@@ -6809,10 +6810,10 @@ end_header
 -0.115545 -1.05435 -0.0198111 
 -0.113692 -1.04946 -0.000515306 
 -0.0953603 -1.0473 0.0111446 
--0.0823868 -1.06835 0.024196 
+-0.0823869 -1.06835 0.024196 
 -0.0767448 -1.08535 0.0215557 
 -0.0587908 -1.08353 0.00734036 
--0.0591493 -1.06649 0.00920421 
+-0.0591493 -1.06649 0.00920422 
 -0.09818 -1.10008 0.0374499 
 -0.0785168 -1.09622 0.0167741 
 -0.0603589 -1.09487 0.00184175 
@@ -6827,7 +6828,7 @@ end_header
 -0.0455993 -1.04883 -0.00888741 
 -0.0653324 -1.04862 0.00367597 
 -0.118301 -1.12528 -0.029176 
--0.0966904 -1.12007 -0.0414192 
+-0.0966903 -1.12007 -0.0414192 
 -0.0724541 -1.11561 -0.0503457 
 -0.0484992 -1.11352 -0.060694 
 -0.0275271 -1.11481 -0.0670171 
@@ -6852,7 +6853,7 @@ end_header
 -0.0894734 -1.1192 -0.0202457 
 -0.0856791 -1.11412 -0.00590784 
 -0.0821386 -1.1058 0.00776949 
--0.0699625 -1.11887 -0.0413806 
+-0.0699624 -1.11887 -0.0413806 
 -0.068404 -1.11718 -0.0315999 
 -0.061872 -1.11185 -0.0193875 
 -0.0627364 -1.10544 -0.007105 
diff --git a/embedded_deformation/CMakeLists.txt b/embedded_deformation/CMakeLists.txt
index 6b552ed..5ca1155 100644
--- a/embedded_deformation/CMakeLists.txt
+++ b/embedded_deformation/CMakeLists.txt
@@ -2,33 +2,55 @@ cmake_minimum_required(VERSION 3.1)
 
 project(embedded_deformation)
 
-SET(INCLUDE_ROOT "${CMAKE_CURRENT_SOURCE_DIR}/include/")
-SET(SRC_ROOT "${CMAKE_CURRENT_SOURCE_DIR}/src/")
+set(CMAKE_CXX_STANDARD 14)          
+set(CMAKE_CXX_STANDARD_REQUIRED ON)  
+set(CMAKE_CXX_EXTENSIONS OFF) 
+
+SET(INCLUDE_ROOT "${CMAKE_CURRENT_SOURCE_DIR}/include")
+SET(SRC_ROOT "${CMAKE_CURRENT_SOURCE_DIR}/src")
 
 find_package(Eigen3 REQUIRED)
 find_package(yaml-cpp REQUIRED)
 find_package(Ceres REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(CUDA REQUIRED)
+
+include_directories(${OpenCV_INCLUDE_DIRS})
+include_directories(${CUDA_INCLUDE_DIRS})
+include_directories(${CMAKE_SOURCE_DIR}/polyscope/include)
+
+file(GLOB containers
+    ${INCLUDE_ROOT}/containers/*.hpp
+    ${INCLUDE_ROOT}/containers/*.cuh
+    ${INCLUDE_ROOT}/containers/*.h
+)
+cuda_add_library(containers ${containers})
 
-set(SRCS
+file(GLOB SRCS
     # embedded_deformation
-    ${SRC_ROOT}/embedded_deformation/embedDeform.cpp
-    ${SRC_ROOT}/embedded_deformation/greedySearch.cpp
+    ${SRC_ROOT}/embedded_deformation/*.cpp
+    ${SRC_ROOT}/imageProcessor/*.cu
+    ${SRC_ROOT}/imageProcessor/*.cpp
+    ${SRC_ROOT}/math/*.cpp
+    ${SRC_ROOT}/rigidSolver/*.cpp
+    ${SRC_ROOT}/rigidSolver/*.cu
 )
 
-set(HEADERS
+file(GLOB HEADERS
     # embedded_deformation
-    ${INCLUDE_ROOT}/embedded_deformation/costFunction.hpp
-    ${INCLUDE_ROOT}/embedded_deformation/downsampling.hpp
-    ${INCLUDE_ROOT}/embedded_deformation/embedDeform.hpp
-    ${INCLUDE_ROOT}/embedded_deformation/farther_sampling.hpp
-    ${INCLUDE_ROOT}/embedded_deformation/getMinMax.hpp
-    ${INCLUDE_ROOT}/embedded_deformation/greedySearch.hpp
-    ${INCLUDE_ROOT}/embedded_deformation/nanoflann.hpp
-    ${INCLUDE_ROOT}/embedded_deformation/nanoflannWrapper.hpp
-    ${INCLUDE_ROOT}/embedded_deformation/options.hpp
+    ${INCLUDE_ROOT}/embedded_deformation/*.hpp
+    ${INCLUDE_ROOT}/imageProcessor/*.h
+    ${INCLUDE_ROOT}/math/*.h
+    ${INCLUDE_ROOT}/math/*.hpp
+    ${INCLUDE_ROOT}/containers/*.h
+    ${INCLUDE_ROOT}/containers/*.hpp
+    ${INCLUDE_ROOT}/containers/*.cpp
+    ${INCLUDE_ROOT}/containers/*.cuh
+    ${INCLUDE_ROOT}/rigidSolver/*.h
+    
 )
 
-add_library(embedded_deformation ${SRCS} ${HEADERS})
+cuda_add_library(embedded_deformation ${SRCS} ${HEADERS})
 
 target_include_directories(embedded_deformation PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include")
 target_include_directories(embedded_deformation PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/include/embedded_deformation")
@@ -39,4 +61,4 @@ target_include_directories(embedded_deformation PRIVATE ${LIB_GRAPH_CPP_INCLUDE_
 
 target_link_libraries(embedded_deformation libGraphCpp)
 target_link_libraries(embedded_deformation ${CERES_LIBRARIES} )
-target_link_libraries(embedded_deformation ${YAML_CPP_LIBRARIES} )
+target_link_libraries(embedded_deformation ${YAML_CPP_LIBRARIES} ${OpenCV_LIBRARIES} ${CUDA_LIBRARIES} containers)
diff --git a/embedded_deformation/include/containers/ArraySlice.h b/embedded_deformation/include/containers/ArraySlice.h
new file mode 100644
index 0000000..75ea2f6
--- /dev/null
+++ b/embedded_deformation/include/containers/ArraySlice.h
@@ -0,0 +1,126 @@
+#pragma once
+#include "imageProcessor/logging.h"
+#include "containers/ArrayView.h"
+#include <cuda_runtime.h>
+#include "containers/device_array.hpp"
+
+using namespace CUDA;
+
+//For friend
+template<typename T>
+class DeviceSliceBufferArray;
+
+//The array slice class, provides non-owned
+//read-WRITE access to some array
+template<typename T>
+class DeviceArraySlice {
+private:
+	T* m_array;
+	size_t m_array_size;
+public:
+	//Default copy/assign/move/destruct
+	__host__ __device__ DeviceArraySlice() : m_array(nullptr), m_array_size(0) {}
+	__host__ __device__ DeviceArraySlice(T* dev_arr, size_t size) : m_array(dev_arr), m_array_size(size) {}
+	__host__ __device__ DeviceArraySlice(T* arr, size_t start, size_t end) {
+		m_array_size = end - start;
+		m_array = arr + start;
+	}
+	explicit __host__ DeviceArraySlice(const DeviceArray<T>& arr) : m_array((T*)arr.ptr()), m_array_size(arr.size()) {}
+
+	//Simple interface
+	__host__ __device__ __forceinline__ size_t Size() const { return m_array_size; }
+	__host__ __device__ __forceinline__ size_t ByteSize() const { return m_array_size * sizeof(T); }
+	__host__ __device__ __forceinline__ const T* RawPtr() const { return m_array; }
+	__host__ __device__ __forceinline__ T* RawPtr() { return m_array; }
+	__host__ __device__ DeviceArrayView<T> ArrayView() const { return DeviceArrayView<T>(m_array, m_array_size); }
+
+	//Implicit convertor
+	operator T*() { return m_array; }
+	operator const T*() const { return m_array; }
+
+	//The accessing method can only be processed on device
+	__device__ __forceinline__ const T& operator[](size_t index) const { return m_array[index]; }
+	__device__ __forceinline__ T& operator[](size_t index) { return m_array[index]; }
+
+	//Download to std::vector
+	void SyncToHost(std::vector<T>& h_vec, cudaStream_t stream = 0) const {
+		h_vec.resize(m_array_size);
+		cudaSafeCall(cudaMemcpyAsync(
+			h_vec.data(),
+			m_array,
+			ByteSize(),
+			cudaMemcpyDeviceToHost,
+			stream
+		));
+	}
+	
+	//Upload from host
+	void SyncFromHost(const std::vector<T>& h_vec, cudaStream_t stream = 0) {
+		SURFELWARP_CHECK_EQ(h_vec.size(), m_array_size);
+		cudaSafeCall(cudaMemcpyAsync(
+			m_array,
+			h_vec.data(),
+			sizeof(T) * h_vec.size(),
+			cudaMemcpyHostToDevice,
+			stream
+		));
+	}
+	
+	//array_size modified by buffer array
+	friend class DeviceSliceBufferArray<T>;
+};
+
+template<typename T>
+class DeviceSliceBufferArray {
+public:
+	//Default copy/assign/move/delete
+	__host__ __device__ DeviceSliceBufferArray(): m_buffer(), m_array() {}
+	__host__ __device__ DeviceSliceBufferArray(T* buffer, size_t capacity)
+	: m_buffer(buffer, capacity), m_array(buffer, 0) {}
+	
+	//The contructor on host will check the size
+	__host__ DeviceSliceBufferArray(T* buffer, size_t capacity, size_t array_size)
+		: m_buffer(buffer, capacity), m_array(buffer, array_size
+	) {
+		//Check the size
+		if(array_size > capacity) {
+			LOG(FATAL) << "The provided buffer is not enough";
+		}
+	}
+	__host__ DeviceSliceBufferArray(const DeviceArray<T>& arr)
+		: m_buffer(arr), m_array((T*)arr.ptr(), 0) {}
+	
+	//Change the size of array
+	__host__ void ResizeArrayOrException(size_t size) {
+		if(size > m_buffer.Size()) {
+			//Kill it
+			LOG(FATAL) << "The provided buffer is not enough";
+		}
+
+		//Safe to resize the array
+		m_array.m_array_size = size;
+	}
+
+	//Interface: no need to make it on device
+	__host__ __forceinline__ size_t Capacity() const { return m_buffer.Size(); }
+	__host__ __forceinline__ size_t BufferSize() const { return m_buffer.Size(); }
+	__host__ __forceinline__ size_t ArraySize() const { return m_array.Size(); }
+	__host__ __forceinline__ const T* Ptr() const { return m_buffer.RawPtr(); }
+	__host__ __forceinline__ T* Ptr() { return m_buffer.RawPtr(); }
+	__host__ __forceinline__ DeviceArraySlice<T> ArraySlice() const { return m_array; }
+	__host__ __forceinline__ DeviceArrayView<T> ArrayReadOnly() const { return DeviceArrayView<T>(Ptr(), ArraySize()); }
+	__host__ __forceinline__ DeviceArrayView<T> ArrayView() const { return DeviceArrayView<T>(Ptr(), ArraySize()); }
+	
+	//To make new slice from this one
+	__host__ DeviceSliceBufferArray<T> BufferArraySlice(size_t start, size_t end) const {
+		if(start > end || end > m_buffer.Size()) {
+			LOG(FATAL) << "Incorrect slice size";
+		}
+		
+		//Return inside slice
+		return DeviceSliceBufferArray<T>((T*)m_buffer.RawPtr() + start, end - start);
+	}
+private:
+	DeviceArraySlice<T> m_buffer;
+	DeviceArraySlice<T> m_array;
+};
diff --git a/embedded_deformation/include/containers/ArrayView.h b/embedded_deformation/include/containers/ArrayView.h
new file mode 100644
index 0000000..1a64cfe
--- /dev/null
+++ b/embedded_deformation/include/containers/ArrayView.h
@@ -0,0 +1,96 @@
+#pragma once
+
+#include "containers/device_array.hpp"
+
+using namespace CUDA;
+	
+//The array view class, as its name, maintains
+//a non-allocate, read-only access to some array.
+template<typename T>
+class DeviceArrayView {
+private:
+	const T* m_array;
+	size_t m_array_size;
+
+public:
+	//Default copy/assign/move/destruct
+	__host__ __device__ DeviceArrayView() : m_array(nullptr), m_array_size(0) {}
+	__host__ __device__ DeviceArrayView(const T* arr, size_t start, size_t end) {
+		m_array_size = end - start;
+		m_array = arr + start;
+	}
+	__host__ __device__ DeviceArrayView(const T* arr, size_t size) : m_array(arr), m_array_size(size) {}
+	explicit __host__ DeviceArrayView(const DeviceArray<T>& arr) : m_array(arr.ptr()), m_array_size(arr.size()) {}
+
+	//Assign operator
+	// =重载
+	__host__ DeviceArrayView<T>& operator=(const DeviceArray<T>& arr) {
+		m_array = arr.ptr();
+		m_array_size = arr.size();
+		return *this;
+	}
+	
+	
+	//Simple interface
+	__host__ __device__ size_t Size() const { return m_array_size; }
+	__host__ __device__ size_t ByteSize() const { return m_array_size * sizeof(T); }
+	__host__ __device__ const T* RawPtr() const { return m_array; }
+	__host__ __device__ operator const T*() const { return m_array; }
+	
+	//The accessing method can only be processed on device
+	__device__ const T& operator[](size_t index) const { return m_array[index]; }
+
+	//Download to std::vector, typically for debugging
+	__host__ void Download(std::vector<T>& h_vec) const {
+		h_vec.resize(Size());
+		cudaSafeCall(cudaMemcpy(h_vec.data(), m_array, Size() * sizeof(T), cudaMemcpyDeviceToHost));
+	}
+};
+
+//The two dimension case of array view
+template<typename T>
+class DeviceArrayView2D {
+private:
+	unsigned short m_rows, m_cols;
+	unsigned m_byte_step; // Note that the step is always in byte
+	const T* m_ptr;
+	
+public:
+	__host__ __device__ DeviceArrayView2D() : m_rows(0), m_cols(0), m_byte_step(0), m_ptr(nullptr) {}
+	__host__ DeviceArrayView2D(const DeviceArray2D<T>& array2D)
+		: m_rows(array2D.rows()), m_cols(array2D.cols()),
+			m_byte_step(array2D.step()), m_ptr(array2D.ptr())
+	{}
+	
+	//The interface
+	__host__ __device__ __forceinline__ unsigned short Rows() const { return m_rows; }
+	__host__ __device__ __forceinline__ unsigned short Cols() const { return m_cols; }
+	__host__ __device__ __forceinline__ unsigned ByteStep() const { return m_byte_step; }
+	__host__ __device__ __forceinline__ const T* RawPtr() const { return m_ptr; }
+	__host__ __device__ __forceinline__ const T* RawPtr(int row) const {
+		return ((const T*)((const char*)(m_ptr) + row * m_byte_step));
+	}
+	__host__ __device__ __forceinline__ const T& operator()(int row, int col) const {
+		return RawPtr(row)[col];
+	}
+};
+
+template<typename T>
+struct PtrStepView {
+private:
+	unsigned m_byte_step; // Note that the step is always in byte
+	const T* m_ptr;
+public:
+	__host__ __device__ PtrStepView() : m_byte_step(0), m_ptr(nullptr) {}
+	__host__ __device__ PtrStepView(DeviceArrayView2D<T> arrayView2D)
+		: m_byte_step(arrayView2D.ByteStep()), m_ptr(arrayView2D.RawPtr()) {}
+	
+	__host__ __device__ __forceinline__ const T* RawPtr() const { return m_ptr; }
+	__host__ __device__ __forceinline__ const T* RawPtr(int row) const {
+		return ((const T*)((const char*)(m_ptr) + row * m_byte_step));
+	}
+	__host__ __device__ __forceinline__ const T& operator()(int row, int col) const {
+		return RawPtr(row)[col];
+	}
+};
+
diff --git a/embedded_deformation/include/containers/DeviceBufferArray.h b/embedded_deformation/include/containers/DeviceBufferArray.h
new file mode 100644
index 0000000..ed7d1cd
--- /dev/null
+++ b/embedded_deformation/include/containers/DeviceBufferArray.h
@@ -0,0 +1,117 @@
+#pragma once
+#include "imageProcessor/logging.h"
+#include "containers/ArrayView.h"
+#include "containers/ArraySlice.h"
+#include "containers/device_array.hpp"
+
+using namespace CUDA;
+
+template<typename T>
+class DeviceBufferArray {
+public:
+	/**
+	 * @brief 无参构造函数
+	 */
+	explicit DeviceBufferArray() : m_buffer(nullptr, 0), m_array(nullptr, 0) {}
+
+	/**
+	 * @brief 有参构造函数，指定缓存大小
+	 */
+	explicit DeviceBufferArray(size_t capacity) {
+		AllocateBuffer(capacity);
+		m_array = DeviceArray<T>(m_buffer.ptr(), 0);
+	}
+
+	/**
+	 * @brief 析构函数
+	 */
+	~DeviceBufferArray() = default;
+
+	//访问与获取的方法
+	CUDA::DeviceArray<T> Array() const { return m_array; }
+	DeviceArrayView<T> ArrayView() const { return DeviceArrayView<T>(m_array.ptr(), m_array.size()); }
+	DeviceArrayView<T> ArrayReadOnly() const { return DeviceArrayView<T>(m_array.ptr(), m_array.size()); }
+	DeviceArraySlice<T> ArraySlice() { return DeviceArraySlice<T>(m_array.ptr(), m_array.size()); }
+	CUDA::DeviceArray<T> Buffer() const { return m_buffer; }
+	
+	/**
+	 * @brief 与传入的数据交换
+	 */
+	void swap(DeviceBufferArray<float>& other) {
+		m_buffer.swap(other.m_buffer);
+		m_array.swap(other.m_array);
+	}
+	
+	//Cast to raw pointer
+	const T* Ptr() const { return m_buffer.ptr(); }
+	T* Ptr() { return m_buffer.ptr(); }
+	operator T*() { return m_buffer.ptr(); }
+	operator const T*() const { return m_buffer.ptr(); }
+	
+	//查询大小
+	size_t Capacity() const { return m_buffer.size(); }
+	size_t BufferSize() const { return m_buffer.size(); }
+	size_t ArraySize() const { return m_array.size(); }
+
+	/**
+	 * @brief 分配缓存
+	 * 
+	 */
+	void AllocateBuffer(size_t capacity) {
+		if(m_buffer.size() > capacity) return;
+		m_buffer.create(capacity);
+		m_array = DeviceArray<T>(m_buffer.ptr(), 0);
+	}
+	/**
+	 * @brief 释放缓存
+	 * 
+	 */
+	void ReleaseBuffer() {
+		if(m_buffer.size() > 0) m_buffer.release();
+	}
+	
+	/**
+	 * @brief 修改数组大小
+	 */
+	bool ResizeArray(size_t size, bool allocate = false) {
+		if(size <= m_buffer.size()) {
+			m_array = DeviceArray<T>(m_buffer.ptr(), size);
+			return true;
+		} 
+		else if(allocate) {
+			const size_t prev_size = m_array.size();
+
+			//需要先拷贝以前的数据
+			DeviceArray<T> old_buffer = m_buffer;
+			//分配新的缓存 是要求size的1.5倍
+			m_buffer.create(static_cast<size_t>(size * 1.5));
+			if(prev_size > 0) {
+				cudaSafeCall(cudaMemcpy(m_buffer.ptr(), old_buffer.ptr(), sizeof(T) * prev_size, cudaMemcpyDeviceToDevice));
+				old_buffer.release();
+			}
+
+			//修改数组大小
+			m_array = DeviceArray<T>(m_buffer.ptr(), size);
+			return true;
+		} 
+		else {
+			return false;
+		}
+	}
+
+	/**
+	 * @brief 修改数组大小，如果不够则抛出异常
+	 */
+	void ResizeArrayOrException(size_t size) {
+		if (size > m_buffer.size()) {
+			LOG(FATAL) << "The pre-allocated buffer is not enough";
+		}
+
+		//Change the size of array
+		m_array = DeviceArray<T>(m_buffer.ptr(), size);
+	}
+private:
+	DeviceArray<T> m_buffer;
+	DeviceArray<T> m_array;
+};
+
diff --git a/embedded_deformation/include/containers/SynchronizeArray.h b/embedded_deformation/include/containers/SynchronizeArray.h
new file mode 100644
index 0000000..8aebc24
--- /dev/null
+++ b/embedded_deformation/include/containers/SynchronizeArray.h
@@ -0,0 +1,100 @@
+#pragma once
+#include "containers/ArrayView.h"
+#include "containers/DeviceBufferArray.h"
+#include "imageProcessor/safe_call.hpp"
+
+/**
+ * \brief The array with synchronize functionalities. Note that the content of the
+ *        array is not guarantee to be synchronized and need explict sync. However, 
+ *        the array size of host and device array are always the same.
+ * \tparam T 
+ */
+template<typename T>
+class SynchronizeArray {
+public:
+	explicit SynchronizeArray() : m_host_array(), m_device_array() {}
+	explicit SynchronizeArray(size_t capacity) {
+		AllocateBuffer(capacity);
+	}
+	~SynchronizeArray() = default;
+	
+	//The accessing interface
+	size_t Capacity() const { return m_device_array.Capacity(); }
+	size_t DeviceArraySize() const { return m_device_array.ArraySize(); }
+	size_t HostArraySize() const { return m_host_array.size(); }
+	
+	DeviceArrayView<T> DeviceArrayReadOnly() const { return DeviceArrayView<T>(m_device_array.Array()); }
+	CUDA::DeviceArray<T> DeviceArray() const { return m_device_array.Array(); }
+	DeviceArraySlice<T> DeviceArrayReadWrite() { return m_device_array.ArraySlice(); }
+	
+	std::vector<T>& HostArray() { return m_host_array; }
+	const std::vector<T>& HostArray() const { return m_host_array; }
+
+	//Access the raw pointer
+	const T* DevicePtr() const { return m_device_array.Ptr(); }
+	T* DevicePtr() { return m_device_array.Ptr(); }
+	
+	//The (possible) allocate interface
+	void AllocateBuffer(size_t capacity) {
+		m_host_array.reserve(capacity);
+		m_device_array.AllocateBuffer(capacity);
+	}
+
+	//the DeviceBufferArray has implement resize with copy
+	bool ResizeArray(size_t size, bool allocate = false) {
+		if(m_device_array.ResizeArray(size, allocate) == true) {
+			m_host_array.resize(size);
+			return true;
+		}
+		
+		//The device array can not resize success
+		//The host and device are in the same size
+		return false;
+	}
+	void ResizeArrayOrException(size_t size) {
+		m_device_array.ResizeArrayOrException(size);
+		m_host_array.resize(size);
+	}
+	
+	//Clear the array of both host and device array
+	//But DO NOT TOUCH the allocated buffer
+	void ClearArray() {
+		ResizeArray(0);
+	}
+	
+	//The sync interface
+	void SynchronizeToDevice(cudaStream_t stream = 0) {
+		//Update the size
+		m_device_array.ResizeArrayOrException(m_host_array.size());
+		
+		//Actual sync
+		cudaSafeCall(cudaMemcpyAsync(
+			m_device_array.Ptr(),
+			m_host_array.data(), 
+			sizeof(T) * m_host_array.size(),
+			cudaMemcpyHostToDevice, stream
+		));
+	}
+	void SynchronizeToHost(cudaStream_t stream = 0, bool sync = true) {
+		//Resize host array
+		m_host_array.resize(m_device_array.ArraySize());
+		
+		//Actual sync
+		cudaSafeCall(cudaMemcpyAsync(
+			m_host_array.data(), 
+			m_device_array.Ptr(),
+			sizeof(T) * m_host_array.size(), 
+			cudaMemcpyDeviceToHost, stream
+		));
+		
+		if(sync) {
+			//Before using on host, must call stream sync
+			//But the call might be delayed
+			cudaSafeCall(cudaStreamSynchronize(stream));
+		}
+	}
+
+private:
+	std::vector<T> m_host_array;
+	DeviceBufferArray<T> m_device_array;
+};
diff --git a/embedded_deformation/include/containers/convenience.cuh b/embedded_deformation/include/containers/convenience.cuh
new file mode 100644
index 0000000..dbbaf22
--- /dev/null
+++ b/embedded_deformation/include/containers/convenience.cuh
@@ -0,0 +1,40 @@
+#ifndef CONVENIENCE_CUH_
+#define CUDA_CONVENIENCE_CUH_
+
+#include <cuda_runtime_api.h>
+#include <cstdlib>
+#include <iostream>
+#include <stdio.h>
+
+#define STR2(x) #x
+#define STRINGIFY(x) STR2(x)
+
+#define FILE_LINE __FILE__ ":" STRINGIFY(__LINE__)
+
+static inline int getGridDim(int x, int y)
+{
+    return (x + y - 1) / y;
+}
+
+/*static inline void cudaCheckError(std::string fileline){
+    cudaSafeCall(cudaGetLastError(), fileline);
+}*/
+
+#define cudaSafeCall(err) __cudaSafeCall(err, __FILE__, __LINE__)
+#define cudaCheckError() __cudaSafeCall(cudaGetLastError(), __FILE__, __LINE__)
+inline void __cudaSafeCall( cudaError err, const char *file, const int line )
+{
+    if( cudaSuccess != err) {
+                printf("%s(%i) : cudaSafeCall() Runtime API error : %s.\n",
+                file, line, cudaGetErrorString(err) );
+        exit(-1);
+    }
+}
+
+static inline void cudaPrintMemory(){
+    size_t free, total;
+    cudaMemGetInfo(&free,&total);
+    std::cout << "Cuda memory: " << float(free)/(1024.0*1024.0) << " / " << float(total)/(1024.0*1024.0) << "MB" << std::endl;
+}
+
+#endif /* CONVENIENCE_CUH_ */
diff --git a/embedded_deformation/include/containers/device_array.hpp b/embedded_deformation/include/containers/device_array.hpp
new file mode 100644
index 0000000..169e327
--- /dev/null
+++ b/embedded_deformation/include/containers/device_array.hpp
@@ -0,0 +1,394 @@
+#ifndef DEVICE_ARRAY_HPP_
+#define DEVICE_ARRAY_HPP_
+
+#include "containers/device_memory.hpp"
+#include <vector>
+
+namespace CUDA{
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b DeviceArray class
+  *
+  * \note Typed container for GPU memory with reference counting.
+  *
+  * \author Anatoly Baksheev
+  */
+template <class T>
+class DeviceArray : public DeviceMemory {
+public:
+  /** \brief Element type. */
+  typedef T type;
+
+  /** \brief Element size. */
+  enum { elem_size = sizeof(T) };
+
+  /** \brief Empty constructor. */
+  DeviceArray();
+
+  /** \brief Allocates internal buffer in GPU memory
+    * \param size_t: number of elements to allocate
+    * */
+  DeviceArray(size_t size);
+
+  /** \brief Initializes with user allocated buffer. Reference counting is disabled in this case.
+    * \param ptr: pointer to buffer
+    * \param size: elemens number
+    * */
+  DeviceArray(T* ptr, size_t size);
+
+  /** \brief Copy constructor. Just increments reference counter. */
+  DeviceArray(const DeviceArray& other);
+
+  /** \brief Assigment operator. Just increments reference counter. */
+  DeviceArray& operator=(const DeviceArray& other);
+
+  /** \brief Allocates internal buffer in GPU memory. If internal buffer was created before the function recreates it with
+   * new size. If new and old sizes are equal it does nothing.
+    * \param size: elemens number
+    * */
+  void create(size_t size);
+
+  /** \brief Decrements reference counter and releases internal buffer if needed. */
+  void release();
+
+  /** \brief Performs data copying. If destination size differs it will be reallocated.
+    * \param other_arg: destination container
+    * */
+  void copyTo(DeviceArray& other) const;
+
+  /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to ensure that intenal buffer size is
+   * enough.
+    * \param host_ptr_arg: pointer to buffer to upload
+    * \param size: elemens number
+    * */
+  void upload(const T* host_ptr, size_t size);
+
+  /** \brief Downloads data from internal buffer to CPU memory
+    * \param host_ptr_arg: pointer to buffer to download
+    * */
+  void download(T* host_ptr) const;
+
+  /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to ensure that intenal buffer size is
+   * enough.
+    * \param data: host vector to upload from
+    * */
+  template <class A>
+  void upload(const std::vector<T, A>& data);
+
+  /** \brief Downloads data from internal buffer to CPU memory
+    * \param data:  host vector to download to
+    * */
+  template <typename A>
+  void download(std::vector<T, A>& data) const;
+
+  /** \brief Performs swap of data pointed with another device array.
+    * \param other: device array to swap with
+    * */
+  void swap(DeviceArray& other_arg);
+
+  /** \brief Returns pointer for internal buffer in GPU memory. */
+  T* ptr();
+
+  /** \brief Returns const pointer for internal buffer in GPU memory. */
+  const T* ptr() const;
+
+  // using DeviceMemory::ptr;
+
+  /** \brief Returns pointer for internal buffer in GPU memory. */
+  operator T*();
+
+  /** \brief Returns const pointer for internal buffer in GPU memory. */
+  operator const T*() const;
+
+  /** \brief Returns size in elements. */
+  size_t size() const;
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b DeviceArray2D class
+  *
+  * \note Typed container for pitched GPU memory with reference counting.
+  *
+  * \author Anatoly Baksheev
+  */
+template <class T>
+class DeviceArray2D : public DeviceMemory2D {
+public:
+  /** \brief Element type. */
+  typedef T type;
+
+  /** \brief Element size. */
+  enum { elem_size = sizeof(T) };
+
+  /** \brief Empty constructor. */
+  DeviceArray2D();
+
+  /** \brief Allocates internal buffer in GPU memory
+    * \param rows: number of rows to allocate
+    * \param cols: number of elements in each row
+    * */
+  DeviceArray2D(int rows, int cols);
+
+  /** \brief Initializes with user allocated buffer. Reference counting is disabled in this case.
+   * \param rows: number of rows
+   * \param cols: number of elements in each row
+   * \param data: pointer to buffer
+   * \param stepBytes: stride between two consecutive rows in bytes
+   * */
+  DeviceArray2D(int rows, int cols, void* data, size_t stepBytes);
+
+  /** \brief Copy constructor. Just increments reference counter. */
+  DeviceArray2D(const DeviceArray2D& other);
+
+  /** \brief Assigment operator. Just increments reference counter. */
+  DeviceArray2D& operator=(const DeviceArray2D& other);
+
+  /** \brief Allocates internal buffer in GPU memory. If internal buffer was created before the function recreates it with
+   * new size. If new and old sizes are equal it does nothing.
+     * \param rows: number of rows to allocate
+     * \param cols: number of elements in each row
+     * */
+  void create(int rows, int cols);
+
+  /** \brief Decrements reference counter and releases internal buffer if needed. */
+  void release();
+
+  /** \brief Performs data copying. If destination size differs it will be reallocated.
+    * \param other: destination container
+    * */
+  void copyTo(DeviceArray2D& other) const;
+
+  /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to ensure that intenal buffer size is
+   * enough.
+    * \param host_ptr: pointer to host buffer to upload
+    * \param host_step: stride between two consecutive rows in bytes for host buffer
+    * \param rows: number of rows to upload
+    * \param cols: number of elements in each row
+    * */
+  void upload(const void* host_ptr, size_t host_step, int rows, int cols);
+
+  /** \brief Downloads data from internal buffer to CPU memory. User is resposible for correct host buffer size.
+    * \param host_ptr: pointer to host buffer to download
+    * \param host_step: stride between two consecutive rows in bytes for host buffer
+    * */
+  void download(void* host_ptr, size_t host_step) const;
+
+  /** \brief Performs swap of data pointed with another device array.
+    * \param other: device array to swap with
+    * */
+  void swap(DeviceArray2D& other_arg);
+
+  /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to ensure that intenal buffer size is
+   * enough.
+    * \param data: host vector to upload from
+    * \param cols: stride in elements between  two consecutive rows in bytes for host buffer
+    * */
+  template <class A>
+  void upload(const std::vector<T, A>& data, int cols);
+
+  /** \brief Downloads data from internal buffer to CPU memory
+     * \param data: host vector to download to
+     * \param cols: Output stride in elements between two consecutive rows in bytes for host vector.
+     * */
+  template <class A>
+  void download(std::vector<T, A>& data, int& cols) const;
+
+  /** \brief Returns pointer to given row in internal buffer.
+    * \param y_arg: row index
+    * */
+  T* ptr(int y = 0);
+
+  /** \brief Returns const pointer to given row in internal buffer.
+    * \param y_arg: row index
+    * */
+  const T* ptr(int y = 0) const;
+
+  // using DeviceMemory2D::ptr;
+
+  /** \brief Returns pointer for internal buffer in GPU memory. */
+  operator T*();
+
+  /** \brief Returns const pointer for internal buffer in GPU memory. */
+  operator const T*() const;
+
+  /** \brief Returns number of elements in each row. */
+  int cols() const;
+
+  /** \brief Returns number of rows. */
+  int rows() const;
+
+  /** \brief Returns step in elements. */
+  size_t elem_step() const;
+};
+
+}
+
+
+/////////////////////  Inline implementations of DeviceArray ////////////////////////////////////////////
+
+template <class T>
+inline CUDA::DeviceArray<T>::DeviceArray() {}
+template <class T>
+inline CUDA::DeviceArray<T>::DeviceArray(size_t size) : DeviceMemory(size * elem_size) {}
+template <class T>
+inline CUDA::DeviceArray<T>::DeviceArray(T* ptr, size_t size) : DeviceMemory(ptr, size * elem_size) {}
+template <class T>
+inline CUDA::DeviceArray<T>::DeviceArray(const DeviceArray& other) : DeviceMemory(other) {}
+template <class T>
+inline CUDA::DeviceArray<T>& CUDA::DeviceArray<T>::operator=(const DeviceArray& other) {
+  DeviceMemory::operator=(other);
+  return *this;
+}
+
+template <class T>
+inline void CUDA::DeviceArray<T>::create(size_t size) {
+  DeviceMemory::create(size * elem_size);
+}
+template <class T>
+inline void CUDA::DeviceArray<T>::release() {
+  DeviceMemory::release();
+}
+
+template <class T>
+inline void CUDA::DeviceArray<T>::copyTo(DeviceArray& other) const {
+  DeviceMemory::copyTo(other);
+}
+template <class T>
+inline void CUDA::DeviceArray<T>::upload(const T* host_ptr, size_t size) {
+  DeviceMemory::upload(host_ptr, size * elem_size);
+}
+template <class T>
+inline void CUDA::DeviceArray<T>::download(T* host_ptr) const {
+  DeviceMemory::download(host_ptr);
+}
+
+template <class T>
+void CUDA::DeviceArray<T>::swap(DeviceArray& other_arg) {
+  DeviceMemory::swap(other_arg);
+}
+
+template <class T>
+inline CUDA::DeviceArray<T>::operator T*() {
+  return ptr();
+}
+template <class T>
+inline CUDA::DeviceArray<T>::operator const T*() const {
+  return ptr();
+}
+template <class T>
+inline size_t CUDA::DeviceArray<T>::size() const {
+  return sizeBytes() / elem_size;
+}
+
+template <class T>
+inline T* CUDA::DeviceArray<T>::ptr() {
+  return DeviceMemory::ptr<T>();
+}
+template <class T>
+inline const T* CUDA::DeviceArray<T>::ptr() const {
+  return DeviceMemory::ptr<T>();
+}
+
+template <class T>
+template <class A>
+inline void CUDA::DeviceArray<T>::upload(const std::vector<T, A>& data) {
+  upload(&data[0], data.size());
+}
+template <class T>
+template <class A>
+inline void CUDA::DeviceArray<T>::download(std::vector<T, A>& data) const {
+  data.resize(size());
+  if (!data.empty()) download(&data[0]);
+}
+
+/////////////////////  Inline implementations of DeviceArray2D ////////////////////////////////////////////
+
+template <class T>
+inline CUDA::DeviceArray2D<T>::DeviceArray2D() {}
+template <class T>
+inline CUDA::DeviceArray2D<T>::DeviceArray2D(int rows, int cols) : DeviceMemory2D(rows, cols * elem_size) {}
+template <class T>
+inline CUDA::DeviceArray2D<T>::DeviceArray2D(int rows, int cols, void* data, size_t stepBytes)
+    : DeviceMemory2D(rows, cols * elem_size, data, stepBytes) {}
+template <class T>
+inline CUDA::DeviceArray2D<T>::DeviceArray2D(const DeviceArray2D& other) : DeviceMemory2D(other) {}
+template <class T>
+inline CUDA::DeviceArray2D<T>& CUDA::DeviceArray2D<T>::operator=(const DeviceArray2D& other) {
+  DeviceMemory2D::operator=(other);
+  return *this;
+}
+
+template <class T>
+inline void CUDA::DeviceArray2D<T>::create(int rows, int cols) {
+  DeviceMemory2D::create(rows, cols * elem_size);
+}
+template <class T>
+inline void CUDA::DeviceArray2D<T>::release() {
+  DeviceMemory2D::release();
+}
+
+template <class T>
+inline void CUDA::DeviceArray2D<T>::copyTo(DeviceArray2D& other) const {
+  DeviceMemory2D::copyTo(other);
+}
+template <class T>
+inline void CUDA::DeviceArray2D<T>::upload(const void* host_ptr, size_t host_step, int rows, int cols) {
+  DeviceMemory2D::upload(host_ptr, host_step, rows, cols * elem_size);
+}
+template <class T>
+inline void CUDA::DeviceArray2D<T>::download(void* host_ptr, size_t host_step) const {
+  DeviceMemory2D::download(host_ptr, host_step);
+}
+
+template <class T>
+template <class A>
+inline void CUDA::DeviceArray2D<T>::upload(const std::vector<T, A>& data, int cols) {
+  upload(&data[0], cols * elem_size, data.size() / cols, cols);
+}
+
+template <class T>
+template <class A>
+inline void CUDA::DeviceArray2D<T>::download(std::vector<T, A>& data, int& elem_step) const {
+  elem_step = cols();
+  data.resize(cols() * rows());
+  if (!data.empty()) download(&data[0], colsBytes());
+}
+
+template <class T>
+void CUDA::DeviceArray2D<T>::swap(DeviceArray2D& other_arg) {
+  DeviceMemory2D::swap(other_arg);
+}
+
+template <class T>
+inline T* CUDA::DeviceArray2D<T>::ptr(int y) {
+  return DeviceMemory2D::ptr<T>(y);
+}
+template <class T>
+inline const T* CUDA::DeviceArray2D<T>::ptr(int y) const {
+  return DeviceMemory2D::ptr<T>(y);
+}
+
+template <class T>
+inline CUDA::DeviceArray2D<T>::operator T*() {
+  return ptr();
+}
+template <class T>
+inline CUDA::DeviceArray2D<T>::operator const T*() const {
+  return ptr();
+}
+
+template <class T>
+inline int CUDA::DeviceArray2D<T>::cols() const {
+  return DeviceMemory2D::colsBytes() / elem_size;
+}
+template <class T>
+inline int CUDA::DeviceArray2D<T>::rows() const {
+  return DeviceMemory2D::rows();
+}
+
+template <class T>
+inline size_t CUDA::DeviceArray2D<T>::elem_step() const {
+  return DeviceMemory2D::step() / elem_size;
+}
+
+#endif /* DEVICE_ARRAY_HPP_ */
\ No newline at end of file
diff --git a/embedded_deformation/include/containers/device_memory.cpp b/embedded_deformation/include/containers/device_memory.cpp
new file mode 100644
index 0000000..a6b5852
--- /dev/null
+++ b/embedded_deformation/include/containers/device_memory.cpp
@@ -0,0 +1,219 @@
+#include "device_memory.hpp"
+#include "convenience.cuh"
+
+#include "cuda_runtime.h"
+#include "assert.h"
+
+//////////////////////////    XADD    ///////////////////////////////
+
+#ifdef __GNUC__
+
+#if __GNUC__ * 10 + __GNUC_MINOR__ >= 42
+
+#if !defined WIN32 && (defined __i486__ || defined __i586__ || defined __i686__ || defined __MMX__ || defined __SSE__ || defined __ppc__)
+#define CV_XADD __sync_fetch_and_add
+#else
+#include <ext/atomicity.h>
+#define CV_XADD __gnu_cxx::__exchange_and_add
+#endif
+#else
+#include <bits/atomicity.h>
+#if __GNUC__ * 10 + __GNUC_MINOR__ >= 34
+#define CV_XADD __gnu_cxx::__exchange_and_add
+#else
+#define CV_XADD __exchange_and_add
+#endif
+#endif
+
+#elif defined WIN32 || defined _WIN32
+#include <intrin.h>
+#define CV_XADD(addr, delta) _InterlockedExchangeAdd((long volatile*)(addr), (delta))
+#else
+
+template <typename _Tp>
+static inline _Tp CV_XADD(_Tp* addr, _Tp delta) {
+  int tmp = *addr;
+  *addr += delta;
+  return tmp;
+}
+
+#endif
+
+////////////////////////    DeviceArray    /////////////////////////////
+
+CUDA::DeviceMemory::DeviceMemory() : data_(0), sizeBytes_(0), refcount_(0) {}
+CUDA::DeviceMemory::DeviceMemory(void* ptr_arg, size_t sizeBytes_arg) : data_(ptr_arg), sizeBytes_(sizeBytes_arg), refcount_(0) {}
+CUDA::DeviceMemory::DeviceMemory(size_t sizeBtes_arg) : data_(0), sizeBytes_(0), refcount_(0) { create(sizeBtes_arg); }
+CUDA::DeviceMemory::~DeviceMemory() { release(); }
+
+CUDA::DeviceMemory::DeviceMemory(const DeviceMemory& other_arg)
+    : data_(other_arg.data_), sizeBytes_(other_arg.sizeBytes_), refcount_(other_arg.refcount_) {
+  if (refcount_) CV_XADD(refcount_, 1);
+}
+
+CUDA::DeviceMemory& CUDA::DeviceMemory::operator=(const DeviceMemory& other_arg) {
+  if (this != &other_arg) {
+    if (other_arg.refcount_) CV_XADD(other_arg.refcount_, 1);
+    release();
+
+    data_ = other_arg.data_;
+    sizeBytes_ = other_arg.sizeBytes_;
+    refcount_ = other_arg.refcount_;
+  }
+  return *this;
+}
+
+void CUDA::DeviceMemory::create(size_t sizeBytes_arg) {
+  if (sizeBytes_arg == sizeBytes_) return;
+
+  if (sizeBytes_arg > 0) {
+    if (data_) release();
+
+    sizeBytes_ = sizeBytes_arg;
+
+    cudaSafeCall(cudaMalloc(&data_, sizeBytes_));
+
+    refcount_ = new int;
+    *refcount_ = 1;
+  }
+}
+
+void CUDA::DeviceMemory::copyTo(DeviceMemory& other) const {
+  if (empty())
+    other.release();
+  else {
+    other.create(sizeBytes_);
+    cudaSafeCall(cudaMemcpy(other.data_, data_, sizeBytes_, cudaMemcpyDeviceToDevice));
+    cudaSafeCall(cudaDeviceSynchronize());
+  }
+}
+
+void CUDA::DeviceMemory::release() {
+  if (refcount_ && CV_XADD(refcount_, -1) == 1) {
+    delete refcount_;
+    cudaSafeCall(cudaFree(data_));
+  }
+  data_ = 0;
+  sizeBytes_ = 0;
+  refcount_ = 0;
+}
+
+void CUDA::DeviceMemory::upload(const void* host_ptr_arg, size_t sizeBytes_arg) {
+  create(sizeBytes_arg);
+  cudaSafeCall(cudaMemcpy(data_, host_ptr_arg, sizeBytes_, cudaMemcpyHostToDevice));
+  cudaSafeCall(cudaDeviceSynchronize());
+}
+
+void CUDA::DeviceMemory::download(void* host_ptr_arg) const {
+  cudaSafeCall(cudaMemcpy(host_ptr_arg, data_, sizeBytes_, cudaMemcpyDeviceToHost));
+  cudaSafeCall(cudaDeviceSynchronize());
+}
+
+void CUDA::DeviceMemory::swap(DeviceMemory& other_arg) {
+  std::swap(data_, other_arg.data_);
+  std::swap(sizeBytes_, other_arg.sizeBytes_);
+  std::swap(refcount_, other_arg.refcount_);
+}
+
+bool CUDA::DeviceMemory::empty() const { return !data_; }
+size_t CUDA::DeviceMemory::sizeBytes() const { return sizeBytes_; }
+
+////////////////////////    DeviceArray2D    /////////////////////////////
+
+CUDA::DeviceMemory2D::DeviceMemory2D() : data_(0), step_(0), colsBytes_(0), rows_(0), refcount_(0) {}
+
+CUDA::DeviceMemory2D::DeviceMemory2D(int rows_arg, int colsBytes_arg) : data_(0), step_(0), colsBytes_(0), rows_(0), refcount_(0) {
+  create(rows_arg, colsBytes_arg);
+}
+
+CUDA::DeviceMemory2D::DeviceMemory2D(int rows_arg, int colsBytes_arg, void* data_arg, size_t step_arg)
+    : data_(data_arg), step_(step_arg), colsBytes_(colsBytes_arg), rows_(rows_arg), refcount_(0) {}
+
+CUDA::DeviceMemory2D::~DeviceMemory2D() { release(); }
+
+CUDA::DeviceMemory2D::DeviceMemory2D(const DeviceMemory2D& other_arg)
+    : data_(other_arg.data_),
+      step_(other_arg.step_),
+      colsBytes_(other_arg.colsBytes_),
+      rows_(other_arg.rows_),
+      refcount_(other_arg.refcount_) {
+  if (refcount_) CV_XADD(refcount_, 1);
+}
+
+CUDA::DeviceMemory2D& CUDA::DeviceMemory2D::operator=(const DeviceMemory2D& other_arg) {
+  if (this != &other_arg) {
+    if (other_arg.refcount_) CV_XADD(other_arg.refcount_, 1);
+    release();
+
+    colsBytes_ = other_arg.colsBytes_;
+    rows_ = other_arg.rows_;
+    data_ = other_arg.data_;
+    step_ = other_arg.step_;
+
+    refcount_ = other_arg.refcount_;
+  }
+  return *this;
+}
+
+void CUDA::DeviceMemory2D::create(int rows_arg, int colsBytes_arg) {
+  if (colsBytes_ == colsBytes_arg && rows_ == rows_arg) return;
+
+  if (rows_arg > 0 && colsBytes_arg > 0) {
+    if (data_) release();
+
+    colsBytes_ = colsBytes_arg;
+    rows_ = rows_arg;
+
+    cudaSafeCall(cudaMallocPitch((void**)&data_, &step_, colsBytes_, rows_));
+
+    refcount_ = new int;
+    *refcount_ = 1;
+  }
+}
+
+void CUDA::DeviceMemory2D::release() {
+  if (refcount_ && CV_XADD(refcount_, -1) == 1) {
+    delete refcount_;
+    cudaSafeCall(cudaFree(data_));
+  }
+
+  colsBytes_ = 0;
+  rows_ = 0;
+  data_ = 0;
+  step_ = 0;
+  refcount_ = 0;
+}
+
+void CUDA::DeviceMemory2D::copyTo(DeviceMemory2D& other) const {
+  if (empty())
+    other.release();
+  else {
+    other.create(rows_, colsBytes_);
+    cudaSafeCall(cudaMemcpy2D(other.data_, other.step_, data_, step_, colsBytes_, rows_, cudaMemcpyDeviceToDevice));
+    cudaSafeCall(cudaDeviceSynchronize());
+  }
+}
+
+void CUDA::DeviceMemory2D::upload(const void* host_ptr_arg, size_t host_step_arg, int rows_arg, int colsBytes_arg) {
+  create(rows_arg, colsBytes_arg);
+  cudaSafeCall(cudaMemcpy2D(data_, step_, host_ptr_arg, host_step_arg, colsBytes_, rows_, cudaMemcpyHostToDevice));
+}
+
+void CUDA::DeviceMemory2D::download(void* host_ptr_arg, size_t host_step_arg) const {
+  cudaSafeCall(cudaMemcpy2D(host_ptr_arg, host_step_arg, data_, step_, colsBytes_, rows_, cudaMemcpyDeviceToHost));
+}
+
+void CUDA::DeviceMemory2D::swap(DeviceMemory2D& other_arg) {
+  std::swap(data_, other_arg.data_);
+  std::swap(step_, other_arg.step_);
+
+  std::swap(colsBytes_, other_arg.colsBytes_);
+  std::swap(rows_, other_arg.rows_);
+  std::swap(refcount_, other_arg.refcount_);
+}
+
+bool CUDA::DeviceMemory2D::empty() const { return !data_; }
+int CUDA::DeviceMemory2D::colsBytes() const { return colsBytes_; }
+int CUDA::DeviceMemory2D::rows() const { return rows_; }
+size_t CUDA::DeviceMemory2D::step() const { return step_; }
+
diff --git a/embedded_deformation/include/containers/device_memory.hpp b/embedded_deformation/include/containers/device_memory.hpp
new file mode 100644
index 0000000..73c8068
--- /dev/null
+++ b/embedded_deformation/include/containers/device_memory.hpp
@@ -0,0 +1,272 @@
+#ifndef DEVICE_MEMORY_HPP_
+#define DEVICE_MEMORY_HPP_
+
+#include "containers/kernel_containers.hpp"
+
+
+namespace CUDA{
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b DeviceMemory class
+  *
+  * \note This is a BLOB container class with reference counting for GPU memory.
+  *
+  * \author Anatoly Baksheev
+  */
+
+class DeviceMemory {
+public:
+  /** \brief Empty constructor. */
+  DeviceMemory();
+
+  /** \brief Destructor. */
+  ~DeviceMemory();
+
+  /** \brief Allocates internal buffer in GPU memory
+    * \param sizeBytes_arg: amount of memory to allocate
+    * */
+  DeviceMemory(size_t sizeBytes_arg);
+
+  /** \brief Initializes with user allocated buffer. Reference counting is disabled in this case.
+    * \param ptr_arg: pointer to buffer
+    * \param sizeBytes_arg: buffer size
+    * */
+  DeviceMemory(void* ptr_arg, size_t sizeBytes_arg);
+
+  /** \brief Copy constructor. Just increments reference counter. */
+  DeviceMemory(const DeviceMemory& other_arg);
+
+  /** \brief Assigment operator. Just increments reference counter. */
+  DeviceMemory& operator=(const DeviceMemory& other_arg);
+
+  /** \brief Allocates internal buffer in GPU memory. If internal buffer was created before the function recreates it with
+   * new size. If new and old sizes are equal it does nothing.
+    * \param sizeBytes_arg: buffer size
+    * */
+  void create(size_t sizeBytes_arg);
+
+  /** \brief Decrements reference counter and releases internal buffer if needed. */
+  void release();
+
+  /** \brief Performs data copying. If destination size differs it will be reallocated.
+    * \param other_arg: destination container
+    * */
+  void copyTo(DeviceMemory& other) const;
+
+  /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to ensure that intenal buffer size is
+   * enough.
+    * \param host_ptr_arg: pointer to buffer to upload
+    * \param sizeBytes_arg: buffer size
+    * */
+  void upload(const void* host_ptr_arg, size_t sizeBytes_arg);
+
+  /** \brief Downloads data from internal buffer to CPU memory
+    * \param host_ptr_arg: pointer to buffer to download
+    * */
+  void download(void* host_ptr_arg) const;
+
+  /** \brief Performs swap of data pointed with another device memory.
+    * \param other: device memory to swap with
+    * */
+  void swap(DeviceMemory& other_arg);
+
+  /** \brief Returns pointer for internal buffer in GPU memory. */
+  template <class T>
+  T* ptr();
+
+  /** \brief Returns constant pointer for internal buffer in GPU memory. */
+  template <class T>
+  const T* ptr() const;
+
+  /** \brief Conversion to PtrSz for passing to kernel functions. */
+  template <class U>
+  operator PtrSz<U>() const;
+
+  /** \brief Returns true if unallocated otherwise false. */
+  bool empty() const;
+
+  size_t sizeBytes() const;
+
+private:
+  /** \brief Device pointer. */
+  void* data_;
+
+  /** \brief Allocated size in bytes. */
+  size_t sizeBytes_;
+
+  /** \brief Pointer to reference counter in CPU memory. */
+  int* refcount_;
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief @b DeviceMemory2D class
+  *
+  * \note This is a BLOB container class with reference counting for pitched GPU memory.
+  *
+  * \author Anatoly Baksheev
+  */
+
+class DeviceMemory2D {
+public:
+  /** \brief Empty constructor. */
+  DeviceMemory2D();
+
+  /** \brief Destructor. */
+  ~DeviceMemory2D();
+
+  /** \brief Allocates internal buffer in GPU memory
+    * \param rows_arg: number of rows to allocate
+    * \param colsBytes_arg: width of the buffer in bytes
+    * */
+  DeviceMemory2D(int rows_arg, int colsBytes_arg);
+
+  /** \brief Initializes with user allocated buffer. Reference counting is disabled in this case.
+    * \param rows_arg: number of rows
+    * \param colsBytes_arg: width of the buffer in bytes
+    * \param data_arg: pointer to buffer
+    * \param stepBytes_arg: stride between two consecutive rows in bytes
+    * */
+  DeviceMemory2D(int rows_arg, int colsBytes_arg, void* data_arg, size_t step_arg);
+
+  /** \brief Copy constructor. Just increments reference counter. */
+  DeviceMemory2D(const DeviceMemory2D& other_arg);
+
+  /** \brief Assigment operator. Just increments reference counter. */
+  DeviceMemory2D& operator=(const DeviceMemory2D& other_arg);
+
+  /** \brief Allocates internal buffer in GPU memory. If internal buffer was created before the function recreates it with
+   * new size. If new and old sizes are equal it does nothing.
+     * \param ptr_arg: number of rows to allocate
+     * \param sizeBytes_arg: width of the buffer in bytes
+     * */
+  void create(int rows_arg, int colsBytes_arg);
+
+  /** \brief Decrements reference counter and releases internal buffer if needed. */
+  void release();
+
+  /** \brief Performs data copying. If destination size differs it will be reallocated.
+    * \param other_arg: destination container
+    * */
+  void copyTo(DeviceMemory2D& other) const;
+
+  /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to ensure that intenal buffer size is
+   * enough.
+    * \param host_ptr_arg: pointer to host buffer to upload
+    * \param host_step_arg: stride between two consecutive rows in bytes for host buffer
+    * \param rows_arg: number of rows to upload
+    * \param sizeBytes_arg: width of host buffer in bytes
+    * */
+  void upload(const void* host_ptr_arg, size_t host_step_arg, int rows_arg, int colsBytes_arg);
+
+  /** \brief Downloads data from internal buffer to CPU memory. User is resposible for correct host buffer size.
+    * \param host_ptr_arg: pointer to host buffer to download
+    * \param host_step_arg: stride between two consecutive rows in bytes for host buffer
+    * */
+  void download(void* host_ptr_arg, size_t host_step_arg) const;
+
+  /** \brief Performs swap of data pointed with another device memory.
+    * \param other: device memory to swap with
+    * */
+  void swap(DeviceMemory2D& other_arg);
+
+  /** \brief Returns pointer to given row in internal buffer.
+    * \param y_arg: row index
+    * */
+  template <class T>
+  T* ptr(int y_arg = 0);
+
+  /** \brief Returns constant pointer to given row in internal buffer.
+    * \param y_arg: row index
+    * */
+  template <class T>
+  const T* ptr(int y_arg = 0) const;
+
+  /** \brief Conversion to PtrStep for passing to kernel functions. */
+  template <class U>
+  operator PtrStep<U>() const;
+
+  /** \brief Conversion to PtrStepSz for passing to kernel functions. */
+  template <class U>
+  operator PtrStepSz<U>() const;
+
+  /** \brief Returns true if unallocated otherwise false. */
+  bool empty() const;
+
+  /** \brief Returns number of bytes in each row. */
+  int colsBytes() const;
+
+  /** \brief Returns number of rows. */
+  int rows() const;
+
+  /** \brief Returns stride between two consecutive rows in bytes for internal buffer. Step is stored always and everywhere
+   * in bytes!!! */
+  size_t step() const;
+
+private:
+  /** \brief Device pointer. */
+  void* data_;
+
+  /** \brief Stride between two consecutive rows in bytes for internal buffer. Step is stored always and everywhere in
+   * bytes!!! */
+  size_t step_;
+
+  /** \brief Width of the buffer in bytes. */
+  int colsBytes_;
+
+  /** \brief Number of rows. */
+  int rows_;
+
+  /** \brief Pointer to reference counter in CPU memory. */
+  int* refcount_;
+};
+
+/////////////////////  Inline implementations of DeviceMemory ////////////////////////////////////////////
+template <class T>
+inline T* DeviceMemory::ptr() {
+  return (T*)data_;
+}
+template <class T>
+inline const T* DeviceMemory::ptr() const {
+  return (const T*)data_;
+}
+
+template <class U>
+inline DeviceMemory::operator PtrSz<U>() const {
+  PtrSz<U> result;
+  result.data = (U*)ptr<U>();
+  result.size = sizeBytes_ / sizeof(U);
+  return result;
+}
+}
+
+
+/////////////////////  Inline implementations of DeviceMemory2D ////////////////////////////////////////////
+
+template <class T>
+T* CUDA::DeviceMemory2D::ptr(int y_arg) {
+  return (T*)((char*)data_ + y_arg * step_);
+}
+template <class T>
+const T* CUDA::DeviceMemory2D::ptr(int y_arg) const {
+  return (const T*)((const char*)data_ + y_arg * step_);
+}
+
+template <class U>
+CUDA::DeviceMemory2D::operator PtrStep<U>() const {
+  PtrStep<U> result;
+  result.data = (U*)ptr<U>();
+  result.step = step_;
+  return result;
+}
+
+template <class U>
+CUDA::DeviceMemory2D::operator PtrStepSz<U>() const {
+  PtrStepSz<U> result;
+  result.data = (U*)ptr<U>();
+  result.step = step_;
+  result.cols = colsBytes_ / sizeof(U);
+  result.rows = rows_;
+  return result;
+}
+
+
+#endif /* COFUSION_DEVICE_MEMORY_HPP_ */
\ No newline at end of file
diff --git a/embedded_deformation/include/containers/kernel_containers.hpp b/embedded_deformation/include/containers/kernel_containers.hpp
new file mode 100644
index 0000000..70af1d9
--- /dev/null
+++ b/embedded_deformation/include/containers/kernel_containers.hpp
@@ -0,0 +1,67 @@
+#ifndef KERNEL_CONTAINERS_HPP_
+#define KERNEL_CONTAINERS_HPP_
+
+#include <cstddef>
+#include "imageProcessor/safe_call.hpp"
+
+#if defined(__CUDACC__)
+#define GPU_HOST_DEVICE__ __host__ __device__ __forceinline__
+#else
+#define GPU_HOST_DEVICE__
+#endif
+
+namespace CUDA{
+  
+template <typename T>
+struct DevPtr {
+  typedef T elem_type;
+  const static size_t elem_size = sizeof(elem_type);
+
+  T* data;
+
+  GPU_HOST_DEVICE__ DevPtr() : data(0) {}                                        
+  GPU_HOST_DEVICE__ DevPtr(T* data_arg) : data(data_arg) {}                       
+
+  GPU_HOST_DEVICE__ size_t elemSize() const { return elem_size; }                 
+  GPU_HOST_DEVICE__ operator T*() { return data; }
+  GPU_HOST_DEVICE__ operator const T*() const { return data; }
+};
+
+// 继承父类 加入元素数量
+template <typename T> 
+struct PtrSz : public DevPtr<T> {
+  GPU_HOST_DEVICE__ PtrSz() : size(0) {}
+  GPU_HOST_DEVICE__ PtrSz(T* data_arg, size_t size_arg) : DevPtr<T>(data_arg), size(size_arg) {}
+
+  size_t size;
+};
+
+// 继承父类 加入表示两个连续行之间的步长 适合用于表示二维数组
+template <typename T>
+struct PtrStep : public DevPtr<T> {
+  GPU_HOST_DEVICE__ PtrStep() : step(0) {}
+  GPU_HOST_DEVICE__ PtrStep(T* data_arg, size_t step_arg) : DevPtr<T>(data_arg), step(step_arg) {}              // 调用基类的构造函数进行初始化
+
+  /** \brief stride between two consecutive rows in bytes. Step is stored always and everywhere in bytes!!! */
+  size_t step;
+
+  GPU_HOST_DEVICE__ T* ptr(int y = 0) { return (T*)((char*)DevPtr<T>::data + y * step); }                          // 用于获取指向指定行的指针
+  GPU_HOST_DEVICE__ const T* ptr(int y = 0) const { return (const T*)((const char*)DevPtr<T>::data + y * step); }
+};
+
+// 用于表示和操作大小和布局都已知的二维数组
+template <typename T>
+struct PtrStepSz : public PtrStep<T> {
+  GPU_HOST_DEVICE__ PtrStepSz() : cols(0), rows(0) {}
+  GPU_HOST_DEVICE__ PtrStepSz(int rows_arg, int cols_arg, T* data_arg, size_t step_arg)                           // 指定行，列，数据类型，
+      : PtrStep<T>(data_arg, step_arg), cols(cols_arg), rows(rows_arg) {}
+
+  int cols;
+  int rows;
+};
+}
+
+
+
+
+#endif /* KERNEL_CONTAINERS_HPP_ */
diff --git a/embedded_deformation/include/embedded_deformation/costFunction.hpp b/embedded_deformation/include/embedded_deformation/costFunction.hpp
index 731e304..0faeebf 100644
--- a/embedded_deformation/include/embedded_deformation/costFunction.hpp
+++ b/embedded_deformation/include/embedded_deformation/costFunction.hpp
@@ -22,6 +22,7 @@ class RigCostFunction: public ceres::CostFunction
 	~RigCostFunction(){
 	}
 
+	// 函数的目标是最小化两个变换之间的差异
 	virtual bool Evaluate(double const* const* parameters,
                           double* residuals,
                           double** jacobians) const
@@ -241,7 +242,7 @@ class RotCostFunction_2: public ceres::CostFunction
 	RotCostFunction_2(double cost_function_weight){
 		// define residual and block_size
 		set_num_residuals(9);
-		std::vector<int>* block_sizes =  mutable_parameter_block_sizes();
+		std::vector<int>* block_sizes =  mutable_parameter_block_sizes();          // 在Ceres中，参数块是优化过程中要求解的变量的集合
 		block_sizes->push_back(12);
 
 		cost_function_weight_ = sqrt(cost_function_weight);
@@ -263,7 +264,7 @@ class RotCostFunction_2: public ceres::CostFunction
         if (jacobians!=NULL){
             if(jacobians[0]!=NULL){
 
-				//R'*R - I (0,0)
+				//R'*R - I (0,0) R的逆乘R- 单位矩阵 有9个元素 即9个残差 每个残差都对12个参数求导 所以一行雅可比有9*12个元素
 				jacobians[0][ 0] = cost_function_weight_*2*R(0,0);	// df/dR(0,0)
 				jacobians[0][ 1] = cost_function_weight_*2*R(1,0);	// df/dR(1,0)
 				jacobians[0][ 2] = cost_function_weight_*2*R(2,0);	// df/dR(2,0)
@@ -408,7 +409,7 @@ class RegCostFunction: public ceres::CostFunction
 		// define residual and block_size
 		set_num_residuals(3);
 		std::vector<int>* block_sizes =  mutable_parameter_block_sizes();
-		block_sizes->push_back(12);
+		block_sizes->push_back(12);                                                    // 两个参数块，每个参数块有12个参数
 		block_sizes->push_back(12);
 
 		g_j_ = g_j;
@@ -436,16 +437,18 @@ class RegCostFunction: public ceres::CostFunction
 
 			if (jacobians!=NULL){
 				if(jacobians[0]!=NULL){
-					jacobians[0][ 0] = cost_function_weight_ * alpha_j_k * ( g_k_(0) - g_j_(0) );
-					jacobians[0][ 1] = 0;
-					jacobians[0][ 2] = 0;
-					jacobians[0][ 3] = cost_function_weight_ * alpha_j_k * ( g_k_(1) - g_j_(1) );
-					jacobians[0][ 4] = 0;
+					// 对第一个参数块中的Rj和tj求导
+					// regularizer term 的残差由有3项 每一项对12个参数求导 所以一行雅可比有3*12个元素
+					jacobians[0][ 0] = cost_function_weight_ * alpha_j_k * ( g_k_(0) - g_j_(0) );    // df/dR(0,0)
+					jacobians[0][ 1] = 0;															 // df/dR(1,0)
+					jacobians[0][ 2] = 0;															 // df/dR(2,0)
+					jacobians[0][ 3] = cost_function_weight_ * alpha_j_k * ( g_k_(1) - g_j_(1) );    // df/dR(0,1)
+					jacobians[0][ 4] = 0;                                                            // df/dR(1,1)
 					jacobians[0][ 5] = 0;
-					jacobians[0][ 6] = cost_function_weight_ * alpha_j_k * ( g_k_(2) - g_j_(2) );
+					jacobians[0][ 6] = cost_function_weight_ * alpha_j_k * ( g_k_(2) - g_j_(2) );    // df/dR(0,2)
 					jacobians[0][ 7] = 0;
 					jacobians[0][ 8] = 0;
-					jacobians[0][ 9] = cost_function_weight_ * alpha_j_k;
+					jacobians[0][ 9] = cost_function_weight_ * alpha_j_k;                            // df/dT(0)
 					jacobians[0][10] = 0;
 					jacobians[0][11] = 0;
 
@@ -475,7 +478,9 @@ class RegCostFunction: public ceres::CostFunction
 					jacobians[0][10 + 12*2] = 0;
 					jacobians[0][11 + 12*2] = cost_function_weight_ * alpha_j_k;
 
+					
 
+					// 对第二个参数块中tk求导
 					jacobians[1][ 0] = 0;
 					jacobians[1][ 1] = 0;
 					jacobians[1][ 2] = 0;
@@ -536,9 +541,9 @@ class ConCostFunction: public ceres::CostFunction
 					Eigen::Vector3d source, 
 					Eigen::Vector3d target){
 		// define residual and block_size
-		source_ = source;
-		target_ = target;
-		vector_g_ = vector_g;
+		source_ = source;                                    // 源点
+		target_ = target;                                    // 目标点
+		vector_g_ = vector_g;                                // 源点邻居
 		cost_function_weight_ = sqrt(cost_function_weight);
 
 		nodes_connectivity = vector_g_.size()-1;
@@ -552,14 +557,14 @@ class ConCostFunction: public ceres::CostFunction
 		// equation (3)
 		w_j_.clear();
 		for (int i = 0; i < nodes_connectivity; ++i)
-			w_j_.push_back( pow(1 - (source_ - vector_g_[i]).squaredNorm() / (source_ - vector_g_.back()).squaredNorm(), 2) );
+			w_j_.push_back( pow(1 - (source_ - vector_g_[i]).squaredNorm() / (source_ - vector_g_.back()).squaredNorm(), 2) );    // 源点与近邻越近权重越大
 
 		double normalization_factor = 0;
 		for (int i = 0; i < nodes_connectivity; ++i)
 			normalization_factor += w_j_[i];
 
 		for (int i = 0; i < nodes_connectivity; ++i)
-			w_j_[i] /= normalization_factor;
+			w_j_[i] /= normalization_factor;                            // 归一化权重
 	}
 
 	~ConCostFunction(){
@@ -577,6 +582,7 @@ class ConCostFunction: public ceres::CostFunction
 			{
 				Eigen::Map<const Eigen::Matrix3d> R_j(parameters[i]);
 				Eigen::Map<const Eigen::Vector3d> t_j(parameters[i]+9);
+				// source根据近邻节点的R和t进行变换
 				new_node_position += w_j_[i] * (R_j * (source_ - vector_g_[i]) + vector_g_[i] + t_j);
 			}
 
@@ -587,15 +593,17 @@ class ConCostFunction: public ceres::CostFunction
 
 			if (jacobians!=NULL){
 				if(jacobians[0]!=NULL){
+					// 有几个近邻
+					// 3个残差 每个残差对12个参数求导 所以一行雅可比有3*12个元素
 					for (int i = 0; i < w_j_.size(); ++i)
 					{
-						jacobians[i][ 0] = cost_function_weight_ * w_j_[i]*( source_(0) - vector_g_[i](0) );
+						jacobians[i][ 0] = cost_function_weight_ * w_j_[i]*( source_(0) - vector_g_[i](0) );       // df1/dR(0,0)
 						jacobians[i][ 1] = 0;
 						jacobians[i][ 2] = 0;
-						jacobians[i][ 3] = cost_function_weight_ * w_j_[i]*( source_(1) - vector_g_[i](1) );
+						jacobians[i][ 3] = cost_function_weight_ * w_j_[i]*( source_(1) - vector_g_[i](1) );       // df1/dR(0,1)
 						jacobians[i][ 4] = 0;
 						jacobians[i][ 5] = 0;
-						jacobians[i][ 6] = cost_function_weight_ * w_j_[i]*( source_(2) - vector_g_[i](2) );
+						jacobians[i][ 6] = cost_function_weight_ * w_j_[i]*( source_(2) - vector_g_[i](2) );       // df1/dR(0,2)
 						jacobians[i][ 7] = 0;
 						jacobians[i][ 8] = 0;
 						jacobians[i][ 9] = cost_function_weight_ * w_j_[i];
@@ -603,7 +611,7 @@ class ConCostFunction: public ceres::CostFunction
 						jacobians[i][11] = 0;
 
 						jacobians[i][ 0 + 12] = 0;
-						jacobians[i][ 1 + 12] = cost_function_weight_ * w_j_[i]*( source_(0) - vector_g_[i](0) );
+						jacobians[i][ 1 + 12] = cost_function_weight_ * w_j_[i]*( source_(0) - vector_g_[i](0) );  // df2/dR(1.0)
 						jacobians[i][ 2 + 12] = 0;
 						jacobians[i][ 3 + 12] = 0;
 						jacobians[i][ 4 + 12] = cost_function_weight_ * w_j_[i]*( source_(1) - vector_g_[i](1) );
@@ -617,7 +625,7 @@ class ConCostFunction: public ceres::CostFunction
 
 						jacobians[i][ 0 + 12*2] = 0;
 						jacobians[i][ 1 + 12*2] = 0;
-						jacobians[i][ 2 + 12*2] = cost_function_weight_ * w_j_[i]*( source_(0) - vector_g_[i](0) );
+						jacobians[i][ 2 + 12*2] = cost_function_weight_ * w_j_[i]*( source_(0) - vector_g_[i](0) );  // df3/dR(2,0)
 						jacobians[i][ 3 + 12*2] = 0;
 						jacobians[i][ 4 + 12*2] = 0;
 						jacobians[i][ 5 + 12*2] = cost_function_weight_ * w_j_[i]*( source_(1) - vector_g_[i](1) );
@@ -643,6 +651,100 @@ class ConCostFunction: public ceres::CostFunction
 };
 
 
+// point to plane 自己添加的
+class p2plCostFunction: public ceres::CostFunction
+{
+public:
+	// constructor
+	p2plCostFunction(double cost_function_weight, 
+	                std::vector<Eigen::Vector3d> vector_g, 
+					Eigen::Vector3d source, 
+					Eigen::Vector3d target,
+					Eigen::Vector3d normal){
+		// define residual and block_size
+		source_ = source;                                    // 源点
+		target_ = target;                                    // 目标点
+		vector_g_ = vector_g;                                // 源点邻居
+		normal_ = normal;                                    // 目标点法向量
+		cost_function_weight_ = sqrt(cost_function_weight);
+
+		nodes_connectivity = vector_g_.size()-1;
+		
+		// set the size of the parameters input
+		set_num_residuals(1);
+		std::vector<int>* block_sizes =  mutable_parameter_block_sizes();
+		for (int i = 0; i < nodes_connectivity; ++i)
+			block_sizes->push_back(12);
+		
+		// equation (3)
+		w_j_.clear();
+		for (int i = 0; i < nodes_connectivity; ++i)
+			w_j_.push_back( pow(1 - (source_ - vector_g_[i]).squaredNorm() / (source_ - vector_g_.back()).squaredNorm(), 2) );    // 源点与近邻越近权重越大
+
+		double normalization_factor = 0;
+		for (int i = 0; i < nodes_connectivity; ++i)
+			normalization_factor += w_j_[i];
+
+		for (int i = 0; i < nodes_connectivity; ++i)
+			w_j_[i] /= normalization_factor;                            // 归一化权重
+	}
+
+	~p2plCostFunction(){
+	}
+
+	virtual bool Evaluate(double const* const* parameters,
+                          double* residuals,
+                          double** jacobians) const{
+			Eigen::Map<Eigen::MatrixXd>res(residuals,1,1);
+
+			// back to equation (8)
+			Eigen::Vector3d new_node_position;
+			new_node_position << 0, 0, 0;
+			for (int i = 0; i < nodes_connectivity; ++i)
+			{
+				Eigen::Map<const Eigen::Matrix3d> R_j(parameters[i]);
+				Eigen::Map<const Eigen::Vector3d> t_j(parameters[i]+9);
+				new_node_position += w_j_[i] * (R_j * (source_ - vector_g_[i]) + vector_g_[i] + t_j);
+			}
+
+			res(0,0) = cost_function_weight_ * (new_node_position - target_).transpose() * normal_;
+
+
+			if (jacobians!=NULL){
+				if(jacobians[0]!=NULL){
+					// 有几个近邻
+					for (int i = 0; i < w_j_.size(); ++i)
+					{
+						jacobians[i][ 0] = cost_function_weight_ * w_j_[i] * normal_(0) * (source_(0) - vector_g_[i](0));       // df/dR(0,0)
+						jacobians[i][ 1] = cost_function_weight_ * w_j_[i] * normal_(1) * (source_(0) - vector_g_[i](0));		// df/dR(1,0)		
+						jacobians[i][ 2] = cost_function_weight_ * w_j_[i] * normal_(2) * (source_(0) - vector_g_[i](0));	    // df/dR(2,0)
+						jacobians[i][ 3] = cost_function_weight_ * w_j_[i] * normal_(0) * (source_(1) - vector_g_[i](1));       // df/dR(0,1)
+						jacobians[i][ 4] = cost_function_weight_ * w_j_[i] * normal_(1) * (source_(1) - vector_g_[i](1));	    // df/dR(1,1)
+						jacobians[i][ 5] = cost_function_weight_ * w_j_[i] * normal_(2) * (source_(1) - vector_g_[i](1));	    // df/dR(2,1)
+						jacobians[i][ 6] = cost_function_weight_ * w_j_[i] * normal_(0) * (source_(2) - vector_g_[i](2));       // df/dR(0,2)
+						jacobians[i][ 7] = cost_function_weight_ * w_j_[i] * normal_(1) * (source_(2) - vector_g_[i](2));       // df/dR(1,2)
+						jacobians[i][ 8] = cost_function_weight_ * w_j_[i] * normal_(2) * (source_(2) - vector_g_[i](2));       // df/dR(2,2)
+						jacobians[i][ 9] = cost_function_weight_ * w_j_[i] * normal_(0);										// df/dT(0)
+						jacobians[i][10] = cost_function_weight_ * w_j_[i] * normal_(1);								        // df/dT(1)
+						jacobians[i][11] = cost_function_weight_ * w_j_[i] * normal_(2);									    // df/dT(2)
+					}
+				}
+			}
+			return true;
+	}
+
+private:
+	Eigen::Vector3d source_;
+	Eigen::Vector3d target_;
+	Eigen::Vector3d normal_;
+	std::vector<Eigen::Vector3d> vector_g_;
+	std::vector<double> w_j_;
+	int nodes_connectivity;
+	double cost_function_weight_;
+};
+
+// point to plane 自己添加的
+
 // equation (20) in ElasticFusion (journal version) 
 class RelCostFunction: public ceres::CostFunction
 {
diff --git a/embedded_deformation/include/embedded_deformation/downsampling.hpp b/embedded_deformation/include/embedded_deformation/downsampling.hpp
index a535c8c..cec76d3 100644
--- a/embedded_deformation/include/embedded_deformation/downsampling.hpp
+++ b/embedded_deformation/include/embedded_deformation/downsampling.hpp
@@ -116,8 +116,9 @@ inline void downsampling(Eigen::MatrixXd & in_cloud,
 {
 	// overwrite the leaf_size
 	if (use_relative_grid) {
+		std::cout<<"use relative grid"<<std::endl;
 		double scale;
-		getScale(in_cloud, scale);
+		getScale(in_cloud, scale);                          // 计算一个能包含所有点云的最小立方体的对角线长度
 		leaf_size = scale / grid_resolution;
 	}
 
@@ -135,13 +136,14 @@ inline void downsampling(Eigen::MatrixXd & in_cloud,
 	out_cloud.resize(downsampled_cloud.rows(), 3);
 
 	nanoflann_wrapper tree(in_cloud);
-	for (int i = 0; i < downsampled_cloud.rows(); ++i)
+	// 在in_cloud中找到downsampled_cloud中每个点的最近邻
+	for (int i = 0; i < downsampled_cloud.rows(); ++i)                                 // 遍历所有采样点
 	{
 		std::vector< int > closest_point;
 		closest_point = tree.return_k_closest_points(downsampled_cloud.row(i), 1);
 
-		out_cloud.row(i) = in_cloud.row( closest_point[0] );
-		in_cloud_samples.push_back(closest_point[0]);
+		out_cloud.row(i) = in_cloud.row( closest_point[0] );                          // 将in_cloud中的最近邻点赋值给out_cloud
+		in_cloud_samples.push_back(closest_point[0]);                                 // 记录in_cloud中的最近邻点的索引
 	}
 
 };
diff --git a/embedded_deformation/include/embedded_deformation/embedDeform.hpp b/embedded_deformation/include/embedded_deformation/embedDeform.hpp
index be08a20..606006b 100644
--- a/embedded_deformation/include/embedded_deformation/embedDeform.hpp
+++ b/embedded_deformation/include/embedded_deformation/embedDeform.hpp
@@ -1,9 +1,3 @@
-/*
-*   embedded deformation implementation
-*   by R. Falque
-*   14/11/2018
-*/
-
 #ifndef EMBEDDED_DEFORMATION
 #define EMBEDDED_DEFORMATION
 
@@ -44,6 +38,7 @@ class EmbeddedDeformation
 						 int k);
 
 	EmbeddedDeformation(Eigen::MatrixXd V_in, 
+						Eigen::MatrixXd normal_in,
 						 options opts);
 
 	// destructor
@@ -51,7 +46,7 @@ class EmbeddedDeformation
 		if(deformation_graph_ptr_ != nullptr) delete deformation_graph_ptr_;
 	}
 
-	void deform(Eigen::MatrixXd sources_in, Eigen::MatrixXd targets_in, Eigen::MatrixXd & V_deformed);
+	void deform(Eigen::MatrixXd sources_in, Eigen::MatrixXd targets_in, Eigen::MatrixXd targets_normal_in, Eigen::MatrixXd & V_deformed, Eigen::MatrixXd & normal_deformed, options opts);
 	void deform_other_points(Eigen::MatrixXd & V);
 	void update_normals(Eigen::MatrixXd & normals);
 	void show_deformation_graph();
@@ -61,7 +56,8 @@ class EmbeddedDeformation
 	double w_rot_ = 1;
 	double w_reg_ = 10;
 	double w_rig_ = 10;
-	double w_con_ = 100;
+	double w_con_ = 1;
+	double w_p2pl_ = 0.1;
 
 	// options
 	bool use_knn_;
@@ -73,6 +69,7 @@ class EmbeddedDeformation
 
 	Eigen::MatrixXd V_;
 	Eigen::MatrixXi F_;
+	Eigen::MatrixXd normal_;
 	
 	libgraphcpp::Graph* deformation_graph_ptr_ = nullptr;;
 
diff --git a/embedded_deformation/include/embedded_deformation/getMinMax.hpp b/embedded_deformation/include/embedded_deformation/getMinMax.hpp
index ef5e6a9..b349543 100644
--- a/embedded_deformation/include/embedded_deformation/getMinMax.hpp
+++ b/embedded_deformation/include/embedded_deformation/getMinMax.hpp
@@ -1,9 +1,3 @@
-/*
-*   Greedy Search
-*   by R. Falque
-*   29/11/2018
-*/
-
 #ifndef GETMINMAX_HPP
 #define GETMINMAX_HPP
 
diff --git a/embedded_deformation/include/embedded_deformation/nanoflannWrapper.hpp b/embedded_deformation/include/embedded_deformation/nanoflannWrapper.hpp
index ceaacf3..979ffc4 100644
--- a/embedded_deformation/include/embedded_deformation/nanoflannWrapper.hpp
+++ b/embedded_deformation/include/embedded_deformation/nanoflannWrapper.hpp
@@ -43,7 +43,7 @@ class nanoflann_wrapper
 			query_pt.push_back( query_point(d) );
 
 		// set wtf vectors
-		std::vector<size_t> ret_indexes(k);
+		std::vector<size_t> ret_indexes(k);                   
 		std::vector<double> out_dists_sqr(k);
 		nanoflann::KNNResultSet<double> resultSet(k);
 		resultSet.init( &ret_indexes.at(0), &out_dists_sqr.at(0) );
diff --git a/embedded_deformation/include/embedded_deformation/options.hpp b/embedded_deformation/include/embedded_deformation/options.hpp
index aa1efe5..e6e81c0 100644
--- a/embedded_deformation/include/embedded_deformation/options.hpp
+++ b/embedded_deformation/include/embedded_deformation/options.hpp
@@ -7,11 +7,16 @@
 struct options
 {
     /* data */
-    std::string path_pairwise_correspondence;
+    // std::string path_pairwise_correspondence;
     std::string path_input_file;
-    std::string path_output_file;
+    // std::string path_output_file;
     std::string path_input_obj;
     std::string path_graph_obj;
+    float principal_x, principal_y, focal_x, focal_y;
+    unsigned width, height;
+    int start_frame;
+    std::string image_path;
+
 
     bool   visualization;
     bool   verbose;
@@ -53,8 +58,8 @@ struct options
         {
             std::cout <<  "path_input_file: " << path_input_file << std::endl;
         }
-        std::cout << "path_output_file: " << path_output_file << std::endl;
-        std::cout << "path_pairwise_correspondence: " << path_pairwise_correspondence << std::endl;
+        // std::cout << "path_output_file: " << path_output_file << std::endl;
+        // std::cout << "path_pairwise_correspondence: " << path_pairwise_correspondence << std::endl;
         std::cout << std::endl;
         std::cout << "*** General parameters ***" << std::endl;
         std::cout << "visualization: " << visualization << std::endl;
@@ -81,11 +86,19 @@ struct options
         YAML::Node config = YAML::LoadFile(config_file);
         
         // IO
-        path_input_file                 = config["io_files"]["input_ply"].as<std::string>();
-        path_output_file                = config["io_files"]["output_ply"].as<std::string>();
-        path_input_obj                  = config["io_files"]["input_obj"].as<std::string>();
-        path_graph_obj                  = config["io_files"]["graph_obj"].as<std::string>();
-        path_pairwise_correspondence    = config["io_files"]["pointwise_correspondence"].as<std::string>();
+        // path_input_file                 = config["io_files"]["input_ply"].as<std::string>();
+        // path_output_file                = config["io_files"]["output_ply"].as<std::string>();
+        principal_x                     = config["io_files"]["principal_x"].as<float>();
+        principal_y                     = config["io_files"]["principal_y"].as<float>();
+        focal_x                         = config["io_files"]["focal_x"].as<float>();
+        focal_y                         = config["io_files"]["focal_y"].as<float>();
+        width                           = config["io_files"]["width"].as<unsigned>();
+        height                          = config["io_files"]["height"].as<unsigned>();
+        image_path                      = config["io_files"]["image_path"].as<std::string>();
+        start_frame                     = config["io_files"]["start_frame"].as<int>();
+        // path_input_obj                  = config["io_files"]["input_obj"].as<std::string>();
+        // path_graph_obj                  = config["io_files"]["graph_obj"].as<std::string>();
+        // path_pairwise_correspondence    = config["io_files"]["pointwise_correspondence"].as<std::string>();
 
         // general options
         visualization                   = config["general_params"]["visualization"].as<bool>();
diff --git a/embedded_deformation/include/imageProcessor/common.h b/embedded_deformation/include/imageProcessor/common.h
new file mode 100644
index 0000000..0a9dbf5
--- /dev/null
+++ b/embedded_deformation/include/imageProcessor/common.h
@@ -0,0 +1,72 @@
+#pragma once
+#include "common_types.hpp"
+#include "cuda_runtime_api.h"
+#include "safe_call.hpp"
+#include "global_configs.h"
+#include <vector>
+#include <glm/vec3.hpp>
+#include <math/vector_ops.hpp>
+#include "containers/device_array.hpp"
+
+using namespace CUDA;
+
+// 创建深度图纹理表面
+void createDepthTextureSurface(const unsigned width, 
+                            const unsigned height, 
+                            cudaTextureObject_t& texture, 
+                            cudaSurfaceObject_t& surface,
+                            cudaArray_t& array);
+// 创建纹理描述资源
+void createDefault2DTextureDesc(cudaTextureDesc &desc);
+
+// 释放纹理内存
+void releaseTexture(CudaTextureSurface &texture);
+
+
+// 深度图处理 滤波
+void BilateralFilterDepth(
+    cudaTextureObject_t raw_depth,
+    cudaSurfaceObject_t filter_depth,
+    const unsigned raw_width,
+    const unsigned raw_height,
+    const unsigned clip_width,
+    const unsigned clip_height,
+    cudaStream_t stream = 0
+);
+
+void createFloat4TextureSurface(const unsigned width, 
+                                const unsigned height,
+                                cudaTextureObject_t &texture,
+                                cudaSurfaceObject_t &surface,
+                                cudaArray_t &array
+                                );
+
+// 计算顶点坐标
+void createVertexMap(
+    cudaTextureObject_t depth_texture,
+    Intrinsic intrinsic,
+    const unsigned width,
+    const unsigned height,
+    cudaSurfaceObject_t vertex_surface,
+    cudaStream_t stream = 0
+);
+
+// 计算法向量
+void createNormalMap(
+    cudaTextureObject_t vertex_map,
+    const unsigned width,
+    const unsigned height,
+    cudaSurfaceObject_t normal_map,
+    cudaStream_t stream = 0
+);
+
+// 收集有效数据
+void CollectValidData(
+    cudaTextureObject_t vertex_map,
+    cudaTextureObject_t normal_map,
+    const unsigned width,
+    const unsigned height,
+    DeviceArray<Surfel>& valid_surfel,
+    int& surfel_count,
+    cudaStream_t stream = 0
+);
diff --git a/embedded_deformation/include/imageProcessor/common_types.hpp b/embedded_deformation/include/imageProcessor/common_types.hpp
new file mode 100644
index 0000000..3204f96
--- /dev/null
+++ b/embedded_deformation/include/imageProcessor/common_types.hpp
@@ -0,0 +1,80 @@
+#pragma once
+#include <cuda_runtime.h>
+#include <Eigen/Eigen>
+
+using Matrix3f = Eigen::Matrix3f;
+using Vector3f = Eigen::Vector3f;
+using Matrix4f = Eigen::Matrix4f;
+using Vector4f = Eigen::Vector4f;
+using Matrix6f = Eigen::Matrix<float, 6, 6>;
+using Vector6f = Eigen::Matrix<float, 6, 1>;
+using MatrixXf = Eigen::MatrixXf;
+using VectorXf = Eigen::VectorXf;
+using Isometry3f = Eigen::Isometry3f;
+
+struct vertex_normal_maps {
+    cudaTextureObject_t vertex_map;
+    cudaTextureObject_t normal_map;
+};
+
+// 相机内参
+struct Intrinsic
+{   
+    __host__ __device__ Intrinsic() 
+		: principal_x(0), principal_y(0), focal_x(0), focal_y(0) 
+        {}
+    
+    __host__ __device__ Intrinsic(
+				const float focal_x_, const float focal_y_,
+				const float principal_x_, const float principal_y_
+		) : principal_x(principal_x_), principal_y(principal_y_),
+			focal_x(focal_x_), focal_y(focal_y_) {}
+
+    //Cast to float4
+    __host__ operator float4() {
+        return make_float4(principal_x, principal_y, focal_x, focal_y);
+    }
+
+    float principal_x, principal_y;
+    float focal_x, focal_y;
+};
+
+// 纹理和表面结构
+struct CudaTextureSurface
+{
+    cudaArray_t array;
+    cudaTextureObject_t texture;
+    cudaSurfaceObject_t surface;
+};
+
+struct PixelCoordinate {
+		unsigned row;
+		unsigned col;
+		__host__ __device__ PixelCoordinate(): row(0), col(0) {}
+		__host__ __device__ PixelCoordinate(const unsigned row_, const unsigned col_) 
+		: row(row_), col(col_) {}
+
+		__host__ __device__ const unsigned& x() const { return col; }
+		__host__ __device__ const unsigned& y() const { return row; }
+		__host__ __device__ unsigned& x() { return col; }
+		__host__ __device__ unsigned& y() { return row; }
+	};
+
+// 面元数据
+struct Surfel {
+    PixelCoordinate pixel_coord;
+    float4 vertex;
+    float4 normal;
+    float4 color;
+};
+
+
+struct correspondence {
+    // PixelCoordinate source_index;
+    // PixelCoordinate target_index;
+    float4 src_vertex;
+    float4 src_normal;
+    float4 tag_vertex;
+    float4 tag_normal;
+};
+
diff --git a/embedded_deformation/include/imageProcessor/common_utils.h b/embedded_deformation/include/imageProcessor/common_utils.h
new file mode 100644
index 0000000..bcd333a
--- /dev/null
+++ b/embedded_deformation/include/imageProcessor/common_utils.h
@@ -0,0 +1,50 @@
+#pragma once
+
+#include "imageProcessor/common_types.hpp"
+#include <cuda_texture_types.h>
+#include <exception>
+#include <cuda.h>
+
+/**
+ * \brief The general swap operation on device.
+ */
+template <typename T>
+__host__ __device__ __forceinline__ void swap(T& a, T& b) noexcept
+{
+    T c(a); a = b; b = c;
+}
+
+//Texture fetching for 1D array: These names are supposed to be more clear
+#if defined(__CUDA_ARCH__)
+template<typename T>
+__device__ __forceinline__ T fetch1DLinear(cudaTextureObject_t texObj, int x) {
+    return tex1Dfetch<T>(texObj, x);
+}
+
+template<typename T>
+__device__ __forceinline__ T fetch1DArray(cudaTextureObject_t texObj, float x) {
+    return tex1D<T>(texObj, x);
+}
+#else
+template<typename T>
+__host__ __forceinline__ T fetch1DLinear(cudaTextureObject_t texObj, int x) {
+    throw new std::runtime_error("The texture object can only be accessed on device! ");
+}
+
+template<typename T>
+__host__ __forceinline__ T fetch1DArray(cudaTextureObject_t texObj, float x) {
+    throw new std::runtime_error("The texture object can only be accessed on device! ");
+}
+#endif
+
+
+/**
+ * \brief Initialize the context and driver api for cuda operations
+ */
+CUcontext initCudaContext(int selected_device = 0);
+
+
+/**
+ * \brief Clear the cuda context at the end of program
+ */
+void destroyCudaContext(CUcontext context);
diff --git a/embedded_deformation/include/imageProcessor/fetchInterface.h b/embedded_deformation/include/imageProcessor/fetchInterface.h
new file mode 100644
index 0000000..328a134
--- /dev/null
+++ b/embedded_deformation/include/imageProcessor/fetchInterface.h
@@ -0,0 +1,40 @@
+#pragma once
+#include <opencv2/opencv.hpp>
+#include <memory>
+#include <boost/filesystem/path.hpp>
+#include <vector>
+#include <mutex>
+
+namespace fs = boost::filesystem;
+using path = boost::filesystem::path;
+
+class FetchInterface
+{
+public:
+    
+    //Default contruct and de-construct
+    FetchInterface(const std::string data_dir, std::string extension = ".png", bool force_no_masks = false);
+    ~FetchInterface() = default;
+
+    //Buffer may be maintained outside fetch object for thread safety
+    void FetchDepthImage(size_t frame_idx, cv::Mat& depth_img);                  // 虚函数
+
+    //Should be rgb, in CV_8UC3 format
+    void FetchRGBImage(size_t frame_idx, cv::Mat& rgb_img);
+private:
+    static int GetFrameNumber(const path& filename);
+    static bool HasSubstringFromSet(const std::string& string, const std::string* set, int set_size);
+    static bool FilenameIndicatesDepthImage(const path& filename, const std::string& valid_extension);
+    static bool FilenameIndicatesRGBImage(const path& filename, const std::string& valid_extension);
+    static bool FilenameIndicatesMaskImage(const path& filename, const std::string& valid_extension);
+
+    std::vector<path> m_rgb_image_paths;
+    std::vector<path> m_depth_image_paths;
+    std::vector<path> m_mask_image_paths;
+
+    size_t m_mask_buffer_ix;
+    cv::Mat m_mask_image_buffer;
+    bool m_use_masks;
+    std::mutex mask_mutex;
+};
+
diff --git a/embedded_deformation/include/imageProcessor/global_configs.h b/embedded_deformation/include/imageProcessor/global_configs.h
new file mode 100644
index 0000000..6fbb254
--- /dev/null
+++ b/embedded_deformation/include/imageProcessor/global_configs.h
@@ -0,0 +1,5 @@
+#pragma once
+
+#ifndef boundary_clip
+#define boundary_clip 10
+#endif
diff --git a/embedded_deformation/include/imageProcessor/imageProcessor.h b/embedded_deformation/include/imageProcessor/imageProcessor.h
new file mode 100644
index 0000000..e94511b
--- /dev/null
+++ b/embedded_deformation/include/imageProcessor/imageProcessor.h
@@ -0,0 +1,148 @@
+#pragma once
+#include <opencv2/opencv.hpp>
+#include "common_types.hpp"
+#include "common.h"
+#include "fetchInterface.h"
+#include "polyscope/polyscope.h"
+#include "polyscope/point_cloud.h"
+#include "containers/DeviceBufferArray.h"
+
+using namespace cv;
+using namespace CUDA;
+
+class imageProcessor
+{
+public:
+    imageProcessor( Intrinsic intrinsic, 
+                    const unsigned width, 
+                    const unsigned height, 
+                    int start_frame,
+                    std::shared_ptr<FetchInterface> image_fetcher
+                    );
+    ~imageProcessor();
+
+private:
+    // 从主机读取的图像
+    cv::Mat m_depth_image, m_depth_image_prev;
+    cv::Mat m_rgb_image, m_rgb_image_prev;
+    
+    // 相机内参 
+    Intrinsic m_raw_intrinsic;
+    // 修剪后的相机内参
+    Intrinsic m_clip_intrinsic;
+
+    // 批量图像读取对象
+    std::shared_ptr<FetchInterface> m_image_fetcher;
+
+    // 原始图像的宽和高
+    const unsigned m_raw_width;
+    const unsigned m_raw_height;
+    // 修剪后图像的宽和高
+    const unsigned m_clip_width;
+    const unsigned m_clip_height;
+
+    // 锁页内存 从主机到设备数据传输的中转站 
+    // 它提供了更快的主机到设备的数据传输
+    void* m_depth_buffer_pagelock;
+    void* m_depth_prev_buffer_pagelock;
+    void* m_rgb_buffer_pagelock;
+    void* m_rgb_prev_buffer_pagelock;
+
+    // 纹理和表面
+    // 用于存储原始深度图 
+    CudaTextureSurface depth_texture_surface_raw;
+    CudaTextureSurface depth_texture_surface_raw_prev;
+    // 用于存储滤波后的深度图
+    CudaTextureSurface depth_texture_surface_filter;
+    CudaTextureSurface depth_texture_surface_filter_prev;
+    // 用于存储顶点图
+    CudaTextureSurface vertex_texture_surface;
+    CudaTextureSurface vertex_texture_surface_prev;
+    // 用于存储法向量图
+    CudaTextureSurface normal_texture_surface;
+    CudaTextureSurface normal_texture_surface_prev;
+    // surfel
+    DeviceBufferArray<Surfel> m_surfels;
+    DeviceBufferArray<Surfel> m_surfels_prev;
+
+    // 用于下载设备端的数据
+    unsigned short* h_depthData;
+    float4* h_vertexData;
+    float4* h_vertexData_prev;
+    float4* h_normalData;
+    float4* h_normalData_prev;
+
+    // 当前图像的索引
+    int m_frame_idx;
+
+    bool no_prev_frame = false;
+
+public:
+
+    // 分配用于读取图像的内存
+    void allocateFetchBuffer();
+    // 释放用于读取图像的内存
+    void releaseFetchBuffer();
+    // 分配存储深度图的纹理和表面的内存
+    void allocateDepthTexture();
+    // 释放存储深度图的纹理和表面的内存
+    void releaseDepthTexture();   
+    // 分配存储顶点图的纹理和表面的内存
+    void allocateVertexTexture();
+    // 释放存储顶点图的纹理和表面的内存
+    void releaseVertexTexture();
+    // 分配存储法向量图的纹理和表面的内存
+    void allocateNormalTexture();
+    // 释放存储法向量图的纹理和表面的内存
+    void releaseNormalTexture();
+    // 分配存储Surfel的内存
+    void allocateSurfelBuffer();
+
+    // 读取图像
+    void FetchFrame(size_t frame_idx);
+    // 读取深度图
+    void FetchDepthImage(size_t frame_idx);
+    void FetchDepthPrevImage(size_t frame_idx);
+    // 读取彩色图
+    void FetchRGBImage(size_t frame_idx);
+    void FetchRGBPrevImage(size_t frame_idx);
+
+    // 将深度图上传到设备
+    void UploadDepthImage(cudaStream_t stream = 0);
+    // 深度图滤波 这里滤掉了>1000的深度值
+    void FilterDepthImage(cudaStream_t stream = 0);
+    // 构建顶点坐标，需要根据数据集设置scale_factor
+    void BuildVertexMap(cudaStream_t stream = 0);
+    // 根据顶点计算法向量
+    void BuildNormalMap(cudaStream_t stream = 0);
+
+    // 将设备上的数据下载到主机
+    void DownloadDepthData(cudaStream_t stream = 0);
+    void DownloadVertexNormalData(cudaStream_t stream = 0);
+
+    // 收集有效的surfel数据 包括Vertx和Normal
+    void CollectValidSurfelData(cudaStream_t stream = 0);
+
+    // 接口函数
+    vertex_normal_maps getVertexNormalMaps() {return {vertex_texture_surface.texture, normal_texture_surface.texture};}
+    vertex_normal_maps getVertexNormalMapsPrev() {return {vertex_texture_surface_prev.texture, normal_texture_surface_prev.texture};}
+    unsigned clip_width() const { return m_clip_width; }
+    unsigned clip_height() const { return m_clip_height; }
+    Intrinsic clip_intrinsic() const { return m_clip_intrinsic; }
+    float4* getVertexData() const { return h_vertexData; }
+    std::vector<Surfel> getSurfelData() const { DeviceArrayView<Surfel> surfel_view = m_surfels.ArrayView();
+                                                std::vector<Surfel> h_surfels;
+                                                surfel_view.Download(h_surfels);
+                                                return h_surfels;
+                                                }
+    std::vector<Surfel> getSurfelDataPrev() const { DeviceArrayView<Surfel> surfel_view = m_surfels_prev.ArrayView();
+                                                std::vector<Surfel> h_surfels;
+                                                surfel_view.Download(h_surfels);
+                                                return h_surfels;
+                                                }
+
+    // 测试函数
+    void test();
+
+
+};
\ No newline at end of file
diff --git a/embedded_deformation/include/imageProcessor/logging.h b/embedded_deformation/include/imageProcessor/logging.h
new file mode 100644
index 0000000..4d43a1d
--- /dev/null
+++ b/embedded_deformation/include/imageProcessor/logging.h
@@ -0,0 +1,115 @@
+#pragma once
+
+#include <cstdio>
+#include <cstdlib>
+#include <string>
+#include <vector>
+#include <stdexcept>
+#include <memory>
+#include <assert.h>
+#include <iostream>
+#include <sstream>
+#include <ctime>
+
+
+struct LogCheckError {
+    LogCheckError() : str(nullptr) {}
+    explicit LogCheckError(const std::string& str_p) : str(new std::string(str_p)) {}
+    ~LogCheckError() {
+        if(str != nullptr) delete str;
+    }
+    //Type conversion
+    operator bool() { return str != nullptr; }
+    //The error string
+    std::string* str;
+};
+
+#define DEFINE_CHECK_FUNC(name, op)                      \
+template <typename X, typename Y>                               \
+inline LogCheckError LogCheck##name(const X& x, const Y& y) {   \
+    if (x op y) return LogCheckError();                         \
+    std::ostringstream os;                                      \
+    os << " (" << x << " vs. " << y << ") ";                    \
+    return LogCheckError(os.str());                             \
+}                                                               \
+inline LogCheckError LogCheck##name(int x, int y) {             \
+    return LogCheck##name<int, int>(x, y);                      \
+}
+DEFINE_CHECK_FUNC(_LT, <)
+DEFINE_CHECK_FUNC(_GT, >)
+DEFINE_CHECK_FUNC(_LE, <=)
+DEFINE_CHECK_FUNC(_GE, >=)
+DEFINE_CHECK_FUNC(_EQ, ==)
+DEFINE_CHECK_FUNC(_NE, !=)
+
+
+
+//Always on checking
+#define CHECK(x)                                         \
+if(!(x))                                                        \
+    LogMessageFatal(__FILE__, __LINE__).stream()    \
+    << "Check failed: " #x << " "
+
+#define SURFELWARP_CHECK_BINARY_OP(name, op, x, y)                         \
+if(LogCheckError err = LogCheck##name(x, y))   \
+    LogMessageFatal(__FILE__, __LINE__).stream()           \
+    << "Check failed: " << #x " " #op " " #y << *(err.str)
+
+#define	CHECK_LT(x, y) CHECK_BINARY_OP(_LT, < , x, y)
+#define	CHECK_GT(x, y) CHECK_BINARY_OP(_GT, > , x, y)
+#define	CHECK_LE(x, y) CHECK_BINARY_OP(_LE, <=, x, y)
+#define	CHECK_GE(x, y) CHECK_BINARY_OP(_GE, >=, x, y)
+#define	CHECK_EQ(x, y) CHECK_BINARY_OP(_EQ, ==, x, y)
+#define	CHECK_NE(x, y) CHECK_BINARY_OP(_NE, !=, x, y)
+
+//The log type for later use
+#define LOG_INFO LogMessage(__FILE__, __LINE__)
+#define LOG_ERROR LOG_INFO
+#define LOG_WARNING LOG_INFO
+#define LOG_FATAL LogMessageFatal(__FILE__, __LINE__)
+#define LOG_BEFORE_THROW LogMessage().stream()
+
+//For different severity
+#define LOG(severity) LOG_##severity.stream()
+
+// The log message
+class LogMessage {
+public:
+    //Constructors
+    LogMessage() : log_stream_(std::cout) {}
+    LogMessage(const char* file, int line) : log_stream_(std::cout) {
+        log_stream_ << file << ":" << line << ": ";
+    }
+    LogMessage(const LogMessage&) = delete;
+    LogMessage& operator=(const LogMessage&) = delete;
+    
+    //Another line
+    ~LogMessage() { log_stream_ << "\n"; }
+    
+    std::ostream& stream() { return log_stream_; }
+protected:
+    std::ostream& log_stream_;
+};
+
+class LogMessageFatal {
+public:
+    LogMessageFatal(const char* file, int line) {
+        log_stream_ << file << ":" << line << ": ";
+    }
+    
+    //No copy/assign
+    LogMessageFatal(const LogMessageFatal&) = delete;
+    LogMessageFatal& operator=(LogMessageFatal&) = delete;
+    
+    //Die the whole system
+    ~LogMessageFatal() {
+        LOG_BEFORE_THROW << log_stream_.str();
+        throw new std::runtime_error(log_stream_.str());
+    }
+    
+    //The output string stream
+    std::ostringstream& stream() { return log_stream_; }
+protected:
+    std::ostringstream log_stream_;
+};
+	
\ No newline at end of file
diff --git a/embedded_deformation/include/imageProcessor/safe_call.hpp b/embedded_deformation/include/imageProcessor/safe_call.hpp
new file mode 100644
index 0000000..73449d7
--- /dev/null
+++ b/embedded_deformation/include/imageProcessor/safe_call.hpp
@@ -0,0 +1,69 @@
+#ifndef __SAFE_CALL_
+#define __SAFE_CALL_
+
+#include "cuda_runtime.h"
+#include <iostream>
+#include <stdlib.h>
+#include <cublas_v2.h>
+#include <cstdio>
+#include <exception>
+#include <stdexcept>
+#include <cuda.h>
+
+#if defined(__GNUC__)
+    #define cudaSafeCall(expr)  ___cudaSafeCall(expr, __FILE__, __LINE__, __func__)
+#else /* defined(__CUDACC__) || defined(__MSVC__) */
+    #define cudaSafeCall(expr)  ___cudaSafeCall(expr, __FILE__, __LINE__)    
+#endif
+
+
+static inline void ___cudaSafeCall(cudaError_t err, const char *file, const int line, const char *func = "")
+{
+    if (cudaSuccess != err){
+        std::cout << "Error: " << err <<"\t" << file << ":" << line << std::endl;
+        printf("%s",cudaGetErrorString(err));
+        std::cout<<std::endl;
+        exit(0);
+    }
+}
+
+static inline int divUp(int total, int grain) 
+{ 
+    return (total + grain - 1) / grain; 
+}
+
+#endif 
+
+#ifndef cublasSafeCall
+#define cublasSafeCall(err)  __cublasSafeCall(err, __FILE__, __LINE__)
+static inline void __cublasSafeCall(cublasStatus_t err, const char *file, const int line)
+{
+    if(CUBLAS_STATUS_SUCCESS != err) {
+        fprintf(stderr, "CUBLAS error in file '%s', line %d\n \nerror %d \nterminating!\n",__FILE__, __LINE__, err);
+        cudaDeviceReset();
+        std::exit(1);
+    }
+}
+#endif
+
+#ifndef cuSafeCall
+#define cuSafeCall(err) __cuSafeCall(err, __FILE__, __LINE__)
+static inline void __cuSafeCall(CUresult err, const char* file, const int line) {
+    if(err != CUDA_SUCCESS) {
+        //Query the name and string of the error
+        const char* error_name;
+        cuGetErrorName(err, &error_name);
+        const char* error_string;
+        cuGetErrorString(err, &error_string);
+        fprintf(stderr, "CUDA driver error %s: %s in the line %d of file %s \n", error_name, error_string, line, file);
+        cudaDeviceReset();
+        std::exit(1);
+    }
+}
+	
+#endif
+
+
+
+
+
diff --git a/embedded_deformation/include/math/Quaternion.hpp b/embedded_deformation/include/math/Quaternion.hpp
new file mode 100644
index 0000000..d0c87b1
--- /dev/null
+++ b/embedded_deformation/include/math/Quaternion.hpp
@@ -0,0 +1,139 @@
+#pragma once
+
+#include "imageProcessor/common_types.hpp"
+#include "math/device_mat.h"
+#include "math/vector_ops.hpp"
+
+/*
+    * Quaternion struct for cuda access
+    * */
+struct Quaternion
+{
+    __host__ __device__ Quaternion() {}
+    __host__ __device__ Quaternion(float _w, float _x, float _y, float _z) : q0(make_float4(_x, _y, _z, _w)) {}
+    __host__ __device__ Quaternion(const float4 &_q) : q0(_q) {}
+    __host__ __device__ Quaternion(const mat33 &_rot)
+    {
+        float tr = _rot.m00() + _rot.m11() + _rot.m22();
+        if (tr > 0) {
+            float s = sqrtf(tr + 1.0f) * 2;
+            q0.w = s*0.25f;
+            q0.x = (_rot.m21() - _rot.m12()) / s;
+            q0.y = (_rot.m02() - _rot.m20()) / s;
+            q0.z = (_rot.m10() - _rot.m01()) / s;
+        }
+        else if ((_rot.m00() > _rot.m11()) && (_rot.m00() > _rot.m22())) {
+            float s = sqrtf(1.0f + _rot.m00() - _rot.m11() - _rot.m22()) * 2;
+            q0.w = (_rot.m21() - _rot.m12()) / s;
+            q0.x = 0.25f*s;
+            q0.y = (_rot.m01() + _rot.m10()) / s;
+            q0.z = (_rot.m02() + _rot.m20()) / s;
+        }
+        else if (_rot.m11() > _rot.m22()) {
+            float s = sqrtf(1.0f + _rot.m11() - _rot.m00() - _rot.m22()) * 2;
+            q0.w = (_rot.m02() - _rot.m20()) / s;
+            q0.x = (_rot.m01() + _rot.m10()) / s;
+            q0.y = 0.25f*s;
+            q0.z = (_rot.m12() + _rot.m21()) / s;
+        }
+        else {
+            float s = sqrtf(1.0f + _rot.m22() - _rot.m00() - _rot.m11()) * 2;
+            q0.w = (_rot.m10() - _rot.m01()) / s;
+            q0.x = (_rot.m02() + _rot.m20()) / s;
+            q0.y = (_rot.m12() + _rot.m21()) / s;
+            q0.z = 0.25f*s;
+        }
+    }
+
+    __host__ __device__ float& x() { return q0.x; }
+    __host__ __device__ float& y() { return q0.y; }
+    __host__ __device__ float& z() { return q0.z; }
+    __host__ __device__ float& w() { return q0.w; }
+
+    __host__ __device__ const float& x() const { return q0.x; }
+    __host__ __device__ const float& y() const { return q0.y; }
+    __host__ __device__ const float& z() const { return q0.z; }
+    __host__ __device__ const float& w() const { return q0.w; }
+
+    __host__ __device__ Quaternion conjugate() const { return Quaternion(q0.w, -q0.x, -q0.y, -q0.z); }
+    __host__ __device__ float square_norm() const { return q0.w*q0.w + q0.x*q0.x + q0.y*q0.y + q0.z*q0.z; }
+    __host__ __device__ float norm() const { return sqrtf(square_norm()); }
+    __host__ __device__ float norm_inversed() const { return ::norm_inversed(q0); }
+    __host__ __device__ float dot(const Quaternion &_quat) const { return q0.w*_quat.w() + q0.x*_quat.x() + q0.y*_quat.y() + q0.z*_quat.z(); }
+    __host__ __device__ void normalize() { ::normalize(q0); }
+    __host__ __device__ Quaternion normalized() const { Quaternion q(*this); q.normalize(); return q; }
+
+    __host__ __device__ mat33 matrix() const
+    {
+        /*normalize quaternion before converting to so3 matrix*/
+        Quaternion q(*this);
+        q.normalize();
+    
+        mat33 rot;
+        rot.m00() = 1 - 2 * q.y()*q.y() - 2 * q.z()*q.z();
+        rot.m01() = 2 * q.x()*q.y() - 2 * q.z()*q.w();
+        rot.m02() = 2 * q.x()*q.z() + 2 * q.y()*q.w();
+        rot.m10() = 2 * q.x()*q.y() + 2 * q.z()*q.w();
+        rot.m11() = 1 - 2 * q.x()*q.x() - 2 * q.z()*q.z();
+        rot.m12() = 2 * q.y()*q.z() - 2 * q.x()*q.w();
+        rot.m20() = 2 * q.x()*q.z() - 2 * q.y()*q.w();
+        rot.m21() = 2 * q.y()*q.z() + 2 * q.x()*q.w();
+        rot.m22() = 1 - 2 * q.x()*q.x() - 2 * q.y()*q.y();
+        return rot;
+    }
+    
+    __host__ __device__ mat33 rotation_matrix(bool normalize) {
+        if(normalize) this->normalize();
+        mat33 rot;
+        rot.m00() = 1 - 2 * y()*y() - 2 * z()*z();
+        rot.m01() = 2 * x()*y() - 2 * z()*w();
+        rot.m02() = 2 * x()*z() + 2 * y()*w();
+        rot.m10() = 2 * x()*y() + 2 * z()*w();
+        rot.m11() = 1 - 2 * x()*x() - 2 * z()*z();
+        rot.m12() = 2 * y()*z() - 2 * x()*w();
+        rot.m20() = 2 * x()*z() - 2 * y()*w();
+        rot.m21() = 2 * y()*z() + 2 * x()*w();
+        rot.m22() = 1 - 2 * x()*x() - 2 * y()*y();
+        return rot;
+    }
+
+    __host__ __device__ float3 vec() const { return make_float3(q0.x, q0.y, q0.z); }
+
+    float4 q0;
+};
+
+
+//Other operators
+__host__ __device__ __forceinline__ Quaternion operator+(const Quaternion &_left, const Quaternion &_right)
+{
+    return{ _left.w() + _right.w(), _left.x() + _right.x(), _left.y() + _right.y(), _left.z() + _right.z() };
+}
+
+__host__ __device__ __forceinline__ Quaternion operator-(const Quaternion &_left, const Quaternion &_right) {
+    return{ _left.w() - _right.w(), _left.x() - _right.x(), _left.y() - _right.y(), _left.z() - _right.z() };
+}
+
+__host__ __device__ __forceinline__ Quaternion operator*(float _scalar, const Quaternion &_quat)
+{
+    return{ _scalar*_quat.w(), _scalar*_quat.x(), _scalar*_quat.y(), _scalar*_quat.z() };
+}
+
+__host__ __device__ __forceinline__ Quaternion operator*(const Quaternion &_quat, float _scalar)
+{
+    return _scalar * _quat;
+}
+
+__host__ __device__ __forceinline__ Quaternion operator*(const Quaternion &_q0, const Quaternion &_q1)
+{
+    Quaternion q;
+    q.w() = _q0.w()*_q1.w() - _q0.x()*_q1.x() - _q0.y()*_q1.y() - _q0.z()*_q1.z();
+    q.x() = _q0.w()*_q1.x() + _q0.x()*_q1.w() + _q0.y()*_q1.z() - _q0.z()*_q1.y();
+    q.y() = _q0.w()*_q1.y() - _q0.x()*_q1.z() + _q0.y()*_q1.w() + _q0.z()*_q1.x();
+    q.z() = _q0.w()*_q1.z() + _q0.x()*_q1.y() - _q0.y()*_q1.x() + _q0.z()*_q1.w();
+    
+    return q;
+}
+
+__host__ __device__ __forceinline__ float dot(const Quaternion& q0, const Quaternion& q1) {
+    return dot(q0.q0, q1.q0);
+}
diff --git a/embedded_deformation/include/math/device_mat.h b/embedded_deformation/include/math/device_mat.h
new file mode 100644
index 0000000..e7810fb
--- /dev/null
+++ b/embedded_deformation/include/math/device_mat.h
@@ -0,0 +1,229 @@
+#pragma once
+#include <vector_functions.h>
+#include "imageProcessor/common_types.hpp"
+#include "math/vector_ops.hpp"
+
+
+//Simple matrix for access on device (and host)
+struct mat33 {
+    __host__ __device__ mat33() {}
+    __host__ __device__ mat33(const ::float3 &_a0, const ::float3 &_a1, const ::float3 &_a2) { cols[0] = _a0; cols[1] = _a1; cols[2] = _a2; }
+    __host__ __device__ mat33(const float *_data)
+    {
+        /*_data MUST have at least 9 float elements, ctor does not check range*/
+        cols[0] = make_float3(_data[0], _data[1], _data[2]);
+        cols[1] = make_float3(_data[3], _data[4], _data[5]);
+        cols[2] = make_float3(_data[6], _data[7], _data[8]);
+    }
+    __host__ mat33(const Eigen::Matrix3f& matrix3f);
+    __host__ mat33& operator=(const Eigen::Matrix3f& matrix3f);
+    
+    __host__ __device__ const float& m00() const { return cols[0].x; }
+    __host__ __device__ const float& m10() const { return cols[0].y; }
+    __host__ __device__ const float& m20() const { return cols[0].z; }
+    __host__ __device__ const float& m01() const { return cols[1].x; }
+    __host__ __device__ const float& m11() const { return cols[1].y; }
+    __host__ __device__ const float& m21() const { return cols[1].z; }
+    __host__ __device__ const float& m02() const { return cols[2].x; }
+    __host__ __device__ const float& m12() const { return cols[2].y; }
+    __host__ __device__ const float& m22() const { return cols[2].z; }
+    
+    __host__ __device__ float& m00() { return cols[0].x; }
+    __host__ __device__ float& m10() { return cols[0].y; }
+    __host__ __device__ float& m20() { return cols[0].z; }
+    __host__ __device__ float& m01() { return cols[1].x; }
+    __host__ __device__ float& m11() { return cols[1].y; }
+    __host__ __device__ float& m21() { return cols[1].z; }
+    __host__ __device__ float& m02() { return cols[2].x; }
+    __host__ __device__ float& m12() { return cols[2].y; }
+    __host__ __device__ float& m22() { return cols[2].z; }
+    
+    __host__ __device__ mat33 transpose() const
+    {
+        ::float3 row0 = make_float3(cols[0].x, cols[1].x, cols[2].x);
+        ::float3 row1 = make_float3(cols[0].y, cols[1].y, cols[2].y);
+        ::float3 row2 = make_float3(cols[0].z, cols[1].z, cols[2].z);
+        return mat33(row0, row1, row2);
+    }
+    
+    __host__ __device__ mat33 operator* (const mat33 &_mat) const
+    {
+        mat33 mat;
+        mat.m00() = m00()*_mat.m00() + m01()*_mat.m10() + m02()*_mat.m20();
+        mat.m01() = m00()*_mat.m01() + m01()*_mat.m11() + m02()*_mat.m21();
+        mat.m02() = m00()*_mat.m02() + m01()*_mat.m12() + m02()*_mat.m22();
+        mat.m10() = m10()*_mat.m00() + m11()*_mat.m10() + m12()*_mat.m20();
+        mat.m11() = m10()*_mat.m01() + m11()*_mat.m11() + m12()*_mat.m21();
+        mat.m12() = m10()*_mat.m02() + m11()*_mat.m12() + m12()*_mat.m22();
+        mat.m20() = m20()*_mat.m00() + m21()*_mat.m10() + m22()*_mat.m20();
+        mat.m21() = m20()*_mat.m01() + m21()*_mat.m11() + m22()*_mat.m21();
+        mat.m22() = m20()*_mat.m02() + m21()*_mat.m12() + m22()*_mat.m22();
+        return mat;
+    }
+    
+    __host__ __device__ mat33 operator+ (const mat33 &_mat) const
+    {
+        mat33 mat_sum;
+        mat_sum.m00() = m00() + _mat.m00();
+        mat_sum.m01() = m01() + _mat.m01();
+        mat_sum.m02() = m02() + _mat.m02();
+        
+        mat_sum.m10() = m10() + _mat.m10();
+        mat_sum.m11() = m11() + _mat.m11();
+        mat_sum.m12() = m12() + _mat.m12();
+        
+        mat_sum.m20() = m20() + _mat.m20();
+        mat_sum.m21() = m21() + _mat.m21();
+        mat_sum.m22() = m22() + _mat.m22();
+        
+        return mat_sum;
+    }
+    
+    __host__ __device__ mat33 operator- (const mat33 &_mat) const
+    {
+        mat33 mat_diff;
+        mat_diff.m00() = m00() - _mat.m00();
+        mat_diff.m01() = m01() - _mat.m01();
+        mat_diff.m02() = m02() - _mat.m02();
+        
+        mat_diff.m10() = m10() - _mat.m10();
+        mat_diff.m11() = m11() - _mat.m11();
+        mat_diff.m12() = m12() - _mat.m12();
+        
+        mat_diff.m20() = m20() - _mat.m20();
+        mat_diff.m21() = m21() - _mat.m21();
+        mat_diff.m22() = m22() - _mat.m22();
+        
+        return mat_diff;
+    }
+    
+    __host__ __device__ mat33 operator-() const
+    {
+        mat33 mat_neg;
+        mat_neg.m00() = -m00();
+        mat_neg.m01() = -m01();
+        mat_neg.m02() = -m02();
+        
+        mat_neg.m10() = -m10();
+        mat_neg.m11() = -m11();
+        mat_neg.m12() = -m12();
+        
+        mat_neg.m20() = -m20();
+        mat_neg.m21() = -m21();
+        mat_neg.m22() = -m22();
+        
+        return mat_neg;
+    }
+    
+    __host__ __device__ mat33& operator*= (const mat33 &_mat)
+    {
+        *this = *this * _mat;
+        return *this;
+    }
+    
+    __host__ __device__ ::float3 operator* (const ::float3 &_vec) const
+    {
+        const float x = m00()*_vec.x + m01()*_vec.y + m02()*_vec.z;
+        const float y = m10()*_vec.x + m11()*_vec.y + m12()*_vec.z;
+        const float z = m20()*_vec.x + m21()*_vec.y + m22()*_vec.z;
+        return make_float3(x, y, z);
+    }
+
+    //Just ignore the vec.w elements
+    __host__ __device__ ::float3 operator* (const float4 &_vec) const
+    {
+        const float x = m00()*_vec.x + m01()*_vec.y + m02()*_vec.z;
+        const float y = m10()*_vec.x + m11()*_vec.y + m12()*_vec.z;
+        const float z = m20()*_vec.x + m21()*_vec.y + m22()*_vec.z;
+        return make_float3(x, y, z);
+    }
+
+    //Conceptually, transpose this matrix and perform a dot product
+    __host__ __device__ ::float3 transpose_dot(const ::float3& _vec) const
+    {
+        const float x = m00()*_vec.x + m10()*_vec.y + m20()*_vec.z;
+        const float y = m01()*_vec.x + m11()*_vec.y + m21()*_vec.z;
+        const float z = m02()*_vec.x + m12()*_vec.y + m22()*_vec.z;
+        return make_float3(x, y, z);
+    }
+
+    __host__ __device__ ::float3 transpose_dot(const float4& _vec) const
+    {
+        const float x = m00()*_vec.x + m10()*_vec.y + m20()*_vec.z;
+        const float y = m01()*_vec.x + m11()*_vec.y + m21()*_vec.z;
+        const float z = m02()*_vec.x + m12()*_vec.y + m22()*_vec.z;
+        return make_float3(x, y, z);
+    }
+    
+    __host__ __device__ void set_identity()
+    {
+        cols[0] = make_float3(1, 0, 0);
+        cols[1] = make_float3(0, 1, 0);
+        cols[2] = make_float3(0, 0, 1);
+    }
+    
+    __host__ __device__ static mat33 identity()
+    {
+        mat33 idmat;
+        idmat.set_identity();
+        return idmat;
+    }
+    
+    ::float3 cols[3]; /*colume major*/
+};
+
+//Simple SE(3) matrix representation
+struct mat34 {
+    __host__ __device__ mat34() {}
+    __host__ __device__ mat34(const mat33 &_rot, const ::float3 &_trans) : rot(_rot), trans(_trans) {}
+    __host__ __device__ mat34(const ::float3& twist_rot, const ::float3& twist_trans);
+    __host__ __device__ static mat34 identity()
+    {
+        return mat34(mat33::identity(), make_float3(0, 0, 0));
+    }
+    __host__ mat34(const Isometry3f& se3);
+    __host__ mat34(const Matrix4f& matrix4f);
+
+    __host__ __device__ mat34 operator* (const mat34 &_right_se3) const
+    {
+        mat34 se3;
+        se3.rot = rot*_right_se3.rot;
+        se3.trans = (rot*_right_se3.trans) + trans;
+        return se3;
+    }
+
+    __host__ __device__ mat34& operator*= (const mat34 &_right_se3)
+    {
+        *this = *this * _right_se3;
+        return *this;
+    }
+    
+    __host__ __device__ __forceinline__ void set_identity() {
+        rot.set_identity();
+        trans.x = trans.y = trans.z = 0.0f;
+    }
+
+    __host__ __device__ __forceinline__ mat34 inverse() const {
+        mat34 inversed;
+        inversed.rot = rot.transpose();
+        inversed.trans = - (inversed.rot * trans);
+        return inversed;
+    }
+
+    __host__ __device__ __forceinline__ ::float3 apply_inversed_se3(const ::float3& vec) const {
+        return rot.transpose_dot(vec - trans);
+    }
+
+    //Just ignore the last elements
+    __host__ __device__ __forceinline__ ::float3 apply_inversed_se3(const float4& vec) const {
+        return rot.transpose_dot(make_float3(
+            vec.x - trans.x, 
+            vec.y - trans.y, 
+            vec.z - trans.z
+        ));
+    }
+
+    mat33 rot;
+    ::float3 trans;
+};
+	
\ No newline at end of file
diff --git a/embedded_deformation/include/math/eigen33.h b/embedded_deformation/include/math/eigen33.h
new file mode 100644
index 0000000..0f91c06
--- /dev/null
+++ b/embedded_deformation/include/math/eigen33.h
@@ -0,0 +1,76 @@
+#pragma once
+
+#include "imageProcessor/common_types.hpp"
+#include "device_mat.h"
+#include "numeric_limits.hpp"
+#include "vector_ops.hpp"
+
+
+/* The struct contains method to compute the eigen vector
+*  of 3x3 positive definite matrix, for normal computation
+*  Copy and modify from the pcl implementation
+*/
+struct eigen33 {
+private:
+	template<int Rows>
+	struct MiniMat
+	{
+		float3 data[Rows];
+		__host__ __device__ __forceinline__ float3& operator[](int i) { return data[i]; }
+		__host__ __device__ __forceinline__ const float3& operator[](int i) const { return data[i]; }
+	};
+	typedef MiniMat<3> Mat33;
+
+public:
+	//Compute the root of x^2 - b x + c = 0, assuming real roots
+	__host__ __device__ static void compute_root2(const float b, const float c, float3& roots);
+
+	//Compute the root of x^3 - c2*x^2 + c1*x - c0 = 0
+	__host__ __device__ static void compute_root3(const float c0, const float c1, const float c2, float3& roots);
+
+	//Constructor
+	__host__ __device__ eigen33(float* psd33) : psd_matrix33(psd33) {}
+
+	//Compute the eigen vectors
+	__host__ __device__ static float3 unit_orthogonal(const float3& src);
+	__host__ __device__ __forceinline__ void compute(Mat33& tmp, Mat33& vec_tmp, Mat33& evecs, float3& evals);
+	__host__ __device__ __forceinline__ void compute(float3& eigen_vec);
+	__host__ __device__ __forceinline__ void compute(float3& eigen_vec, float& eigen_value);
+
+private:
+	//The psd matrix to compute eigen vector, in the size of 6
+	float* psd_matrix33;
+
+	//For accessing
+	__host__ __device__  __forceinline__ float m00() const { return psd_matrix33[0]; }
+	__host__ __device__  __forceinline__ float m01() const { return psd_matrix33[1]; }
+	__host__ __device__  __forceinline__ float m02() const { return psd_matrix33[2]; }
+	__host__ __device__  __forceinline__ float m10() const { return psd_matrix33[1]; }
+	__host__ __device__  __forceinline__ float m11() const { return psd_matrix33[3]; }
+	__host__ __device__  __forceinline__ float m12() const { return psd_matrix33[4]; }
+	__host__ __device__  __forceinline__ float m20() const { return psd_matrix33[2]; }
+	__host__ __device__  __forceinline__ float m21() const { return psd_matrix33[4]; }
+	__host__ __device__  __forceinline__ float m22() const { return psd_matrix33[5]; }
+
+	__host__ __device__  __forceinline__ float3 row0() const { return make_float3(m00(), m01(), m02()); }
+	__host__ __device__  __forceinline__ float3 row1() const { return make_float3(m10(), m11(), m12()); }
+	__host__ __device__  __forceinline__ float3 row2() const { return make_float3(m20(), m21(), m22()); }
+
+	__host__ __device__ __forceinline__ static bool isMuchSmallerThan(float x, float y) {
+		const float prec_sqr = numeric_limits<float>::epsilon() * numeric_limits<float>::epsilon();
+		return x * x <= prec_sqr * y * y;
+	}
+
+	//The inverse sqrt function
+#if defined (__CUDACC__)
+	__host__ __device__ __forceinline__ static float inv_sqrt(float x) {
+		return rsqrtf(x);
+	}
+#else
+	__host__ __device__ __forceinline__ static float inv_sqrt(float x) {
+		return 1.0f / sqrtf(x);
+	}
+#endif
+};
+
+#include "math/eigen33.hpp"
\ No newline at end of file
diff --git a/embedded_deformation/include/math/eigen33.hpp b/embedded_deformation/include/math/eigen33.hpp
new file mode 100644
index 0000000..1f56296
--- /dev/null
+++ b/embedded_deformation/include/math/eigen33.hpp
@@ -0,0 +1,331 @@
+#pragma once
+#include "imageProcessor/common_utils.h"
+#include "math/eigen33.h"
+#include "math/vector_ops.hpp"
+
+__host__ __device__ __forceinline__ void eigen33::compute_root2(const float b, const float c, float3 & roots)
+{
+	roots.x = 0.0f; //Used in compute_root3
+	float d = b * b - 4.f * c;
+	if (d < 0.f) // no real roots!!!! THIS SHOULD NOT HAPPEN!
+		d = 0.f;
+
+	float sd = sqrtf(d);
+
+	roots.z = 0.5f * (b + sd);
+	roots.y = 0.5f * (b - sd);
+}
+
+
+__host__ __device__ __forceinline__ float3 eigen33::unit_orthogonal(const float3 & src)
+{
+	float3 perp;
+	/* Compute the crossed product of *this with a vector
+	*  that is not too close to being colinear to *this.
+	*/
+
+	/* unless the x and y coords are both close to zero, we can
+	* simply take ( -y, x, 0 ) and normalize it.
+	*/
+	if (!isMuchSmallerThan(src.x, src.z) || !isMuchSmallerThan(src.y, src.z))
+	{
+		float invnm = inv_sqrt(src.x*src.x + src.y*src.y);
+		perp.x = -src.y * invnm;
+		perp.y = src.x * invnm;
+		perp.z = 0.0f;
+	}
+	/* if both x and y are close to zero, then the vector is close
+	* to the z-axis, so it's far from colinear to the x-axis for instance.
+	* So we take the crossed product with (1,0,0) and normalize it.
+	*/
+	else
+	{
+		float invnm = inv_sqrt(src.z * src.z + src.y * src.y);
+		perp.x = 0.0f;
+		perp.y = -src.z * invnm;
+		perp.z = src.y * invnm;
+	}
+
+	return perp;
+}
+
+
+
+__host__ __device__ __forceinline__ void eigen33::compute_root3(const float c0, const float c1, const float c2, float3 & roots)
+{
+	if (fabsf(c0) < numeric_limits<float>::epsilon()) {
+		compute_root2(c2, c1, roots);
+	}
+	else {
+		const float s_inv3 = 1.f / 3.f;
+		const float s_sqrt3 = sqrtf(3.f);
+		// Construct the parameters used in classifying the roots of the equation
+		// and in solving the equation for the roots in closed form.
+		float c2_over_3 = c2 * s_inv3;
+		float a_over_3 = (c1 - c2 * c2_over_3) * s_inv3;
+		if (a_over_3 > 0.f)
+			a_over_3 = 0.f;
+		float half_b = 0.5f * (c0 + c2_over_3 * (2.f * c2_over_3 * c2_over_3 - c1));
+		float q = half_b * half_b + a_over_3 * a_over_3 * a_over_3;
+		if (q > 0.f)
+			q = 0.f;
+
+		//Compute the eigenvalues by solving for the roots of the polynomial
+		float rho = sqrtf(-a_over_3);
+		float theta = atan2f(sqrtf(-q), half_b)*s_inv3;
+
+		//Using intrinsic here
+		float cos_theta, sin_theta;
+		cos_theta = cosf(theta);
+		sin_theta = sinf(theta);
+		//Compute the roots
+		roots.x = c2_over_3 + 2.f * rho * cos_theta;
+		roots.y = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);
+		roots.z = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);
+
+		//Sort the root according to their values
+		if (roots.x >= roots.y)
+			swap(roots.x, roots.y);
+
+		if (roots.y >= roots.z) {
+			swap(roots.y, roots.z);
+			if (roots.x >= roots.y)
+				swap(roots.x, roots.y);
+		}
+
+		//eigenvalues for symetric positive semi-definite matrix can not be negative! Set it to 0
+		if (roots.x <= 0.0f)
+			compute_root2(c2, c1, roots);
+	}
+}
+
+__host__ __device__ __forceinline__ void eigen33::compute(Mat33 & tmp, Mat33 & vec_tmp, Mat33 & evecs, float3 & evals)
+{
+	// Scale the matrix so its entries are in [-1,1].  The scaling is applied
+	// only when at least one matrix entry has magnitude larger than 1.
+	float max01 = fmaxf(fabsf(psd_matrix33[0]), fabsf(psd_matrix33[1]));
+	float max23 = fmaxf(fabsf(psd_matrix33[2]), fabsf(psd_matrix33[3]));
+	float max45 = fmaxf(fabsf(psd_matrix33[4]), fabsf(psd_matrix33[5]));
+	float m0123 = fmaxf(max01, max23);
+	float scale = fmaxf(max45, m0123);
+
+	if (scale <= numeric_limits<float>::min_positive())
+		scale = 1.f;
+
+	psd_matrix33[0] /= scale;
+	psd_matrix33[1] /= scale;
+	psd_matrix33[2] /= scale;
+	psd_matrix33[3] /= scale;
+	psd_matrix33[4] /= scale;
+	psd_matrix33[5] /= scale;
+
+	// The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0.  The
+	// eigenvalues are the roots to this equation, all guaranteed to be
+	// real-valued, because the matrix is symmetric.
+	float c0 = m00() * m11() * m22()
+		+ 2.f * m01() * m02() * m12()
+		- m00() * m12() * m12()
+		- m11() * m02() * m02()
+		- m22() * m01() * m01();
+	float c1 = m00() * m11() -
+		m01() * m01() +
+		m00() * m22() -
+		m02() * m02() +
+		m11() * m22() -
+		m12() * m12();
+	float c2 = m00() + m11() + m22();
+
+	compute_root3(c0, c1, c2, evals);
+
+	if (evals.z - evals.x <= numeric_limits<float>::epsilon())
+	{
+		evecs[0] = make_float3(1.f, 0.f, 0.f);
+		evecs[1] = make_float3(0.f, 1.f, 0.f);
+		evecs[2] = make_float3(0.f, 0.f, 1.f);
+	}
+	else if (evals.y - evals.x <= numeric_limits<float>::epsilon())
+	{
+		// first and second equal                
+		tmp[0] = row0();  tmp[1] = row1();  tmp[2] = row2();
+		tmp[0].x -= evals.z; tmp[1].y -= evals.z; tmp[2].z -= evals.z;
+
+		vec_tmp[0] = cross(tmp[0], tmp[1]);
+		vec_tmp[1] = cross(tmp[0], tmp[2]);
+		vec_tmp[2] = cross(tmp[1], tmp[2]);
+
+		float len1 = dot(vec_tmp[0], vec_tmp[0]);
+		float len2 = dot(vec_tmp[1], vec_tmp[1]);
+		float len3 = dot(vec_tmp[2], vec_tmp[2]);
+
+		if (len1 >= len2 && len1 >= len3)
+		{
+			evecs[2] = vec_tmp[0] * inv_sqrt(len1);
+		}
+		else if (len2 >= len1 && len2 >= len3)
+		{
+			evecs[2] = vec_tmp[1] * inv_sqrt(len2);
+		}
+		else
+		{
+			evecs[2] = vec_tmp[2] * inv_sqrt(len3);
+		}
+
+		evecs[1] = unit_orthogonal(evecs[2]);
+		evecs[0] = cross(evecs[1], evecs[2]);
+	}
+	else if (evals.z - evals.y <= numeric_limits<float>::epsilon())
+	{
+		// second and third equal                                    
+		tmp[0] = row0();  tmp[1] = row1();  tmp[2] = row2();
+		tmp[0].x -= evals.x; tmp[1].y -= evals.x; tmp[2].z -= evals.x;
+
+		vec_tmp[0] = cross(tmp[0], tmp[1]);
+		vec_tmp[1] = cross(tmp[0], tmp[2]);
+		vec_tmp[2] = cross(tmp[1], tmp[2]);
+
+		float len1 = dot(vec_tmp[0], vec_tmp[0]);
+		float len2 = dot(vec_tmp[1], vec_tmp[1]);
+		float len3 = dot(vec_tmp[2], vec_tmp[2]);
+
+		if (len1 >= len2 && len1 >= len3)
+		{
+			evecs[0] = vec_tmp[0] * inv_sqrt(len1);
+		}
+		else if (len2 >= len1 && len2 >= len3)
+		{
+			evecs[0] = vec_tmp[1] * inv_sqrt(len2);
+		}
+		else
+		{
+			evecs[0] = vec_tmp[2] * inv_sqrt(len3);
+		}
+
+		evecs[1] = unit_orthogonal(evecs[0]);
+		evecs[2] = cross(evecs[0], evecs[1]);
+	}
+	else
+	{
+
+		tmp[0] = row0();  tmp[1] = row1();  tmp[2] = row2();
+		tmp[0].x -= evals.z; tmp[1].y -= evals.z; tmp[2].z -= evals.z;
+
+		vec_tmp[0] = cross(tmp[0], tmp[1]);
+		vec_tmp[1] = cross(tmp[0], tmp[2]);
+		vec_tmp[2] = cross(tmp[1], tmp[2]);
+
+		float len1 = dot(vec_tmp[0], vec_tmp[0]);
+		float len2 = dot(vec_tmp[1], vec_tmp[1]);
+		float len3 = dot(vec_tmp[2], vec_tmp[2]);
+
+		float mmax[3];
+
+		unsigned int min_el = 2;
+		unsigned int max_el = 2;
+		if (len1 >= len2 && len1 >= len3)
+		{
+			mmax[2] = len1;
+			evecs[2] = vec_tmp[0] * inv_sqrt(len1);
+		}
+		else if (len2 >= len1 && len2 >= len3)
+		{
+			mmax[2] = len2;
+			evecs[2] = vec_tmp[1] * inv_sqrt(len2);
+		}
+		else
+		{
+			mmax[2] = len3;
+			evecs[2] = vec_tmp[2] * inv_sqrt(len3);
+		}
+
+		tmp[0] = row0();  tmp[1] = row1();  tmp[2] = row2();
+		tmp[0].x -= evals.y; tmp[1].y -= evals.y; tmp[2].z -= evals.y;
+
+		vec_tmp[0] = cross(tmp[0], tmp[1]);
+		vec_tmp[1] = cross(tmp[0], tmp[2]);
+		vec_tmp[2] = cross(tmp[1], tmp[2]);
+
+		len1 = dot(vec_tmp[0], vec_tmp[0]);
+		len2 = dot(vec_tmp[1], vec_tmp[1]);
+		len3 = dot(vec_tmp[2], vec_tmp[2]);
+
+		if (len1 >= len2 && len1 >= len3)
+		{
+			mmax[1] = len1;
+			evecs[1] = vec_tmp[0] * inv_sqrt(len1);
+			min_el = len1 <= mmax[min_el] ? 1 : min_el;
+			max_el = len1  > mmax[max_el] ? 1 : max_el;
+		}
+		else if (len2 >= len1 && len2 >= len3)
+		{
+			mmax[1] = len2;
+			evecs[1] = vec_tmp[1] * inv_sqrt(len2);
+			min_el = len2 <= mmax[min_el] ? 1 : min_el;
+			max_el = len2  > mmax[max_el] ? 1 : max_el;
+		}
+		else
+		{
+			mmax[1] = len3;
+			evecs[1] = vec_tmp[2] * inv_sqrt(len3);
+			min_el = len3 <= mmax[min_el] ? 1 : min_el;
+			max_el = len3 >  mmax[max_el] ? 1 : max_el;
+		}
+
+		tmp[0] = row0();  tmp[1] = row1();  tmp[2] = row2();
+		tmp[0].x -= evals.x; tmp[1].y -= evals.x; tmp[2].z -= evals.x;
+
+		vec_tmp[0] = cross(tmp[0], tmp[1]);
+		vec_tmp[1] = cross(tmp[0], tmp[2]);
+		vec_tmp[2] = cross(tmp[1], tmp[2]);
+
+		len1 = dot(vec_tmp[0], vec_tmp[0]);
+		len2 = dot(vec_tmp[1], vec_tmp[1]);
+		len3 = dot(vec_tmp[2], vec_tmp[2]);
+
+
+		if (len1 >= len2 && len1 >= len3)
+		{
+			mmax[0] = len1;
+			evecs[0] = vec_tmp[0] * inv_sqrt(len1);
+			min_el = len3 <= mmax[min_el] ? 0 : min_el;
+			max_el = len3  > mmax[max_el] ? 0 : max_el;
+		}
+		else if (len2 >= len1 && len2 >= len3)
+		{
+			mmax[0] = len2;
+			evecs[0] = vec_tmp[1] * inv_sqrt(len2);
+			min_el = len3 <= mmax[min_el] ? 0 : min_el;
+			max_el = len3  > mmax[max_el] ? 0 : max_el;
+		}
+		else
+		{
+			mmax[0] = len3;
+			evecs[0] = vec_tmp[2] * inv_sqrt(len3);
+			min_el = len3 <= mmax[min_el] ? 0 : min_el;
+			max_el = len3  > mmax[max_el] ? 0 : max_el;
+		}
+
+		unsigned mid_el = 3 - min_el - max_el;
+		evecs[min_el] = normalized(cross(evecs[(min_el + 1) % 3], evecs[(min_el + 2) % 3]));
+		evecs[mid_el] = normalized(cross(evecs[(mid_el + 1) % 3], evecs[(mid_el + 2) % 3]));
+	}
+	// Rescale back to the original size.
+	evals = evals * scale;
+}
+
+
+__host__ __device__ __forceinline__ void eigen33::compute(float3 & eigen_vec)
+{
+	Mat33 tmp, vec_tmp, evecs;
+	float3 evals;
+	compute(tmp, vec_tmp, evecs, evals);
+	eigen_vec = evecs[0];
+}
+
+__host__ __device__ __forceinline__ void eigen33::compute(float3 & eigen_vec, float& eigen_value)
+{
+	Mat33 tmp, vec_tmp, evecs;
+	float3 evals;
+	compute(tmp, vec_tmp, evecs, evals);
+	eigen_vec = evecs[0];
+	eigen_value = evals.x;
+}
\ No newline at end of file
diff --git a/embedded_deformation/include/math/eigen_device_transfer.h b/embedded_deformation/include/math/eigen_device_transfer.h
new file mode 100644
index 0000000..861e570
--- /dev/null
+++ b/embedded_deformation/include/math/eigen_device_transfer.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#include "imageProcessor/common_types.hpp"
+#include "math/device_mat.h"
+#include "math/Quaternion.hpp"
+
+//Transfer the device vector/matrix to Eigen
+Matrix3f toEigen(const mat33 &rhs);
+
+Matrix4f toEigen(const mat34 &rhs);
+
+Vector3f toEigen(const float3 &rhs);
+
+Vector4f toEigen(const float4 &rhs);
+
+//For basic device vector
+float3 fromEigen(const Vector3f &rhs);
+
+float4 fromEigen(const Vector4f &rhs);
+
+void fromEigen(const Isometry3f& se3, Quaternion& rotation, float3& translation);
\ No newline at end of file
diff --git a/embedded_deformation/include/math/numeric_limits.hpp b/embedded_deformation/include/math/numeric_limits.hpp
new file mode 100644
index 0000000..0d0474d
--- /dev/null
+++ b/embedded_deformation/include/math/numeric_limits.hpp
@@ -0,0 +1,58 @@
+#pragma once
+#include <vector_types.h>
+
+/* The struct for the range of numeric values
+*/
+template <typename T> struct numeric_limits;
+
+
+/* For float type, the host and device code might be different
+*/
+template <> struct numeric_limits<float> {
+#if defined (__CUDACC__)
+	__device__ __forceinline__ static float quiet_nan() {
+		return __int_as_float(0x7fffffff);
+	}
+#else
+	__host__ __device__ __forceinline__ static float quiet_nan() {
+		int value = 0x7fffffff;
+		return (*((float*)&(value)));
+	}
+#endif
+
+	__host__ __device__ __forceinline__ static float epsilon() {
+		return 1.192092896e-07f;
+	}
+
+	__host__ __device__ __forceinline__ static float min_positive() {
+		return 1.175494351e-38f;
+	}
+
+	__host__ __device__ __forceinline__ static float max() {
+		return 3.402823466e+38f;
+	}
+};
+
+/* For 16 bit signed short
+*/
+template <> struct numeric_limits<signed short> {
+	__host__ __device__ __forceinline__ static signed short min() {
+		return -32768;
+	}
+
+	__host__ __device__ __forceinline__ static signed short max() {
+		return 32767;
+	}
+};
+
+/* For 16 bit unsigned short type
+*/
+template <> struct numeric_limits<unsigned short> {
+	__host__ __device__ __forceinline__ static unsigned short min() {
+		return 0;
+	}
+
+	__host__ __device__ __forceinline__ static unsigned short max() {
+		return 0xffff;
+	}
+};
diff --git a/embedded_deformation/include/math/vector_ops.hpp b/embedded_deformation/include/math/vector_ops.hpp
new file mode 100644
index 0000000..dc3bbc0
--- /dev/null
+++ b/embedded_deformation/include/math/vector_ops.hpp
@@ -0,0 +1,265 @@
+#pragma once
+
+#include "imageProcessor/common_types.hpp"
+#include <vector_functions.h>
+#include <cmath>
+
+/* Compute the norm of a vector
+    */
+__host__ __device__ __forceinline__
+float squared_norm(const float4 &vec) {
+    return vec.x * vec.x + vec.y * vec.y + vec.z * vec.z + vec.w * vec.w;
+}
+__host__ __device__ __forceinline__
+    float squared_norm(const ::float3 &vec) {
+    return vec.x * vec.x + vec.y * vec.y + vec.z * vec.z;
+}
+__host__ __device__ __forceinline__
+float squared_norm(const ::float2& vec){
+    return vec.x * vec.x + vec.y * vec.y;
+}
+
+__host__ __device__ __forceinline__
+    float squared_norm_xyz(const float4 &vec) {
+    return vec.x * vec.x + vec.y * vec.y + vec.z * vec.z;
+}
+
+//The squared distance between two vector, only care xyz component
+__host__ __device__ __forceinline__
+float squared_distance(const ::float3& v3, const float4& v4) {
+    return (v3.x - v4.x) * (v3.x - v4.x) + (v3.y - v4.y) * (v3.y - v4.y) +(v3.z - v4.z) * (v3.z - v4.z);
+}
+
+__host__ __device__ __forceinline__
+float norm(const float4& vec) {
+    return sqrtf(squared_norm(vec));
+}
+__host__ __device__ __forceinline__
+    float norm(const ::float3& vec) {
+    return sqrtf(squared_norm(vec));
+}
+
+
+/* Compute the inversed normal of a vector
+*/
+#if defined(__CUDA_ARCH__)
+__device__ __forceinline__ float norm_inversed(const float4& vec) {
+    return rsqrt(squared_norm(vec));
+}
+__device__ __forceinline__ float norm_inversed(const ::float3& vec) {
+    return rsqrt(squared_norm(vec));
+}
+#else
+__host__ __forceinline__ float norm_inversed(const float4& vec) {
+    return 1.0f / sqrt(squared_norm(vec));
+}
+__host__ __forceinline__ float norm_inversed(const ::float3& vec) {
+    return 1.0f / sqrt(squared_norm(vec));
+}
+#endif
+
+#if defined(__CUDA_ARCH__)
+__device__ __forceinline__ void normalize(float4& vec) {
+    const float inv_vecnorm = rsqrtf(squared_norm(vec));
+    vec.x *= inv_vecnorm;
+    vec.y *= inv_vecnorm;
+    vec.z *= inv_vecnorm;
+    vec.w *= inv_vecnorm;
+}
+#else
+__host__ __forceinline__ void normalize(float4 &vec) {
+    const float inv_vecnorm = 1.0f / sqrtf(squared_norm(vec));
+    vec.x *= inv_vecnorm;
+    vec.y *= inv_vecnorm;
+    vec.z *= inv_vecnorm;
+    vec.w *= inv_vecnorm;
+}
+#endif
+
+
+/* Return the normlized vector while keeping the original copy
+*/
+__host__ __device__ __forceinline__ float4 normalized(const float4 &vec) {
+    const float inv_vecnorm = norm_inversed(vec);
+    const float4 normalized_vec = make_float4(
+        vec.x * inv_vecnorm, vec.y * inv_vecnorm, 
+        vec.z * inv_vecnorm, vec.w * inv_vecnorm
+    );
+    return normalized_vec;
+}
+
+__host__ __device__ __forceinline__ ::float3 normalized(const ::float3 &vec) {
+    const float inv_vecnorm = norm_inversed(vec);
+    const ::float3 normalized_vec = make_float3(
+        vec.x * inv_vecnorm, 
+        vec.y * inv_vecnorm,
+        vec.z * inv_vecnorm
+    );
+    return normalized_vec;
+}
+
+
+/* Check if the vertex or normal is zero
+*/
+__host__ __device__ __forceinline__ bool
+is_zero_vertex(const float4& v) {
+    return fabsf(v.x) < 1e-3 && fabsf(v.y) < 1e-3 && fabsf(v.z) < 1e-3;
+}
+__host__ __device__ __forceinline__ bool
+is_zero_vertex(const ::float3& v) {
+    return fabsf(v.x) < 1e-3 && fabsf(v.y) < 1e-3 && fabsf(v.z) < 1e-3;
+}
+
+
+/* Operator for add
+*/
+__host__ __device__ __forceinline__ ::float3 
+    operator+(const ::float3& vec, const float& scalar)
+{
+    return make_float3(vec.x + scalar, vec.y + scalar, vec.z + scalar);
+}
+
+__host__ __device__ __forceinline__ ::float3
+    operator+(const float& scalar, const ::float3& vec)
+{
+    return make_float3(vec.x + scalar, vec.y + scalar, vec.z + scalar);
+}
+
+__host__ __device__ __forceinline__ ::float3
+    operator+(const ::float3& vec_0, const ::float3& vec_1)
+{
+    return make_float3(vec_0.x + vec_1.x, vec_0.y + vec_1.y, vec_0.z + vec_1.z);
+}
+
+
+/* Operations for subtraction
+*/
+__host__ __device__ __forceinline__ ::float3
+    operator-(const ::float3& vec_0, const ::float3& vec_1) {
+    return make_float3(vec_0.x - vec_1.x, vec_0.y - vec_1.y, vec_0.z - vec_1.z);
+}
+
+__host__ __device__ __forceinline__ float4
+    operator-(const float4& vec_0, const float4& vec_1) {
+    return make_float4(vec_0.x - vec_1.x, vec_0.y - vec_1.y, vec_0.z - vec_1.z, vec_0.w - vec_1.w);
+}
+
+
+/* Operations for product
+*/
+__host__ __device__ __forceinline__ ::float3
+    operator*(const float& v, const ::float3& v1)
+{
+    return make_float3(v * v1.x, v * v1.y, v * v1.z);
+}
+__host__ __device__ __forceinline__ ::float3
+    operator*(const ::float3& v1, const float& v)
+{
+    return make_float3(v1.x * v, v1.y * v, v1.z * v);
+}
+__host__ __device__ __forceinline__ ::float2
+    operator*(const float& v, const ::float2& v1)
+{
+    return make_float2(v * v1.x, v * v1.y);
+}
+__host__ __device__ __forceinline__ ::float2
+    operator*(const ::float2& v1, const float& v)
+{
+    return make_float2(v1.x * v, v1.y * v);
+}
+
+
+/* Operations for +=, *=
+*/
+__host__ __device__ __forceinline__ ::float3&
+    operator*=(::float3& vec, const float& v)
+{
+    vec.x *= v;
+    vec.y *= v;
+    vec.z *= v;
+    return vec;
+}
+__host__ __device__ __forceinline__ float4&
+    operator*=(float4& vec, const float& v)
+{
+    vec.x *= v;
+    vec.y *= v;
+    vec.z *= v;
+    vec.w *= v;
+    return vec;
+}
+
+__host__ __device__ __forceinline__ ::float3&
+    operator+=(::float3& vec_0, const ::float3& vec_1)
+{
+    vec_0.x += vec_1.x;
+    vec_0.y += vec_1.y;
+    vec_0.z += vec_1.z;
+    return vec_0;
+}
+
+/* Operator for negative
+    */
+__host__ __device__ __forceinline__ ::float3 operator-(const ::float3& vec)
+{
+    ::float3 negative_vec;
+    negative_vec.x = - vec.x;
+    negative_vec.y = - vec.y;
+    negative_vec.z = - vec.z;
+    return negative_vec;
+}
+
+
+/* The dot and cross product
+*/
+__host__ __device__ __forceinline__ float 
+dot(const ::float3& v1, const ::float3& v2) {
+    return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
+}
+
+__host__ __device__ __forceinline__ float
+dot(const float4& v1, const float4& v2) {
+    return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z + v1.w * v2.w;
+}
+
+
+//Only use the first 3 elements of vec4
+__host__ __device__ __forceinline__ float 
+dotxyz(const float4& v1, const float4& v2)
+{
+    return v1.x * v2.x + v1.y*v2.y + v1.z*v2.z;
+}
+__host__ __device__ __forceinline__ float
+    dotxyz(const ::float3& v1, const float4& v2)
+{
+    return v1.x * v2.x + v1.y*v2.y + v1.z*v2.z;
+}
+
+
+__host__ __device__ __forceinline__ ::float3
+    cross(const ::float3& v1, const ::float3& v2)
+{
+    return make_float3(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x);
+}
+
+__host__ __device__ __forceinline__ ::float3
+    cross_xyz(const ::float3& v1, const float4& v2)
+{
+    return make_float3(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x);
+}
+
+//The sum of the vector
+__host__ __device__ __forceinline__ float fabsf_sum(const ::float3& vec) {
+    return fabsf(vec.x) + fabsf(vec.y) + fabsf(vec.z);
+}
+__host__ __device__ __forceinline__ float fabsf_sum(const float4& vec) {
+    return fabsf(vec.x) + fabsf(vec.y) + fabsf(vec.z) + fabsf(vec.w);
+}
+
+//The L1 error between two vector
+__host__ __device__ __forceinline__ float fabsf_diff_xyz(
+    const ::float3& vec_0, 
+    const float4& vec_1
+) {
+    return fabsf(vec_0.x - vec_1.x) + fabsf(vec_0.y - vec_1.y) + fabsf(vec_0.z - vec_1.z);
+}
diff --git a/embedded_deformation/include/rigidSolver/device_intrinsics.h b/embedded_deformation/include/rigidSolver/device_intrinsics.h
new file mode 100644
index 0000000..89f8b52
--- /dev/null
+++ b/embedded_deformation/include/rigidSolver/device_intrinsics.h
@@ -0,0 +1,86 @@
+#pragma once
+
+#include <vector_functions.h>
+
+__device__ __forceinline__
+float shfl_add(float x, int offset)
+{
+    float result = 0;
+
+    asm volatile (
+    "{.reg .f32 r0;"
+            ".reg .pred p;"
+            "shfl.up.b32 r0|p, %1, %2, 0;"
+            "@p add.f32 r0, r0, %1;"
+            "mov.f32 %0, r0;}"
+    : "=f"(result) : "f"(x), "r"(offset));
+
+    return result;
+}
+
+__device__ __forceinline__
+float warp_scan(float data)
+{
+    data = shfl_add(data, 1);
+    data = shfl_add(data, 2);
+    data = shfl_add(data, 4);
+    data = shfl_add(data, 8);
+    data = shfl_add(data, 16);
+    return data;
+}
+
+
+__device__ __forceinline__
+int shfl_add(int x, int offset)
+{
+    int result = 0;
+    
+    asm volatile (
+    "{.reg .s32 r0;"
+        ".reg .pred p;"
+        "shfl.up.b32 r0|p, %1, %2, 0;"
+        "@p add.s32 r0, r0, %1;"
+        "mov.s32 %0, r0;}"
+    : "=r"(result) : "r"(x), "r"(offset));
+    
+    return result;
+}
+
+__device__ __forceinline__
+int warp_scan(int data)
+{
+    data = shfl_add(data, 1);
+    data = shfl_add(data, 2);
+    data = shfl_add(data, 4);
+    data = shfl_add(data, 8);
+    data = shfl_add(data, 16);
+    return data;
+}
+
+__device__ __forceinline__
+unsigned shfl_add(unsigned x, unsigned offset)
+{
+    unsigned result = 0;
+    
+    asm volatile (
+    "{.reg .u32 r0;"
+        ".reg .pred p;"
+        "shfl.up.b32 r0|p, %1, %2, 0;"
+        "@p add.u32 r0, r0, %1;"
+        "mov.u32 %0, r0;}"
+    : "=r"(result) : "r"(x), "r"(offset));
+    
+    return result;
+}
+
+__device__ __forceinline__
+unsigned warp_scan(unsigned data)
+{
+    data = shfl_add(data, 1u);
+    data = shfl_add(data, 2u);
+    data = shfl_add(data, 4u);
+    data = shfl_add(data, 8u);
+    data = shfl_add(data, 16u);
+    return data;
+}
+
diff --git a/embedded_deformation/include/rigidSolver/rigidSolver.h b/embedded_deformation/include/rigidSolver/rigidSolver.h
new file mode 100644
index 0000000..0504faa
--- /dev/null
+++ b/embedded_deformation/include/rigidSolver/rigidSolver.h
@@ -0,0 +1,89 @@
+#pragma once
+
+#include "imageProcessor/common_types.hpp"
+#include "math/device_mat.h"
+#include "containers/SynchronizeArray.h"
+#include "containers/device_array.hpp"
+
+using namespace CUDA;
+
+class RigidSolver {
+private:
+    Intrinsic m_project_intrinsic;
+    unsigned m_image_width, m_image_height;
+
+    // 当前帧
+    struct {
+        cudaTextureObject_t vertex_map;
+        cudaTextureObject_t normal_map;
+    } m_live_maps;
+
+    // 参考帧 这里是上一帧 按道理应该是从model中获取
+    struct {
+        cudaTextureObject_t vertex_map;
+        cudaTextureObject_t normal_map;
+    } m_reference_maps;
+
+    mat34 m_curr_refer2live;
+
+
+public:
+    RigidSolver(const unsigned width,
+                const unsigned height,
+                const Intrinsic& intrinsic);
+    ~RigidSolver();
+
+    void SetInputMaps(
+        const vertex_normal_maps& live_maps,
+        const vertex_normal_maps& reference_maps,
+        const mat34& init_world2camera
+    );
+    Eigen::Matrix<float, 3, 4> test(
+        const vertex_normal_maps& live_maps,
+        const vertex_normal_maps& reference_maps);
+
+    mat34 Solve(int max_iters = 3, cudaStream_t stream = 0);
+
+    
+
+private:
+    DeviceArray2D<float> m_reduce_buffer;
+    SynchronizeArray<float> m_reduced_matrix_vector;
+    DeviceBufferArray<correspondence> m_correspondences;
+
+    void allocateReduceBuffer();
+    void rigidSolverDeviceIteration(cudaStream_t stream = 0);
+    // 找匹配点
+    // 图像处理后找匹配点
+    void findCorrespondences(cudaStream_t stream = 0);
+    void FindCorrespondences(DeviceArray<correspondence>& correspondences,
+                            int& correspondence_count,
+                            cudaStream_t stream = 0);
+    
+
+    
+
+    Eigen::Matrix<float, 6, 6> JtJ_;  // Ci       Cx = d
+    Eigen::Matrix<float, 6, 1> JtErr_;  // di
+    void rigidSolverHostIterationSync(cudaStream_t stream = 0);
+
+public:
+    // 函数接口
+    std::vector<correspondence> getCorrespondence() const {
+        DeviceArrayView<correspondence> correspondence_view = m_correspondences.ArrayView();
+        std::vector<correspondence> correspondences;
+        correspondence_view.Download(correspondences);
+        return correspondences;
+        }
+
+    // 一次非刚性求解后找匹配点
+    void findCorrespondences(Eigen::MatrixXd& source_vertex,
+                            Eigen::MatrixXd& source_normal,
+                            DeviceBufferArray<correspondence>& correspondences,
+                            cudaStream_t stream = 0);
+    void FindCorrespondences(DeviceArray<correspondence>& correspondences,
+                            Eigen::MatrixXd& source_vertex,
+                            Eigen::MatrixXd& source_normal,
+                            int& correspondence_count,
+                            cudaStream_t stream = 0); 
+};
\ No newline at end of file
diff --git a/embedded_deformation/src/embedded_deformation/embedDeform.cpp b/embedded_deformation/src/embedded_deformation/embedDeform.cpp
index 032b60e..6b3096b 100644
--- a/embedded_deformation/src/embedded_deformation/embedDeform.cpp
+++ b/embedded_deformation/src/embedded_deformation/embedDeform.cpp
@@ -1,10 +1,3 @@
-/*
-*   embedded deformation implementation
-*   by R. Falque
-*   14/11/2018
-*/
-
-
 
 /* TODO list:
  *  - add the distance in the creation of the graph
@@ -21,8 +14,6 @@
 #include "greedySearch.hpp"
 #include "downsampling.hpp"
 
-//#include "costFunction.hpp"
-
 #include <vector>
 #include <chrono>
 #include <cmath>
@@ -197,7 +188,7 @@ EmbeddedDeformation::EmbeddedDeformation(Eigen::MatrixXd V_in,
 	nodes_connectivity_ = nodes_connectivity;
 
 	// extract point cloud
-	Eigen::MatrixXd N;
+	Eigen::MatrixXd N;                                      // 从V中提取出来的点云
 	//downsampling(V_, N, indexes_of_deformation_graph_in_V_, grid_resolution, use_farthest_sampling_);
 	downsampling(V_, N, indexes_of_deformation_graph_in_V_, grid_resolution, 0, use_farthest_sampling_, true);
 
@@ -222,6 +213,7 @@ EmbeddedDeformation::EmbeddedDeformation(Eigen::MatrixXd V_in,
 
 
 EmbeddedDeformation::EmbeddedDeformation(Eigen::MatrixXd V_in,
+										 Eigen::MatrixXd normal_in,
 										   options opts)
 {
 	std::cout << "use knn to look for closest point\n";
@@ -232,11 +224,14 @@ EmbeddedDeformation::EmbeddedDeformation(Eigen::MatrixXd V_in,
 		transpose_input_and_output_ = true;
 	} else if (V_in.cols() == 3) {		
 		V_ = V_in;
+		normal_ = normal_in;
 		transpose_input_and_output_ = false;
 	} else {
 		throw std::invalid_argument( "wrong input size" );
 	}
 
+	
+
 	// set up class variables
 	verbose_ = true;
 	use_knn_ = true;
@@ -248,6 +243,7 @@ EmbeddedDeformation::EmbeddedDeformation(Eigen::MatrixXd V_in,
 	w_reg_ = opts.w_reg;
 	w_rig_ = opts.w_rig;
 	w_con_ = opts.w_con;
+	std::cout<<"w_rot_:"<<w_rot_<<std::endl;
 
 	// extract point cloud
 	Eigen::MatrixXd N;
@@ -264,7 +260,7 @@ EmbeddedDeformation::EmbeddedDeformation(Eigen::MatrixXd V_in,
 		closest_points = tree_2.return_k_closest_points(N.row(i), nodes_connectivity_+ 2 );
 
 		for (int j = 0; j < closest_points.size(); ++j)
-			if (i != closest_points[j]) {
+			if (i != closest_points[j]) {                // 避免自己指向自己
 				E(counter, 0) = i;
 				E(counter, 1) = closest_points[j];
 				counter ++;
@@ -278,16 +274,23 @@ EmbeddedDeformation::EmbeddedDeformation(Eigen::MatrixXd V_in,
 
 
 
-void EmbeddedDeformation::deform(Eigen::MatrixXd sources_in, Eigen::MatrixXd targets_in, Eigen::MatrixXd & V_deformed)
+void EmbeddedDeformation::deform(Eigen::MatrixXd sources_in, 
+								Eigen::MatrixXd targets_in, 
+								Eigen::MatrixXd targets_normal_in,
+								Eigen::MatrixXd & V_deformed, 
+								Eigen::MatrixXd & normal_deformed, 
+								options opts)
 {
-	Eigen::MatrixXd sources, targets;
+	Eigen::MatrixXd sources, targets, targets_normal;
 
 	if (sources_in.cols() == 3 && targets_in.cols() == 3) {		
 		sources = sources_in;
 		targets = targets_in;
+		targets_normal = targets_normal_in;
 	} else if (sources_in.rows() == 3 && targets_in.rows() == 3) {
 		sources = sources_in.transpose();
 		targets = targets_in.transpose();
+		targets_normal = targets_normal_in.transpose();
 	} else {
 		throw std::invalid_argument( "wrong input size" );
 	}
@@ -304,26 +307,35 @@ void EmbeddedDeformation::deform(Eigen::MatrixXd sources_in, Eigen::MatrixXd tar
 	if (use_dijkstra_)
 		deformation_graph_greedy_search = new greedy_search(V_, F_, indexes_of_deformation_graph_in_V_);
 	if (use_knn_)
+	{
+		std::cout<<"use knn to look for closest point\n"<<std::endl;
 		deformation_graph_kdtree = new nanoflann_wrapper(deformation_graph_ptr_->get_nodes());
+	}
+		
 
 	// set the sources neighbours
+	// 找到每个source的最近的nodes
     sources_nodes_neighbours.resize(sources.rows());
 	nanoflann_wrapper tree(V_);
 	for (int i = 0; i < sources.rows(); ++i)
 	{
 		// find closest point on the mesh
 		std::vector< int > closest_point;
-		closest_point = tree.return_k_closest_points(sources.row(i), 1);
+		closest_point = tree.return_k_closest_points(sources.row(i), 1);           // 在所有Vertex中找到离source最近的点
 
+		// nodes中源点的最近邻
 		if (use_dijkstra_)
 			sources_nodes_neighbours.at(i) = deformation_graph_greedy_search->return_k_closest_points(closest_point.at(0), nodes_connectivity_+1);
 		if (use_knn_)
-			sources_nodes_neighbours.at(i) = deformation_graph_kdtree->return_k_closest_points(V_.row(closest_point.at(0)), nodes_connectivity_+1);
+			sources_nodes_neighbours.at(i) = deformation_graph_kdtree->return_k_closest_points(V_.row(closest_point.at(0)), nodes_connectivity_+1); 
 	}
 
 	// initialize the parameters that needs to be optimized (nodes rotations and translations)
+	// 每个nodes有12个参数，前9个是rotation matrix，后3个是translation
 	std::vector< std::vector <double> > params;
 
+	// 有多少个节点就有多少个params
+	// 在这里可以为节点赋值，即rigid_solver求解的结果
 	params.resize(deformation_graph_ptr_->num_nodes());
 	for (int i = 0; i < params.size(); ++i)
 	{
@@ -343,12 +355,13 @@ void EmbeddedDeformation::deform(Eigen::MatrixXd sources_in, Eigen::MatrixXd tar
 		params[i][11] = 0;
 	}
 
-	// run levenberg marquardt optimization
+	// run levenberg marquardt optimization LM非线性优化
 	ceres::Problem problem;
 
 	if (verbose_)
 		std::cout << "progress : add the residuals for RotCostFunction ..." << std::endl;
-		
+	
+	// 保证每个节点的3*3矩阵尽可能是旋转矩阵
 	for (int i = 0; i < deformation_graph_ptr_->num_nodes(); ++i)
 	{
 		//RotCostFunction* cost_function = new RotCostFunction(w_rot_);
@@ -359,8 +372,11 @@ void EmbeddedDeformation::deform(Eigen::MatrixXd sources_in, Eigen::MatrixXd tar
 	if (verbose_)
 		std::cout << "progress : add the residuals for RegCostFunction ..." << std::endl;
 
+	// regularizer项 as rigid as possible核心公式
 	for (int i = 0; i < deformation_graph_ptr_->num_nodes(); ++i)
 	{
+		// 遍历每个节点的近邻
+		// 第i个节点对应第i个params
 		for (int j = 0; j < deformation_graph_ptr_->get_adjacency_list(i).size(); ++j)
 		{
 			int k = deformation_graph_ptr_->get_adjacency_list(i, j);
@@ -373,27 +389,29 @@ void EmbeddedDeformation::deform(Eigen::MatrixXd sources_in, Eigen::MatrixXd tar
 		}
 	}
 	
-	if (verbose_)
-		std::cout << "progress : add the residuals for RigCostFunction ..." << std::endl;
+	// if (verbose_)
+	// 	std::cout << "progress : add the residuals for RigCostFunction ..." << std::endl;
 
-	std::vector<int> bridges;
-	deformation_graph_ptr_->has_bridges(bridges);
-	for (int i=0; i<bridges.size(); i++)
-	{
-		Eigen::Vector2i nodes_list = deformation_graph_ptr_->get_edge(bridges[i]);
-		RigCostFunction* cost_function = new RigCostFunction(w_rig_);
+	// std::vector<int> bridges;
+	// deformation_graph_ptr_->has_bridges(bridges);
+	// // 相邻节点的旋转矩阵的约束 也是 as rigid as possible的一种
+	// for (int i=0; i<bridges.size(); i++)
+	// {
+	// 	Eigen::Vector2i nodes_list = deformation_graph_ptr_->get_edge(bridges[i]);
+	// 	RigCostFunction* cost_function = new RigCostFunction(w_rig_);
 
-		// add residual block
-		problem.AddResidualBlock(cost_function, NULL, &params[nodes_list(0)][0], &params[nodes_list(1)][0]);
-	}
+	// 	// add residual block
+	// 	problem.AddResidualBlock(cost_function, NULL, &params[nodes_list(0)][0], &params[nodes_list(1)][0]);
+	// }
 
 	if (verbose_)
 	std::cout << "progress : add the residuals for ConCostFunction ..." << std::endl;
+	// point to point icp项 常被称为data项
 	for (int i = 0; i < sources.rows(); ++i)
 	{
 		// stack the parameters
 		std::vector<double *> parameter_blocks;
-		std::vector<Eigen::Vector3d> vector_g;
+		std::vector<Eigen::Vector3d> vector_g;                      // source的近邻节点
 
 		// define cost function and associated parameters
 		for (int j = 0; j < nodes_connectivity_; ++j)
@@ -408,6 +426,31 @@ void EmbeddedDeformation::deform(Eigen::MatrixXd sources_in, Eigen::MatrixXd tar
 		// add residual block
 		problem.AddResidualBlock(cost_function, NULL, parameter_blocks);
 	}
+	
+	// 自己添加的代码
+	if (verbose_)
+	std::cout << "progress : add the residuals for point to plane CostFunction ..." << std::endl;
+	// point to plane icp项 data项
+	for (int i = 0; i < sources.rows(); ++i)
+	{
+		// stack the parameters
+		std::vector<double *> parameter_blocks;
+		std::vector<Eigen::Vector3d> vector_g;                      // source的近邻节点
+
+		// define cost function and associated parameters
+		for (int j = 0; j < nodes_connectivity_; ++j)
+			parameter_blocks.push_back(&params[ sources_nodes_neighbours[i][j] ][0]);
+
+		for (int j = 0; j < sources_nodes_neighbours[i].size(); ++j)
+			vector_g.push_back( deformation_graph_ptr_->get_node(sources_nodes_neighbours[i][j]) );
+
+		// create the cost function
+		p2plCostFunction* cost_function = new p2plCostFunction(w_p2pl_, vector_g, sources.row(i), targets.row(i), targets_normal.row(i));
+
+		// add residual block
+		problem.AddResidualBlock(cost_function, NULL, parameter_blocks);
+	}
+	// 自己添加的代码
 
 	// Run the solver
 	ceres::Solver::Options options;
@@ -435,9 +478,10 @@ void EmbeddedDeformation::deform(Eigen::MatrixXd sources_in, Eigen::MatrixXd tar
 
 	// redefine the deformed mesh
 	Eigen::MatrixXd V_temp = V_;
+	Eigen::MatrixXd normal_temp = normal_;
 
 	std::vector<double> w_j;
-	Eigen::Vector3d new_point;
+	Eigen::Vector3d new_point, new_normal;
 	for (int i = 0; i < V_.rows(); ++i)
 	{
 		std::vector< int > neighbours_nodes;
@@ -462,18 +506,57 @@ void EmbeddedDeformation::deform(Eigen::MatrixXd sources_in, Eigen::MatrixXd tar
 			w_j[j] /= normalization_factor;
 
 		new_point << 0, 0, 0;
-		for (int j = 0; j < nodes_connectivity_; ++j)
+		new_normal << 0, 0, 0;
+		for (int j = 0; j < nodes_connectivity_; ++j) {
 			new_point += w_j[j] * (rotation_matrices_[ neighbours_nodes[j] ] * (V_.row(i).transpose() - deformation_graph_ptr_->get_node( neighbours_nodes[j] ) ) 
-				                                                           + deformation_graph_ptr_->get_node( neighbours_nodes[j] ) + translations_[ neighbours_nodes[j] ]);
-
+																					+ deformation_graph_ptr_->get_node( neighbours_nodes[j] ) + translations_[ neighbours_nodes[j] ]);
+			new_normal += w_j[j] * (rotation_matrices_[ neighbours_nodes[j] ].inverse().transpose() * normal_.row(i).transpose() );
+		}
+			
 		V_temp.row(i) = new_point;
+		normal_temp.row(i) = new_normal;
 
 	}
 
-	if (transpose_input_and_output_)
+
+	
+
+	if (transpose_input_and_output_) {
 		V_deformed = V_temp.transpose();
-	else
+		normal_deformed = normal_temp.transpose();
+	}
+	else {
 		V_deformed = V_temp;
+		normal_deformed = normal_temp;
+	}
+
+	// 根据变形过的上一帧顶点图，更新deformation graph
+	if(deformation_graph_ptr_ !=nullptr)
+	{
+		delete deformation_graph_ptr_;
+		deformation_graph_ptr_ = nullptr;
+	}
+	Eigen::MatrixXd N;
+	downsampling(V_deformed, N, indexes_of_deformation_graph_in_V_, 
+	             opts.grid_resolution, opts.grid_size, 
+				 opts.use_farthest_sampling, opts.use_relative);
+
+	Eigen::MatrixXi E(N.rows()*(nodes_connectivity_+1), 2);
+	nanoflann_wrapper tree_2(N);
+	int counter = 0;
+	for (int i = 0; i < N.rows(); ++i) {
+		std::vector< int > closest_points;
+		closest_points = tree_2.return_k_closest_points(N.row(i), nodes_connectivity_+ 2 );
+
+		for (int j = 0; j < closest_points.size(); ++j)
+			if (i != closest_points[j]) {                // 避免自己指向自己
+				E(counter, 0) = i;
+				E(counter, 1) = closest_points[j];
+				counter ++;
+			}
+	}
+	deformation_graph_ptr_ = new libgraphcpp::Graph(N, E);
+		
 
 	if (deformation_graph_greedy_search != nullptr) delete deformation_graph_greedy_search;
     if (deformation_graph_kdtree != nullptr) delete deformation_graph_kdtree;
@@ -497,7 +580,7 @@ void EmbeddedDeformation::deform_other_points(Eigen::MatrixXd & V) {
 	Eigen::MatrixXd V_temp = V_in;
 
 	std::vector<double> w_j;
-	Eigen::Vector3d new_point;
+	Eigen::Vector3d new_point, new_normal;
 	for (int i = 0; i < V_in.rows(); ++i)
 	{
 		std::vector< int > neighbours_nodes;
diff --git a/embedded_deformation/src/imageProcessor/common.cu b/embedded_deformation/src/imageProcessor/common.cu
new file mode 100644
index 0000000..038e951
--- /dev/null
+++ b/embedded_deformation/src/imageProcessor/common.cu
@@ -0,0 +1,326 @@
+#include "imageProcessor/common.h"
+#include "math/eigen33.h"
+
+namespace device{
+
+
+    __global__ void CollectValidDataKernel(
+        cudaTextureObject_t vertex_map,
+        cudaTextureObject_t normal_map,
+        const unsigned width,
+        const unsigned height,
+        PtrSz<Surfel> valid_surfel,
+        int* valid_surfel_counter,
+        cudaStream_t stream = 0
+    )
+    {
+        const auto x = threadIdx.x + blockIdx.x * blockDim.x;
+        const auto y = threadIdx.y + blockIdx.y * blockDim.y;
+        if(x >= width || y >= height) return;
+
+        float4 vertex = tex2D<float4>(vertex_map, x, y);
+        // 只判断了顶点是否为0 其中许多normal也是0但没有被判断出来
+        if (is_zero_vertex(vertex)) return;
+        Surfel surfel;
+        surfel.pixel_coord.x() = x;
+        surfel.pixel_coord.y() = y;
+        surfel.vertex = vertex;
+        surfel.normal = tex2D<float4>(normal_map, x, y);
+        const int index = atomicAdd(valid_surfel_counter, 1);
+        valid_surfel[index] = surfel;
+    }
+
+    enum {
+		window_dim = 7,
+		halfsize = 3,
+		window_size = window_dim * window_dim
+	};
+
+    __global__ void createVertexMapKernel(
+        cudaTextureObject_t depth_texture,
+        const Intrinsic intrinsic,
+        const unsigned width,
+        const unsigned height,
+        cudaSurfaceObject_t vertex_surface
+    )
+    {
+        const auto x = threadIdx.x + blockIdx.x * blockDim.x;
+        const auto y = threadIdx.y + blockIdx.y * blockDim.y;
+
+        if(x >= width || y >= height) return;
+
+        const unsigned short depth =  tex2D<unsigned short>(depth_texture,x,y);
+        float4 vertex;
+
+        // scale the depth to [m]
+        // bonn 数据集对应的应该是5000
+        vertex.z = float(depth) * 0.0010000000474974513;
+        vertex.x = vertex.z *(x- intrinsic.principal_x) / intrinsic.focal_x;
+        vertex.y = vertex.z *(y- intrinsic.principal_y) / intrinsic.focal_y;
+        vertex.w = 0.0f;
+        surf2Dwrite(vertex, vertex_surface, x * sizeof(float4), y);
+    }
+
+    __global__ void createNormalMapKernel(
+        cudaTextureObject_t vertex_map,
+        const unsigned width,
+        const unsigned height,
+        cudaSurfaceObject_t normal_map
+    ) {
+        
+        const auto x = threadIdx.x + blockIdx.x * blockDim.x;
+        const auto y = threadIdx.y + blockIdx.y * blockDim.y;
+        if(x >= width || y >= height) return;
+        
+        const float4 vertex_center = tex2D<float4>(vertex_map, x, y);
+
+        float4 normal = make_float4(0, 0, 0, 0);
+        if(!is_zero_vertex(vertex_center)) {
+            float4 centeroid = make_float4(0, 0, 0, 0);
+            int counter = 0;
+            for (int cy = y - halfsize; cy <= y + halfsize; cy += 1) {                   // 7*7窗口求和
+				for (int cx = x - halfsize; cx <= x + halfsize; cx += 1) {
+					const float4 p = tex2D<float4>(vertex_map, cx, cy);
+					if (!is_zero_vertex(p)) {
+						centeroid.x += p.x;
+						centeroid.y += p.y;
+						centeroid.z += p.z;
+						counter++;
+					}
+				}
+			}
+
+            //At least half of the window is valid
+			if(counter > (window_size / 2)) {                                           
+				centeroid *= (1.0f / counter);
+				float covariance[6] = { 0 };
+
+				//Second window search to compute the normal
+				for (int cy = y - halfsize; cy < y + halfsize; cy += 1) {
+					for (int cx = x - halfsize; cx < x + halfsize; cx += 1) {
+						const float4 p = tex2D<float4>(vertex_map, cx, cy);
+						if (!is_zero_vertex(p)) {
+							const float4 diff = p - centeroid;
+							//Compute the covariance
+							covariance[0] += diff.x * diff.x; //(0, 0)
+							covariance[1] += diff.x * diff.y; //(0, 1)
+							covariance[2] += diff.x * diff.z; //(0, 2)
+							covariance[3] += diff.y * diff.y; //(1, 1)
+							covariance[4] += diff.y * diff.z; //(1, 2)
+							covariance[5] += diff.z * diff.z; //(2, 2)
+						}
+					}
+				}
+                eigen33 eigen(covariance);
+				float3 normal_value;
+				eigen.compute(normal_value);
+				if (dotxyz(normal_value, vertex_center) >= 0.0f) normal *= -1;
+
+				//The radius
+				const float radius = 0.0;
+
+				//Write to local variable
+				normal.x = normal_value.x;
+				normal.y = normal_value.y;
+				normal.z = normal_value.z;
+				normal.w = radius;
+            }   
+        }
+        surf2Dwrite(normal, normal_map, x * sizeof(float4), y);
+    }
+    
+    __global__ void bilateralFilterKernel(
+        cudaTextureObject_t raw_depth,
+        const unsigned width,
+        const unsigned height,
+        const unsigned clip_width,
+        const unsigned clip_height,
+        const float sigma_s_inv_square,
+        const float sigma_r_inv_square,
+        cudaSurfaceObject_t filter_depth
+    )
+    {
+        const auto x = threadIdx.x + blockIdx.x * blockDim.x;           // 0~640
+        const auto y = threadIdx.y + blockIdx.y * blockDim.y;           // 0~480
+        if(y >= clip_height || x >= clip_width) return;                 // 0~620 0~460
+        
+        const auto half_width = 5;
+        const auto raw_x = x + boundary_clip;                           // 10~630
+        const auto raw_y = y + boundary_clip;                           // 10~470
+        const unsigned short center_depth = tex2D<unsigned short> (raw_depth, raw_x, raw_y);
+
+        float sum_all = 0.0f;
+        float sum_weight = 0.0f;
+        for(auto y_idx = raw_y-half_width; y_idx <= raw_y + half_width; y_idx++)
+        {
+            for(auto x_idx = raw_x-half_width; x_idx <= raw_x + half_width; x_idx++)
+            {
+                const unsigned short depth = tex2D<unsigned short> (raw_depth, x_idx, y_idx);
+                const float depth_diff2 = (depth -center_depth) * (depth - center_depth);
+                const float pixel_diff2 = (x_idx - raw_x) * (x_idx - raw_x) + (y_idx - raw_y) * (y_idx - raw_y);
+                const float this_weight = (depth > 0) * expf(-sigma_s_inv_square * pixel_diff2) * expf(-sigma_r_inv_square * depth_diff2);
+                sum_weight += this_weight;
+                sum_all += this_weight * depth;
+            }
+        }
+
+        unsigned short filtered_depth_value = __float2uint_rn(sum_all / sum_weight);
+        if(filtered_depth_value > 1000) filtered_depth_value = 0;
+        surf2Dwrite(filtered_depth_value,filter_depth,x*sizeof(unsigned short),y);
+    }
+}; // namespace device
+
+void createDefault2DTextureDesc(cudaTextureDesc &desc)
+{
+    memset(&desc,0,sizeof(desc));
+    desc.addressMode[0] = cudaAddressModeBorder;     // 定义了纹理在各个维度（X、Y、Z）上的寻址模式
+    desc.addressMode[1] = cudaAddressModeBorder;
+    desc.addressMode[2] = cudaAddressModeBorder;
+    desc.filterMode = cudaFilterModePoint;
+    desc.readMode = cudaReadModeElementType;
+    desc.normalizedCoords = 0;
+}
+
+// 创建纹理和表面对象
+// 三步：1. 创建数组 1.1 创建资源描述符 2. 创建纹理对象 3. 创建表面对象
+void createDepthTextureSurface(const unsigned width, const unsigned height, 
+                                cudaTextureObject_t& texture, 
+                                cudaSurfaceObject_t& surface, 
+                                cudaArray_t& array)
+{
+    // 为array分配内存
+    cudaChannelFormatDesc depth_channel_desc = cudaCreateChannelDesc(16,0,0,0,cudaChannelFormatKindUnsigned);
+    cudaSafeCall(cudaMallocArray(&array, &depth_channel_desc, width, height));
+
+    // 创建资源描述符
+    cudaResourceDesc res_desc;
+    memset(&res_desc,0,sizeof(cudaResourceDesc));
+    res_desc.resType = cudaResourceTypeArray;         // 指定资源类型为数组
+    res_desc.res.array.array = array;                 // 指定数组句柄
+
+    // 创建纹理对象 需要纹理描述符
+    cudaTextureDesc depth_texture_desc;
+    createDefault2DTextureDesc(depth_texture_desc);
+    cudaSafeCall(cudaCreateTextureObject(&texture, &res_desc, &depth_texture_desc, NULL));
+
+    // 创建表面对象
+    cudaSafeCall(cudaCreateSurfaceObject(&surface, &res_desc));
+}
+
+void createFloat4TextureSurface(const unsigned width, 
+                                const unsigned height,
+                                cudaTextureObject_t &texture,
+                                cudaSurfaceObject_t &surface,
+                                cudaArray_t &array)
+{
+    cudaChannelFormatDesc float4_channel_desc = cudaCreateChannelDesc(32,32,32,32,cudaChannelFormatKindFloat);
+    cudaSafeCall(cudaMallocArray(&array, &float4_channel_desc, width, height));
+
+    cudaResourceDesc res_desc;
+    memset(&res_desc,0,sizeof(cudaResourceDesc));
+    res_desc.resType = cudaResourceTypeArray;
+    res_desc.res.array.array = array;  
+
+    cudaTextureDesc float4_texture_desc;
+    createDefault2DTextureDesc(float4_texture_desc);
+    cudaSafeCall(cudaCreateTextureObject(&texture, &res_desc, &float4_texture_desc, NULL));
+
+    cudaSafeCall(cudaCreateSurfaceObject(&surface, &res_desc));
+}
+
+// 释放纹理和表面
+void releaseTexture(CudaTextureSurface &texture)
+{
+    cudaSafeCall(cudaDestroyTextureObject(texture.texture));
+    cudaSafeCall(cudaDestroySurfaceObject(texture.surface));
+    cudaSafeCall(cudaFreeArray(texture.array));
+}
+
+// 深度图双边滤波滤波处理
+void BilateralFilterDepth(
+    cudaTextureObject_t raw_depth,
+    cudaSurfaceObject_t filter_depth,
+    const unsigned raw_width,
+    const unsigned raw_height,
+    const unsigned clip_wdith,
+    const unsigned clip_height,
+    cudaStream_t stream
+)
+{
+    const float sigma_s = 100.f;
+    const float sigma_r = 100.f;
+    const float sigma_s_inv_square = 1.0f / (sigma_s * sigma_s);
+    const float sigma_r_inv_square = 1.0f / (sigma_r * sigma_r);
+
+    dim3 blk(16,16);
+    dim3 grid(divUp(raw_width,blk.x),divUp(raw_height,blk.y));
+    device::bilateralFilterKernel<<<grid,blk,0,stream>>>(
+        raw_depth,
+        raw_width, raw_height,
+        clip_wdith, clip_height,
+        sigma_s_inv_square, sigma_r_inv_square,
+        filter_depth
+    );
+}
+
+// 计算顶点坐标
+void createVertexMap(
+    cudaTextureObject_t depth_texture,
+    Intrinsic intrinsic,
+    const unsigned width,
+    const unsigned height,
+    cudaSurfaceObject_t vertex_surface,
+    cudaStream_t stream
+)
+{
+    dim3 blk(16,16);
+    dim3 grid(divUp(width,blk.x),divUp(height,blk.y));
+    device::createVertexMapKernel<<<grid,blk,0,stream>>>(
+        depth_texture,
+        intrinsic,
+        width,height,
+        vertex_surface
+    );
+}
+
+void createNormalMap(
+    cudaTextureObject_t vertex_map,
+    const unsigned width,
+    const unsigned height,
+    cudaSurfaceObject_t normal_map,
+    cudaStream_t stream
+) {
+    dim3 blk(16,16);
+    dim3 grid(divUp(width,blk.x),divUp(height,blk.y));
+    device::createNormalMapKernel<<<grid, blk, 0, stream>>>(
+        vertex_map, width, height, normal_map
+    );
+}
+
+void CollectValidData(
+    cudaTextureObject_t vertex_map,
+    cudaTextureObject_t normal_map,
+    const unsigned width,
+    const unsigned height,
+    DeviceArray<Surfel>& valid_surfel,
+    int& surfel_counter,
+    cudaStream_t stream
+)
+{
+    int* valid_surfel_counter;
+    cudaMalloc((void**)& valid_surfel_counter, sizeof(int));
+    cudaMemset(valid_surfel_counter, 0, sizeof(int));
+    dim3 blk(16,16);
+    dim3 grid(divUp(width,blk.x),divUp(height,blk.y));
+    device::CollectValidDataKernel<<<grid,blk,0,stream>>>(
+        vertex_map, 
+        normal_map, 
+        width, height, 
+        valid_surfel, 
+        valid_surfel_counter
+    );
+    int h_surfel_counter;
+    cudaSafeCall(cudaMemcpy(&h_surfel_counter, valid_surfel_counter, sizeof(int), cudaMemcpyDeviceToHost));
+    surfel_counter = h_surfel_counter;
+    cudaFree(valid_surfel_counter);
+}
diff --git a/embedded_deformation/src/imageProcessor/common_utils.cu b/embedded_deformation/src/imageProcessor/common_utils.cu
new file mode 100644
index 0000000..fa02174
--- /dev/null
+++ b/embedded_deformation/src/imageProcessor/common_utils.cu
@@ -0,0 +1,41 @@
+#include "imageProcessor/common_utils.h"
+#include "imageProcessor/safe_call.hpp"
+#include <cuda_runtime_api.h>
+#include <cuda.h>
+#include <driver_types.h>
+
+
+CUcontext initCudaContext(int selected_device) {
+    //Initialize the cuda driver api
+    cuSafeCall(cuInit(0));
+
+    //Query the device
+    int device_count = 0;
+    cuSafeCall(cuDeviceGetCount(&device_count));
+    for(auto dev_idx = 0; dev_idx < device_count; dev_idx++) {
+        char dev_name[256] = { 0 };
+        cuSafeCall(cuDeviceGetName(dev_name, 256, dev_idx));
+        printf("device %d: %s\n", dev_idx, dev_name);
+    }
+
+    //Select the device
+    printf("device %d is used as parallel processor.\n", selected_device);
+    CUdevice cuda_device;
+    cuSafeCall(cuDeviceGet(&cuda_device, selected_device));
+
+    //Create cuda context
+    CUcontext cuda_context;
+    cuSafeCall(cuCtxCreate(&cuda_context, CU_CTX_SCHED_AUTO, cuda_device));
+    return cuda_context;
+}
+
+void destroyCudaContext(CUcontext context) {
+    cudaDeviceSynchronize();
+    cuSafeCall(cuCtxDestroy(context));
+}
+
+
+
+
+
+
diff --git a/embedded_deformation/src/imageProcessor/fetchInterface.cpp b/embedded_deformation/src/imageProcessor/fetchInterface.cpp
new file mode 100644
index 0000000..d83ec52
--- /dev/null
+++ b/embedded_deformation/src/imageProcessor/fetchInterface.cpp
@@ -0,0 +1,165 @@
+#include "imageProcessor/fetchInterface.h"
+#include <opencv2/core.hpp>
+#include <boost/filesystem/operations.hpp>
+#include <exception>
+#include <regex>
+#include <iterator>
+#include <array>
+#include "imageProcessor/logging.h"
+
+
+const int CV_ANYCOLOR = cv::IMREAD_ANYCOLOR;
+const int CV_ANYDEPTH = cv::IMREAD_ANYDEPTH;
+
+FetchInterface::FetchInterface(const std::string data_dir, std::string extension, bool force_no_masks) :
+		m_mask_buffer_ix(SIZE_MAX), m_use_masks(false)              // 默认不使用掩码
+{
+	path data_path(data_dir);
+	std::vector<path> sorted_paths;
+	if (extension.length() > 0 && extension[0] != '.') {
+		extension = "." + extension;
+	}
+	std::copy(fs::directory_iterator(data_path), fs::directory_iterator(), std::back_inserter(sorted_paths));       // 把data_path 文件下所有目录拷贝到 sorted_path
+	std::sort(sorted_paths.begin(), sorted_paths.end());                             								// 默认按照升序排列
+	bool unexpected_mask_frame_number = false;
+	
+
+	// 把所有的color depth mask 图片的路径放在对应的 vector中
+	for (auto& path : sorted_paths) {
+		if (FilenameIndicatesDepthImage(path.filename().string(), extension)) {
+			int frame_number = GetFrameNumber(path);                                       // 将图片名字中数字字符串转换成整数
+			if (frame_number != m_depth_image_paths.size()) {
+				throw std::runtime_error("Unexpected depth frame number encountered");
+			}
+			m_depth_image_paths.push_back(path);
+		} else if (FilenameIndicatesRGBImage(path.filename().string(), extension)) {
+			int frame_number = GetFrameNumber(path);
+			if (frame_number != m_rgb_image_paths.size()) {
+				throw std::runtime_error("Unexpected RGB frame number encountered");
+			}
+			m_rgb_image_paths.push_back(path);
+		} else if (!force_no_masks && FilenameIndicatesMaskImage(path.filename().string(), extension)) {
+			int frame_number = GetFrameNumber(path);
+			if (frame_number != m_mask_image_paths.size()) {
+				unexpected_mask_frame_number = true;
+			}
+			m_mask_image_paths.push_back(path);
+		}
+	}
+
+
+	
+	// 如果depth和rgb的number不一致
+	if (m_depth_image_paths.size() != m_rgb_image_paths.size()) {
+		LOG(FATAL) << "Presumed depth image count (" << m_depth_image_paths.size() <<
+		           ") doesn't equal presumed rgb image count." << m_rgb_image_paths.size();
+	}
+
+	// 是否强制不适用mask 默认是false
+	if (!force_no_masks) {
+		if (unexpected_mask_frame_number) {
+			LOG(WARNING)
+					<< "Warning: inconsistent mask frame numbers encountered in the filenames. Proceeding without masks.";
+		} else if (!m_mask_image_paths.empty()) {               // 掩码路径不为空
+			if (m_depth_image_paths.size() != m_mask_image_paths.size() && !m_mask_image_paths.empty()) {       // mask与depthshu数量不一致
+				LOG(WARNING)
+						<< "Warning: seems like there were some mask image files, but their number doesn't match the "
+						   "number of depth frames. Proceeding without masks.";
+			} else {
+				m_use_masks = true;
+			}
+		}
+	}
+}
+
+bool FetchInterface::HasSubstringFromSet(const std::string& string, const std::string* set, int set_size)
+{
+	bool found_indicator = false;
+	for (int i_target_string = 0; i_target_string < set_size; i_target_string++) {
+		if (string.find(set[i_target_string]) != std::string::npos) {                          // 在字符串 string 中查找子字符串
+			return true;
+		}
+	}
+	return false;
+}
+
+bool FetchInterface::FilenameIndicatesDepthImage(const path& filename, const std::string& valid_extension)
+{
+	if (filename.extension() != valid_extension) return false;                                                                // 首先检查扩展名
+	const std::array<std::string, 3> possible_depth_indicators = {"depth", "DEPTH", "Depth"};
+	return HasSubstringFromSet(filename.string(), possible_depth_indicators.data(), possible_depth_indicators.size());
+}
+
+bool FetchInterface::FilenameIndicatesRGBImage(const path& filename, const std::string& valid_extension)
+{
+	if (filename.extension() != valid_extension) return false;
+	const std::array<std::string, 5> possible_depth_indicators = {"color", "COLOR", "Color", "rgb", "RGB"};
+	return HasSubstringFromSet(filename.string(), possible_depth_indicators.data(), possible_depth_indicators.size());
+}
+
+
+bool FetchInterface::FilenameIndicatesMaskImage(const path& filename,
+                                                              const std::string& valid_extension)
+{
+	if (filename.extension() != valid_extension) return false;
+	const std::array<std::string, 3> possible_depth_indicators = {"mask", "Mask", "MASK"};
+	return HasSubstringFromSet(filename.string(), possible_depth_indicators.data(), possible_depth_indicators.size());
+}
+
+int FetchInterface::GetFrameNumber(const path& filename)        // 将图片名字中数字字符串转换成整数
+{
+	const std::regex digits_regex("\\d+");
+	std::smatch match_result;
+	const std::string filename_stem = filename.stem().string();
+	if (!std::regex_search(filename_stem, match_result, digits_regex)) {
+		throw std::runtime_error("Could not find frame number in filename.");
+	};
+	return std::stoi(match_result.str(0));
+}
+
+/**
+ * @brief 根据深度图像路径读取深度图
+ * 
+ */
+void FetchInterface::FetchDepthImage(size_t frame_idx, cv::Mat& depth_img)
+{
+	path file_path = this->m_depth_image_paths[frame_idx];
+	// Read the image
+	depth_img = cv::imread(file_path.string(), CV_ANYCOLOR | CV_ANYDEPTH);
+	// 如果提供了掩码图，那么将深度图中掩码图为0的像素置为0
+	if (this->m_use_masks) {
+		mask_mutex.lock();
+		if (this->m_mask_buffer_ix != frame_idx) {
+			m_mask_image_buffer = cv::imread(this->m_mask_image_paths[frame_idx].string(),
+											 CV_ANYCOLOR | CV_ANYDEPTH);
+			m_mask_buffer_ix = frame_idx;
+		}
+		cv::Mat masked;
+		// m_mask_image_buffer的非零元素对应的depth_img的元素保留，其余元素置为0
+		depth_img.copyTo(masked, m_mask_image_buffer);
+		mask_mutex.unlock();
+		depth_img = masked;
+	}
+}
+
+
+void FetchInterface::FetchRGBImage(size_t frame_idx, cv::Mat& rgb_img)
+{
+	path file_path = this->m_rgb_image_paths[frame_idx];
+	//Read the image
+	rgb_img = cv::imread(file_path.string(), CV_ANYCOLOR | CV_ANYDEPTH); 
+
+	if (this->m_use_masks) {
+		mask_mutex.lock();
+		if (this->m_mask_buffer_ix != frame_idx) {
+			m_mask_image_buffer = cv::imread(this->m_mask_image_paths[frame_idx].string(),
+			                                 CV_ANYCOLOR | CV_ANYDEPTH); 
+			m_mask_buffer_ix = frame_idx;
+		}
+		cv::Mat masked;
+		// m_mask_image_buffer的非零元素对应的depth_img的元素保留，其余元素置为0
+		rgb_img.copyTo(masked, m_mask_image_buffer);
+		mask_mutex.unlock();
+		rgb_img = masked;
+	}
+}
diff --git a/embedded_deformation/src/imageProcessor/imageProcessor.cpp b/embedded_deformation/src/imageProcessor/imageProcessor.cpp
new file mode 100644
index 0000000..3f72748
--- /dev/null
+++ b/embedded_deformation/src/imageProcessor/imageProcessor.cpp
@@ -0,0 +1,431 @@
+#include "imageProcessor/imageProcessor.h"
+
+imageProcessor::imageProcessor( Intrinsic raw_Intrinsic,
+                                const unsigned width, 
+                                const unsigned height, 
+                                int start_frame,
+                                std::shared_ptr<FetchInterface> image_fetcher
+                                ):
+m_raw_intrinsic(raw_Intrinsic),
+m_raw_width(width),
+m_raw_height(height),
+m_clip_width(width-2*boundary_clip),
+m_clip_height(height-2*boundary_clip),
+m_frame_idx(start_frame),
+m_image_fetcher(image_fetcher)
+{   
+    m_clip_intrinsic = Intrinsic(m_raw_intrinsic.focal_x, 
+                                m_raw_intrinsic.focal_y,
+                                m_raw_intrinsic.principal_x-boundary_clip, 
+                                m_raw_intrinsic.principal_y-boundary_clip);
+
+    // m_depth_image = cv::imread(m_depth_path, cv::IMREAD_UNCHANGED);
+    // m_mask_image = cv::imread(m_mask_path, cv::IMREAD_GRAYSCALE);
+    // if(m_depth_image.empty()) {
+    //     std::cerr << "Error: Unable to load depth image from " << m_depth_path << std::endl;
+    // }
+    // if (m_mask_image.empty()) {
+    //     std::cerr << "Error: Unable to load mask image from " << m_mask_path<< std::endl;
+    // }
+}
+
+
+imageProcessor::~imageProcessor()
+{
+    releaseDepthTexture();
+    releaseFetchBuffer();
+    releaseVertexTexture();
+    releaseNormalTexture();
+}
+
+// 分配与释放内存
+// 分配用于主机与设备之间数据传输的图像和锁页
+void imageProcessor::allocateFetchBuffer()
+{
+    const auto raw_img_size = m_raw_width * m_raw_height;
+    cudaSafeCall(cudaMallocHost(&m_depth_buffer_pagelock, sizeof(unsigned short)*raw_img_size));
+    cudaSafeCall(cudaMallocHost(&m_depth_prev_buffer_pagelock,sizeof(unsigned short)*raw_img_size));
+    cudaSafeCall(cudaDeviceSynchronize());
+    cudaSafeCall(cudaGetLastError());
+    m_depth_image = cv::Mat(cv::Size(m_raw_width, m_raw_height), CV_16UC1);
+    m_depth_image_prev = cv::Mat(cv::Size(m_raw_width, m_raw_height), CV_16UC1);
+}
+
+void imageProcessor::releaseFetchBuffer()
+{
+    cudaSafeCall(cudaFreeHost(m_depth_buffer_pagelock));
+    // cudaSafeCall(cudaFreeHost(m_rgb_buffer_packlock));
+    cudaSafeCall(cudaFreeHost(m_depth_prev_buffer_pagelock));
+    cudaSafeCall(cudaDeviceSynchronize());
+    cudaSafeCall(cudaGetLastError());
+}
+
+void imageProcessor::allocateDepthTexture()
+{
+    createDepthTextureSurface(m_raw_width, m_raw_height,
+                                depth_texture_surface_raw.texture,
+                                depth_texture_surface_raw.surface,
+                                depth_texture_surface_raw.array);
+    
+    createDepthTextureSurface(m_raw_width, m_raw_height,
+                                depth_texture_surface_raw_prev.texture,
+                                depth_texture_surface_raw_prev.surface,
+                                depth_texture_surface_raw_prev.array);
+
+    createDepthTextureSurface(m_clip_width, m_clip_height,
+                                depth_texture_surface_filter.texture,
+                                depth_texture_surface_filter.surface,
+                                depth_texture_surface_filter.array);
+    
+    createDepthTextureSurface(m_clip_width, m_clip_height,
+                                depth_texture_surface_filter_prev.texture,
+                                depth_texture_surface_filter_prev.surface,
+                                depth_texture_surface_filter_prev.array);
+}
+
+void imageProcessor::releaseDepthTexture()
+{
+    releaseTexture(depth_texture_surface_raw);
+    releaseTexture(depth_texture_surface_filter);
+    releaseTexture(depth_texture_surface_raw_prev);
+    releaseTexture(depth_texture_surface_filter_prev);
+
+    if(h_depthData != nullptr)
+    {
+        delete[] h_depthData;
+        h_depthData = nullptr;
+    }
+
+}
+
+void imageProcessor::allocateVertexTexture()
+{
+    createFloat4TextureSurface(m_clip_width, m_clip_height,
+                                vertex_texture_surface.texture,
+                                vertex_texture_surface.surface,
+                                vertex_texture_surface.array);
+    
+    createFloat4TextureSurface(m_clip_width, m_clip_height,
+                                vertex_texture_surface_prev.texture,
+                                vertex_texture_surface_prev.surface,
+                                vertex_texture_surface_prev.array);
+}
+
+void imageProcessor::releaseVertexTexture()
+{
+    releaseTexture(vertex_texture_surface);
+    releaseTexture(vertex_texture_surface_prev);
+
+    if(h_vertexData != nullptr){
+        delete[] h_vertexData;
+        h_vertexData = nullptr;
+    }
+
+    if(h_vertexData_prev != nullptr){
+        delete[] h_vertexData_prev;
+        h_vertexData_prev = nullptr;
+    }
+}
+
+void imageProcessor::allocateNormalTexture()
+{
+    createFloat4TextureSurface(m_clip_width, m_clip_height,
+                                normal_texture_surface.texture,
+                                normal_texture_surface.surface,
+                                normal_texture_surface.array);
+    
+    createFloat4TextureSurface(m_clip_width, m_clip_height,
+                                normal_texture_surface_prev.texture,
+                                normal_texture_surface_prev.surface,
+                                normal_texture_surface_prev.array);
+}
+
+void imageProcessor::releaseNormalTexture()
+{
+    releaseTexture(normal_texture_surface);
+    releaseTexture(normal_texture_surface_prev);
+
+    if(h_normalData != nullptr){
+        delete[] h_normalData;
+        h_normalData = nullptr;
+    }
+
+    if(h_normalData_prev != nullptr){
+        delete[] h_normalData_prev;
+        h_normalData_prev = nullptr;
+    }
+}
+
+void imageProcessor::allocateSurfelBuffer()
+{
+    const auto num_pixels = m_clip_height*m_clip_width;
+    m_surfels.AllocateBuffer(num_pixels);
+    m_surfels_prev.AllocateBuffer(num_pixels);
+}
+
+// 图像抓取
+void imageProcessor::FetchDepthImage(size_t frame_idx)
+{
+    m_image_fetcher->FetchDepthImage(frame_idx, m_depth_image);
+    memcpy(m_depth_buffer_pagelock, m_depth_image.data, 
+            sizeof(unsigned short)*m_raw_width*m_raw_height);
+    cudaSafeCall(cudaGetLastError());
+}
+
+void imageProcessor::FetchDepthPrevImage(size_t frame_idx)
+{
+    size_t prev_frame_idx = frame_idx - 10;
+    m_image_fetcher->FetchDepthImage(prev_frame_idx, m_depth_image_prev);
+    memcpy(m_depth_prev_buffer_pagelock, m_depth_image_prev.data,
+            sizeof(unsigned short)*m_raw_width*m_raw_height);
+}
+
+void imageProcessor::FetchRGBImage(size_t frame_idx)
+{
+    m_image_fetcher->FetchRGBImage(frame_idx, m_rgb_image);
+    memcpy(m_rgb_buffer_pagelock, m_rgb_image.data,
+                sizeof(uchar3) * m_raw_width * m_raw_height);
+}
+
+void imageProcessor::FetchRGBPrevImage(size_t frame_idx)
+{
+    size_t prev_frame_idx = frame_idx - 3;
+    m_image_fetcher->FetchRGBImage(prev_frame_idx, m_rgb_image_prev);
+    memcpy(m_rgb_prev_buffer_pagelock, m_rgb_image_prev.data,
+            sizeof(uchar3)*m_raw_width*m_raw_height);
+}
+
+void imageProcessor::FetchFrame(size_t frame_idx)
+{   
+    if(frame_idx == 0){
+        no_prev_frame = true;
+        return;
+    } 
+
+    FetchDepthImage(frame_idx);
+
+    if(!no_prev_frame){
+        FetchDepthPrevImage(frame_idx);
+    }
+    
+}
+
+void imageProcessor::UploadDepthImage(cudaStream_t stream)
+{
+    cudaSafeCall(cudaMemcpyToArrayAsync(
+        depth_texture_surface_raw.array,
+        0,0,
+        m_depth_buffer_pagelock,
+        sizeof(unsigned short)*m_raw_width*m_raw_height,
+        cudaMemcpyHostToDevice,
+        stream
+    ));
+    
+    if(!no_prev_frame){
+        cudaSafeCall(cudaMemcpyToArrayAsync(
+            depth_texture_surface_raw_prev.array,
+            0,0,
+            m_depth_prev_buffer_pagelock,
+            sizeof(unsigned short)*m_raw_width*m_raw_height,
+            cudaMemcpyHostToDevice,
+            stream
+        )); 
+    }
+    
+}
+
+void imageProcessor::FilterDepthImage(cudaStream_t stream)
+{
+    BilateralFilterDepth(
+        depth_texture_surface_raw.texture,
+        depth_texture_surface_filter.surface,
+        m_raw_width, m_raw_height,
+        m_clip_width, m_clip_height,
+        stream
+    );
+
+    if(!no_prev_frame){
+        BilateralFilterDepth(
+            depth_texture_surface_raw_prev.texture,
+            depth_texture_surface_filter_prev.surface,
+            m_raw_width, m_raw_height,
+            m_clip_width, m_clip_height,
+            stream
+        );
+    }
+}
+
+void imageProcessor::BuildVertexMap(cudaStream_t stream)
+{
+    createVertexMap(
+        depth_texture_surface_filter.texture,
+        m_clip_intrinsic,
+        m_clip_width, m_clip_height,
+        vertex_texture_surface.surface,
+        stream
+    );
+
+    if(!no_prev_frame){
+        createVertexMap(
+            depth_texture_surface_filter_prev.texture,
+            m_clip_intrinsic,
+            m_clip_width, m_clip_height,
+            vertex_texture_surface_prev.surface,
+            stream
+        );
+    }
+}
+
+void imageProcessor::BuildNormalMap(cudaStream_t stream)
+{
+    createNormalMap(
+        vertex_texture_surface.texture,
+        m_clip_width, m_clip_height,
+        normal_texture_surface.surface,
+        stream
+    ); 
+
+    if(!no_prev_frame){
+        createNormalMap(
+            vertex_texture_surface_prev.texture,
+            m_clip_width, m_clip_height,
+            normal_texture_surface_prev.surface,
+            stream
+        );
+    }
+}
+
+void imageProcessor::DownloadDepthData(cudaStream_t stream)
+{
+    h_depthData = (unsigned short*)malloc(sizeof(unsigned short)*m_clip_width*m_clip_height);
+    cudaSafeCall(cudaMemcpyFromArray(h_depthData, depth_texture_surface_filter_prev.array, 
+                        0, 0, sizeof(unsigned short)*m_clip_width*m_clip_height, 
+                        cudaMemcpyDeviceToHost));
+    
+    cv::Mat filtered_depth(m_clip_height, m_clip_width, CV_16UC1, h_depthData);
+    cv::Mat normalizedFilteredDepth;
+    cv::normalize(filtered_depth, normalizedFilteredDepth, 0, 255, cv::NORM_MINMAX, CV_8UC1);
+    cv::namedWindow("Normalized Filtered Depth Image", cv::WINDOW_AUTOSIZE);
+    cv::imshow("Normalized Filtered Depth Image", normalizedFilteredDepth);
+    
+    cv::Mat normalizedDepth;
+    cv::normalize(m_depth_image_prev, normalizedDepth, 0, 255, cv::NORM_MINMAX, CV_8UC1);
+    cv::namedWindow("Normalized Depth Image", cv::WINDOW_AUTOSIZE);
+    cv::imshow("Normalized Depth Image", normalizedDepth);
+    cv::waitKey(0); // 等待按键继续
+    cv::destroyAllWindows(); // 关闭所有窗口
+}
+
+void imageProcessor::DownloadVertexNormalData(cudaStream_t stream)
+{
+    
+    // h_vertexData = (float4*)malloc(sizeof(float4)*m_clip_width*m_clip_height);
+    // cudaSafeCall(cudaMemcpyFromArray(h_vertexData, vertex_texture_surface.array, 
+    //                     0, 0, sizeof(float4)*m_clip_width*m_clip_height, 
+    //                     cudaMemcpyDeviceToHost));
+    
+    // h_vertexData_prev = (float4*)malloc(sizeof(float4)*m_clip_width*m_clip_height);
+    // cudaSafeCall(cudaMemcpyFromArray(h_vertexData_prev, vertex_texture_surface_prev.array, 
+    //                     0, 0, sizeof(float4)*m_clip_width*m_clip_height, 
+    //                     cudaMemcpyDeviceToHost));
+    
+    // h_normalData = (float4*) malloc(sizeof(float4)* m_clip_width* m_clip_height);
+    // cudaSafeCall(cudaMemcpyFromArray(h_normalData, normal_texture_surface.array,
+    //                     0, 0, sizeof(float4)* m_clip_width* m_clip_height,
+    //                     cudaMemcpyDeviceToHost));
+    // h_normalData_prev = (float4*) malloc(sizeof(float4)* m_clip_width* m_clip_height);
+    // cudaSafeCall(cudaMemcpyFromArray(h_normalData_prev, normal_texture_surface_prev.array,
+    //                     0, 0, sizeof(float4)* m_clip_width* m_clip_height,
+    //                     cudaMemcpyDeviceToHost));
+
+    
+    // std::vector<glm::vec3> points;
+    // std::vector<glm::vec3> points_prev;
+    // std::vector<glm::vec3> normals;
+    // std::vector<glm::vec3> normals_prev;
+
+    // for(int i=0; i<m_clip_height; i++)
+    // {
+    //     for(int j=0; j<m_clip_width; j++)
+    //     {   
+    //         float x = h_vertexData[i*m_clip_width+j].x;
+    //         float y = h_vertexData[i*m_clip_width+j].y;
+    //         float z = h_vertexData[i*m_clip_width+j].z;
+            // float x_prev = h_vertexData_prev[i*m_clip_width+j].x;
+            // float y_prev = h_vertexData_prev[i*m_clip_width+j].y;
+            // float z_prev = h_vertexData_prev[i*m_clip_width+j].z;
+            // float nx = h_normalData[i*m_clip_width+j].x;
+            // float ny = h_normalData[i*m_clip_width+j].y;
+            // float nz = h_normalData[i*m_clip_width+j].z;
+            // float nx_prev = h_normalData_prev[i*m_clip_width+j].x;
+            // float ny_prev = h_normalData_prev[i*m_clip_width+j].y;
+            // float nz_prev = h_normalData_prev[i*m_clip_width+j].z;
+
+            // points.push_back(glm::vec3(x, y, z));
+            // normals.push_back(glm::vec3(nx, ny, nz));
+            // points_prev.push_back(glm::vec3(x_prev, y_prev, z_prev));
+            // normals_prev.push_back(glm::vec3(nx_prev, ny_prev, nz_prev));
+    //     }
+    // }
+    // polyscope::init();
+    // 注册第一组点云并设置为红色
+    // auto* psCloud1 = polyscope::registerPointCloud("Point Cloud 1", points);
+    // polyscope::getPointCloud("Point Cloud 1")->addVectorQuantity("normals 1", normals);
+    // psCloud1->setPointColor(glm::vec3(1.0, 0.0, 0.0)); // RGB颜色，这里是红色
+
+    // // 注册第二组点云并设置为蓝色
+    // auto* psCloud2 = polyscope::registerPointCloud("Point Cloud 2", points_prev);
+    // psCloud2->setPointColor(glm::vec3(0.0, 0.0, 1.0)); // RGB颜色，这里是蓝色
+    // polyscope::getPointCloud("Point Cloud 2")->addVectorQuantity("normals 2", normals_prev);
+    // polyscope::show();
+
+    
+}
+
+void imageProcessor::CollectValidSurfelData(cudaStream_t stream)
+{
+    int valid_surfel_count = 0;
+    DeviceArray<Surfel> valid_surfels = m_surfels.Array();
+    CollectValidData(
+        vertex_texture_surface.texture,
+        normal_texture_surface.texture,
+        m_clip_width, m_clip_height,
+        valid_surfels,
+        valid_surfel_count,
+        stream
+    );
+    m_surfels.ResizeArrayOrException(valid_surfel_count);
+
+    int valid_surfel_count_prev = 0;
+    DeviceArray<Surfel> valid_surfels_prev = m_surfels_prev.Array();
+    if(!no_prev_frame) {
+    CollectValidData(
+        vertex_texture_surface_prev.texture,
+        normal_texture_surface_prev.texture,
+        m_clip_width, m_clip_height,
+        valid_surfels_prev,
+        valid_surfel_count_prev,
+        stream
+    );
+    m_surfels_prev.ResizeArrayOrException(valid_surfel_count_prev);
+    }
+}
+
+void imageProcessor::test()
+{
+    allocateFetchBuffer();
+    allocateDepthTexture();
+    allocateVertexTexture();
+    allocateNormalTexture();
+    allocateSurfelBuffer();
+    FetchFrame(m_frame_idx);
+    UploadDepthImage();
+    FilterDepthImage();
+    // DownloadDepthData()
+    BuildVertexMap();
+    BuildNormalMap();
+    CollectValidSurfelData();
+    // DownloadVertexNormalData();
+    
+    
+}
diff --git a/embedded_deformation/src/math/device_mat.cpp b/embedded_deformation/src/math/device_mat.cpp
new file mode 100644
index 0000000..a682395
--- /dev/null
+++ b/embedded_deformation/src/math/device_mat.cpp
@@ -0,0 +1,58 @@
+#include "imageProcessor/common_types.hpp"
+#include "math/device_mat.h"
+#include "math/eigen_device_transfer.h"
+#include "math/vector_ops.hpp"
+
+
+
+mat33::mat33(const Eigen::Matrix3f &rhs) {
+	cols[0] = make_float3(rhs(0, 0), rhs(1, 0), rhs(2, 0));
+	cols[1] = make_float3(rhs(0, 1), rhs(1, 1), rhs(2, 1));
+	cols[2] = make_float3(rhs(0, 2), rhs(1, 2), rhs(2, 2));
+}
+
+mat33& mat33::operator=(const Eigen::Matrix3f &rhs) {
+	cols[0] = make_float3(rhs(0, 0), rhs(1, 0), rhs(2, 0));
+	cols[1] = make_float3(rhs(0, 1), rhs(1, 1), rhs(2, 1));
+	cols[2] = make_float3(rhs(0, 2), rhs(1, 2), rhs(2, 2));
+	return *this;
+}
+
+mat34::mat34(const Isometry3f &se3) : rot(se3.linear().matrix()) {
+	Eigen::Vector3f translation = se3.translation();
+	trans = fromEigen(translation);
+}
+
+mat34::mat34(const Matrix4f &rhs) : rot(rhs.block<3, 3>(0, 0)) {
+	Eigen::Vector3f eigen_trans = rhs.block<3, 1>(0, 3);
+	trans = fromEigen(eigen_trans);
+}
+
+mat34::mat34(const ::float3 &twist_rot, const ::float3 &twist_trans) {
+	if (fabsf_sum(twist_rot) < 1e-4f) {
+		rot.set_identity();
+	}
+	else {
+		float angle = ::norm(twist_rot);
+		float3 axis = (1.0f / angle) * twist_rot;
+		
+		float c = cosf(angle);
+		float s = sinf(angle);
+		float t = 1.0f - c;
+		
+		rot.m00() = t*axis.x*axis.x + c;
+		rot.m01() = t*axis.x*axis.y - axis.z*s;
+		rot.m02() = t*axis.x*axis.z + axis.y*s;
+		
+		rot.m10() = t*axis.x*axis.y + axis.z*s;
+		rot.m11() = t*axis.y*axis.y + c;
+		rot.m12() = t*axis.y*axis.z - axis.x*s;
+		
+		rot.m20() = t*axis.x*axis.z - axis.y*s;
+		rot.m21() = t*axis.y*axis.z + axis.x*s;
+		rot.m22() = t*axis.z*axis.z + c;
+	}
+	
+	//The translation part
+	trans = twist_trans;
+}
diff --git a/embedded_deformation/src/math/eigen_device_transfer.cpp b/embedded_deformation/src/math/eigen_device_transfer.cpp
new file mode 100644
index 0000000..4ddfe1b
--- /dev/null
+++ b/embedded_deformation/src/math/eigen_device_transfer.cpp
@@ -0,0 +1,80 @@
+#include "math/eigen_device_transfer.h"
+
+Eigen::Matrix3f toEigen(const mat33 &rhs) {
+    Matrix3f lhs;
+    lhs(0, 0) = rhs.m00();
+    lhs(0, 1) = rhs.m01();
+    lhs(0, 2) = rhs.m02();
+    lhs(1, 0) = rhs.m10();
+    lhs(1, 1) = rhs.m11();
+    lhs(1, 2) = rhs.m12();
+    lhs(2, 0) = rhs.m20();
+    lhs(2, 1) = rhs.m21();
+    lhs(2, 2) = rhs.m22();
+    return lhs;
+}
+
+Eigen::Matrix4f toEigen(const mat34 &rhs) {
+    Matrix4f lhs;
+    lhs.setIdentity();
+    //The rotational part
+    lhs(0, 0) = rhs.rot.m00();
+    lhs(0, 1) = rhs.rot.m01();
+    lhs(0, 2) = rhs.rot.m02();
+    lhs(1, 0) = rhs.rot.m10();
+    lhs(1, 1) = rhs.rot.m11();
+    lhs(1, 2) = rhs.rot.m12();
+    lhs(2, 0) = rhs.rot.m20();
+    lhs(2, 1) = rhs.rot.m21();
+    lhs(2, 2) = rhs.rot.m22();
+    //The translation part
+    lhs.block<3, 1>(0, 3) = toEigen(rhs.trans);
+    return lhs;
+}
+
+
+Eigen::Vector3f toEigen(const float3 &rhs) {
+    Vector3f lhs;
+    lhs(0) = rhs.x;
+    lhs(1) = rhs.y;
+    lhs(2) = rhs.z;
+    return lhs;
+}
+
+Eigen::Vector4f toEigen(const float4 &rhs) {
+    Vector4f lhs;
+    lhs(0) = rhs.x;
+    lhs(1) = rhs.y;
+    lhs(2) = rhs.z;
+    lhs(3) = rhs.w;
+    return lhs;
+}
+
+float3 fromEigen(const Vector3f &rhs) {
+    float3 lhs;
+    lhs.x = rhs(0);
+    lhs.y = rhs(1);
+    lhs.z = rhs(2);
+    return lhs;
+}
+
+float4 fromEigen(const Vector4f &rhs) {
+    float4 lhs;
+    lhs.x = rhs(0);
+    lhs.y = rhs(1);
+    lhs.z = rhs(2);
+    lhs.w = rhs(3);
+    return lhs;
+}
+
+
+void fromEigen(
+        const Isometry3f &se3,
+        Quaternion &rotation,
+        float3 &translation
+) {
+    mat33 rot(se3.linear().matrix());
+    rotation = Quaternion(rot);
+    Vector3f trans_eigen = se3.translation();
+    translation = fromEigen(trans_eigen);
+}
\ No newline at end of file
diff --git a/embedded_deformation/src/rigidSolver/rigidSolver.cpp b/embedded_deformation/src/rigidSolver/rigidSolver.cpp
new file mode 100644
index 0000000..f42cd08
--- /dev/null
+++ b/embedded_deformation/src/rigidSolver/rigidSolver.cpp
@@ -0,0 +1,120 @@
+#include "rigidSolver/rigidSolver.h"
+
+RigidSolver::RigidSolver(const unsigned width,
+                        const unsigned height,
+                        const Intrinsic& intrinsic
+) {
+    m_project_intrinsic = intrinsic;
+    m_image_width = width;
+    m_image_height = height;
+    m_curr_refer2live = mat34::identity();
+
+    allocateReduceBuffer();
+
+}
+
+RigidSolver::~RigidSolver(){
+
+}
+
+void RigidSolver::SetInputMaps(
+    const vertex_normal_maps& live_maps,
+    const vertex_normal_maps& reference_maps,
+    const mat34& init_refer2live
+) {
+    m_live_maps.vertex_map = live_maps.vertex_map;
+    m_live_maps.normal_map = live_maps.normal_map;
+    m_reference_maps.vertex_map = reference_maps.vertex_map;
+    m_reference_maps.normal_map = reference_maps.normal_map;
+    m_curr_refer2live = init_refer2live;
+}
+
+mat34 RigidSolver::Solve(int max_iters, cudaStream_t stream) {
+    for(int i = 0; i< max_iters; i++){
+        rigidSolverDeviceIteration(stream);
+        rigidSolverHostIterationSync(stream);
+    }
+    return m_curr_refer2live;
+}
+
+void RigidSolver::rigidSolverHostIterationSync(cudaStream_t stream) {
+    cudaSafeCall(cudaStreamSynchronize(stream));
+
+    const auto& host_array = m_reduced_matrix_vector.HostArray();
+
+    auto shift = 0;
+    for(int i = 0; i < 6; i++){
+        for(int j = i; j < 6; j++){
+            const float value = host_array[shift++];
+            JtJ_(i,j) = value;
+            JtJ_(j,i) = value;
+        }
+    }
+    for (int i = 0; i < 6; i++) {
+		const float value = host_array[shift++];
+		JtErr_[i] = value;
+	}
+
+    //Solve it
+	Eigen::Matrix<float, 6, 1> x = JtJ_.llt().solve(JtErr_).cast<float>();
+
+	//Update the se3
+	const float3 twist_rot = make_float3(x(0), x(1), x(2));
+	const float3 twist_trans = make_float3(x(3), x(4), x(5));
+	const mat34 se3_update(twist_rot, twist_trans);
+	m_curr_refer2live = se3_update * m_curr_refer2live;
+
+}
+
+void RigidSolver::findCorrespondences(cudaStream_t stream) {
+    int correspondence_count = 0;
+    DeviceArray<correspondence> correspondences = m_correspondences.Array();
+    FindCorrespondences(correspondences,
+                        correspondence_count);
+    m_correspondences.ResizeArrayOrException(correspondence_count);
+    std::cout<<"find " << correspondence_count << " correspondences"<<std::endl;
+
+}
+
+void RigidSolver::findCorrespondences(Eigen::MatrixXd& source_vertex,
+                                    Eigen::MatrixXd& source_normal,
+                                    DeviceBufferArray<correspondence>& correspondences,
+                                    cudaStream_t stream) {
+    int correspondence_count = 0;
+    DeviceArray<correspondence> correspondences_array = correspondences.Array();
+    FindCorrespondences(correspondences_array,
+                        source_vertex,
+                        source_normal,
+                        correspondence_count,
+                        stream);
+    correspondences.ResizeArrayOrException(correspondence_count);
+    std::cout<<"find new " << correspondence_count << " correspondences"<<std::endl;
+}
+
+Eigen::Matrix<float, 3, 4> RigidSolver::test(
+    const vertex_normal_maps& live_maps,
+    const vertex_normal_maps& reference_maps
+){
+    SetInputMaps(live_maps, reference_maps, mat34::identity());
+    const mat34 refer2live = Solve();
+    Eigen::Matrix<float, 3, 4> init_guess;
+    init_guess << refer2live.rot.cols[0].x, refer2live.rot.cols[1].x, refer2live.rot.cols[2].x, refer2live.trans.x,
+                refer2live.rot.cols[0].y, refer2live.rot.cols[1].y, refer2live.rot.cols[2].y, refer2live.trans.y,
+                refer2live.rot.cols[0].z, refer2live.rot.cols[1].z, refer2live.rot.cols[2].z, refer2live.trans.z;
+    // std::cout << "refer2live: " <<std::endl;
+    // std::cout << refer2live.rot.cols[0].x << " " 
+    //         << refer2live.rot.cols[0].y << " " 
+    //         << refer2live.rot.cols[0].z << " " 
+    //         << refer2live.trans.x << std::endl;
+    // std::cout<< refer2live.rot.cols[1].x << " " 
+    //         << refer2live.rot.cols[1].y << " " 
+    //         << refer2live.rot.cols[1].z << " "
+    //         << refer2live.trans.y << std::endl;
+    // std::cout<< refer2live.rot.cols[2].x << " " 
+    //         << refer2live.rot.cols[2].y << " " 
+    //         << refer2live.rot.cols[2].z << " "
+    //         << refer2live.trans.z << std::endl;
+    findCorrespondences();
+    return init_guess;
+
+}
\ No newline at end of file
diff --git a/embedded_deformation/src/rigidSolver/rigidSolver.cu b/embedded_deformation/src/rigidSolver/rigidSolver.cu
new file mode 100644
index 0000000..1b3386d
--- /dev/null
+++ b/embedded_deformation/src/rigidSolver/rigidSolver.cu
@@ -0,0 +1,353 @@
+#include "imageProcessor/common_types.hpp"
+#include "math/device_mat.h"
+#include "rigidSolver/device_intrinsics.h"
+#include "rigidSolver/rigidSolver.h"
+
+namespace device {
+    struct RigidSolverDevice {
+
+		//The constants for matrix size and blk size
+		enum
+		{
+			//The stoge layout
+            // 27个数据项
+			lhs_matrix_size = 21,
+			rhs_vector_size = 6,
+			total_shared_size = lhs_matrix_size + rhs_vector_size,
+
+			//The block size
+			block_size = 256,
+			num_warps = block_size / 32,
+		};
+		
+		//The map from the renderer
+		struct {
+			cudaTextureObject_t vertex_map;
+			cudaTextureObject_t normal_map;
+		} reference_maps;
+
+		//The map from the depth image
+		struct {
+			cudaTextureObject_t vertex_map;
+			cudaTextureObject_t normal_map;
+		} live_maps;
+
+		//The camera information
+		mat34 init_refer2live;
+		Intrinsic intrinsic;
+
+		//The image information
+		unsigned image_rows;
+		unsigned image_cols;
+
+		//The processing interface
+		__device__ __forceinline__ void solverIteration(
+			PtrStep<float> reduce_buffer
+		) const {
+			const auto flatten_pixel_idx = threadIdx.x + blockDim.x * blockIdx.x;
+			const auto x = flatten_pixel_idx % image_cols;
+			const auto y = flatten_pixel_idx / image_cols;
+
+			//Prepare the jacobian and err
+			float jacobian[6] = {0};
+			float err = 0.0f;
+
+			// 范围检查
+			if(x < image_cols && y < image_rows)
+			{
+				// 参考帧 应该是从全局模型渲染得到
+				const float4 reference_v4 = tex2D<float4>(reference_maps.vertex_map, x, y);                           // 参考帧顶点的索引
+				const float4 reference_n4 = tex2D<float4>(reference_maps.normal_map, x, y);
+
+				// 转换到相机坐标系下
+				const auto reference_v = init_refer2live.rot * reference_v4 + init_refer2live.trans;
+				const auto reference_n = init_refer2live.rot * reference_n4;
+
+				// 反投影 当前帧顶点的索引
+				const ushort2 img_coord = {
+					__float2uint_rn(((reference_v.x / (reference_v.z + 1e-10)) * intrinsic.focal_x) + intrinsic.principal_x),
+					__float2uint_rn(((reference_v.y / (reference_v.z + 1e-10)) * intrinsic.focal_y) + intrinsic.principal_y)
+				};
+
+				// 范围检查
+				if(img_coord.x < image_cols && img_coord.y < image_rows)
+				{
+					// 当前帧定点图
+					const float4 live_v4 = tex2D<float4>(live_maps.vertex_map, img_coord.x, img_coord.y);
+					const float4 live_n4 = tex2D<float4>(live_maps.normal_map, img_coord.x, img_coord.y);
+
+					//Check correspondence
+					if(dotxyz(reference_n, live_n4) < 0.8f || squared_distance(reference_v, live_v4) > (0.01f * 0.01f) || is_zero_vertex(live_v4)) {
+						//Pass
+					}
+					else {
+		
+						err = dotxyz(live_n4, make_float4(live_v4.x - reference_v.x, live_v4.y - reference_v.y,  live_v4.z - reference_v.z, 0.0f));
+						*(float3*)jacobian = cross_xyz(reference_v, live_n4);
+						*(float3*)(jacobian + 3) = make_float3(live_n4.x, live_n4.y, live_n4.z);
+		
+					}
+				}
+			}
+
+			//Time to do reduction
+			__shared__ float reduce_mem[total_shared_size][num_warps];
+			unsigned shift = 0;
+			const auto warp_id = threadIdx.x >> 5;                              // 当前线程所在的warp ID
+			const auto lane_id = threadIdx.x & 31;                              // 当前线程在其所在warp中的位置
+
+			//Reduce on matrix
+			for (int i = 0; i < 6; i++) { //Row index
+				for (int j = i; j < 6; j++) { //Column index, the matrix is symmetry
+					float data = (jacobian[i] * jacobian[j]);
+					data = warp_scan(data);
+					// 检查当前线程是否是warp中的最后一个线程
+					if (lane_id == 31) {
+						reduce_mem[shift++][warp_id] = data;
+					}
+					//Another sync here for reduced mem
+					__syncthreads();
+				}
+			}
+
+			//Reduce on vector
+			for (int i = 0; i < 6; i++) {
+				float data = (err * jacobian[i]);
+				data = warp_scan(data);
+				if (lane_id == 31) {
+					reduce_mem[shift++][warp_id] = data;
+				}
+				//Another sync here for reduced mem
+				__syncthreads();
+			}
+
+			//Store the result to global memory
+			const auto flatten_blk = blockIdx.x;
+			for (int i = threadIdx.x; i < total_shared_size; i += 32) {
+				if (warp_id == 0) {
+					const auto warp_sum = reduce_mem[i][0] + reduce_mem[i][1] + reduce_mem[i][2] + reduce_mem[i][3] 
+							+ reduce_mem[i][4] + reduce_mem[i][5] + reduce_mem[i][6] + reduce_mem[i][7];
+					reduce_buffer.ptr(i)[flatten_blk] = warp_sum;
+				}
+			}
+		}
+	};
+
+    __global__ void rigidSolveIterationKernel(
+		const RigidSolverDevice solver,
+		PtrStep<float> reduce_buffer
+	) {
+		solver.solverIteration(reduce_buffer);
+	}
+
+    __global__ void columnReduceKernel(
+        const PtrStepSz<const float> global_buffer,
+        float* target
+    ) {
+        const auto idx = threadIdx.x;
+        const auto y = threadIdx.y + blockIdx.y * blockDim.y;
+        float sum = 0.0f;
+        for (auto i = threadIdx.x; i < global_buffer.cols; i += 32) {
+            sum += global_buffer.ptr(y)[i];
+        }
+
+        sum = warp_scan(sum);
+        if(idx==31){
+            target[y] = sum;
+        }
+    }
+
+	__global__ void FindCorresponceKernel(
+		cudaTextureObject_t reference_vertex_map,
+		cudaTextureObject_t reference_normal_map,
+		cudaTextureObject_t live_vertex_map,
+		cudaTextureObject_t live_normal_map,
+		PtrSz<correspondence> correspondence,
+		const unsigned width,
+		const unsigned height,
+		const Intrinsic intrinsic,
+		int* correspondence_counter,
+		const mat34 refer2live
+	) {
+		const auto x = threadIdx.x + blockIdx.x * blockDim.x;
+		const auto y = threadIdx.y + blockIdx.y * blockDim.y;
+		if( x >= width || y >= height) return;
+		const float4 reference_v4 = tex2D<float4>(reference_vertex_map, x, y);
+		const float4 reference_n4 = tex2D<float4>(reference_normal_map, x, y);
+		const float3 reference_v = refer2live.rot * reference_v4 + refer2live.trans;
+		const float3 reference_n = refer2live.rot * reference_n4;
+
+		const ushort2 img_coord = {
+					__float2uint_rn(((reference_v.x / (reference_v.z + 1e-10)) * intrinsic.focal_x) + intrinsic.principal_x),
+					__float2uint_rn(((reference_v.y / (reference_v.z + 1e-10)) * intrinsic.focal_y) + intrinsic.principal_y)
+				};
+		
+		
+		if(img_coord.x < width && img_coord.y < height)
+		{
+			// 当前帧定点图
+			const float4 live_v4 = tex2D<float4>(live_vertex_map, img_coord.x, img_coord.y);
+			const float4 live_n4 = tex2D<float4>(live_normal_map, img_coord.x, img_coord.y);
+			//Check correspondence
+			if(dotxyz(reference_n, live_n4) < 0.8f || squared_distance(reference_v, live_v4) > (0.01f * 0.01f) || is_zero_vertex(live_v4)) {
+				//Pass
+			}
+			else {
+				const int index = atomicAdd(correspondence_counter, 1);
+				correspondence[index].src_vertex = make_float4(reference_v.x, reference_v.y, reference_v.z, 0.0f);
+				correspondence[index].src_normal = make_float4(reference_n.x, reference_n.y, reference_n.z, 0.0f);
+				correspondence[index].tag_vertex = live_v4;
+				correspondence[index].tag_normal = live_n4;
+			}
+		}
+	}
+
+	__global__ void FindCorresponceKernel(
+		double* src_vertex,
+		double* src_normal,
+		cudaTextureObject_t live_vertex_map,
+		cudaTextureObject_t live_normal_map,
+		const unsigned width,
+		const unsigned height,
+		int boundary,
+		PtrSz<correspondence> correspondence,
+		const Intrinsic intrinsic,
+		int* correspondence_counter
+	) {
+		const auto x = threadIdx.x + blockIdx.x * blockDim.x;
+		if (x >= boundary) return;
+		
+		const float3 reference_v = make_float3(src_vertex[x], src_vertex[boundary+x], src_vertex[boundary*2+x]);
+		const float3 reference_n = make_float3(src_normal[x], src_normal[boundary+x], src_normal[boundary*2+x]);
+		const ushort2 img_coord = {
+					__float2uint_rn(((reference_v.x / (reference_v.z + 1e-10)) * intrinsic.focal_x) + intrinsic.principal_x),
+					__float2uint_rn(((reference_v.y / (reference_v.z + 1e-10)) * intrinsic.focal_y) + intrinsic.principal_y)
+				};
+		if(img_coord.x < width && img_coord.y < height)
+		{	
+			// 当前帧定点图
+			const float4 live_v4 = tex2D<float4>(live_vertex_map, img_coord.x, img_coord.y);
+			const float4 live_n4 = tex2D<float4>(live_normal_map, img_coord.x, img_coord.y);
+			//Check correspondence
+			if(dotxyz(reference_n, live_n4) < 0.8f || squared_distance(reference_v, live_v4) > (0.01f * 0.01f) || is_zero_vertex(live_v4)) {
+				//Pass
+			}
+			else {
+				const int index = atomicAdd(correspondence_counter, 1);
+				correspondence[index].src_vertex = make_float4(reference_v.x, reference_v.y, reference_v.z, 0.0f);
+				correspondence[index].src_normal = make_float4(reference_n.x, reference_n.y, reference_n.z, 0.0f);
+				correspondence[index].tag_vertex = live_v4;
+				correspondence[index].tag_normal = live_n4;
+
+			}
+		}
+	}
+}
+
+void RigidSolver::allocateReduceBuffer() {
+    m_reduced_matrix_vector.AllocateBuffer(device::RigidSolverDevice::total_shared_size);
+    m_reduced_matrix_vector.ResizeArrayOrException(device::RigidSolverDevice::total_shared_size);
+
+    const auto& pixel_size = m_image_height*m_image_width;
+    // 27项数据的global_buffer 共有27*num of blocks个数据 每一行代表27项中的一项数据
+    m_reduce_buffer.create(device::RigidSolverDevice::total_shared_size, 
+                            divUp(pixel_size,device::RigidSolverDevice::block_size));
+	m_correspondences.AllocateBuffer(pixel_size);
+}
+
+void RigidSolver::rigidSolverDeviceIteration(cudaStream_t stream) {
+    device::RigidSolverDevice solver;
+
+    solver.intrinsic = m_project_intrinsic;
+    solver.init_refer2live = m_curr_refer2live;
+    solver.image_rows = m_image_height;
+    solver.image_cols = m_image_width;
+    solver.reference_maps.vertex_map = m_reference_maps.vertex_map;
+    solver.reference_maps.normal_map = m_reference_maps.normal_map;
+    solver.live_maps.vertex_map = m_live_maps.vertex_map;
+    solver.live_maps.normal_map = m_live_maps.normal_map;
+
+    dim3 blk(device::RigidSolverDevice::block_size);
+    dim3 grid(divUp(m_image_width*m_image_height, device::RigidSolverDevice::block_size));
+
+    device::rigidSolveIterationKernel<<<grid, blk, 0, stream>>>(solver, m_reduce_buffer);
+
+    device::columnReduceKernel<<<dim3(1,1,1),dim3(32,device::RigidSolverDevice::total_shared_size,1)>>>(
+        m_reduce_buffer,
+        m_reduced_matrix_vector.DevicePtr()
+    );
+
+    m_reduced_matrix_vector.SynchronizeToHost(stream, false);
+
+}
+
+void RigidSolver::FindCorrespondences(DeviceArray<correspondence>& correspondences,
+										int& correspondence_count,
+										cudaStream_t stream
+) {
+	int* d_correspondence_count;
+	cudaMalloc((void**)&d_correspondence_count, sizeof(int));
+	cudaMemset(d_correspondence_count, 0, sizeof(int));
+	dim3 block(16, 16);
+	dim3 grid(divUp(m_image_width, block.x), divUp(m_image_height, block.y));
+	device::FindCorresponceKernel<<<grid, block, 0, stream>>>(
+		m_reference_maps.vertex_map,
+		m_reference_maps.normal_map,
+		m_live_maps.vertex_map,
+		m_live_maps.normal_map,
+		correspondences,
+		m_image_width,
+		m_image_height,
+		m_project_intrinsic,
+		d_correspondence_count,
+		m_curr_refer2live
+	);
+
+	int h_correspondence_count;
+	cudaSafeCall(cudaMemcpy(&h_correspondence_count, d_correspondence_count, sizeof(int), cudaMemcpyDeviceToHost));
+	correspondence_count = h_correspondence_count;
+	cudaFree(d_correspondence_count);
+}
+
+void RigidSolver::FindCorrespondences(DeviceArray<correspondence>& correspondences,
+									Eigen::MatrixXd& src_vertex,
+									Eigen::MatrixXd& src_normal,
+                            		int& correspondence_count,
+                            		cudaStream_t stream
+) {
+	int* d_correspondence_count;
+	cudaMalloc((void**)&d_correspondence_count, sizeof(int));
+	cudaMemset(d_correspondence_count, 0, sizeof(int));
+
+	double* d_src_vertex;
+	double* d_src_normal;
+	size_t src_size = sizeof(double) * src_vertex.rows() * src_vertex.cols();
+	// for(int i = 0; i < src_vertex.rows(); i++)
+	// {
+	// 	printf("src_vertex[%d]: %f %f %f\n", i, src_vertex(i, 0), src_vertex(i, 1), src_vertex(i, 2));
+	// }
+	cudaSafeCall(cudaMalloc((void**)&d_src_vertex, src_size));
+	cudaSafeCall(cudaMemcpy(d_src_vertex, src_vertex.data(),src_size, cudaMemcpyHostToDevice));
+	cudaSafeCall(cudaMalloc((void**)&d_src_normal, src_size));
+	cudaSafeCall(cudaMemcpy(d_src_normal, src_normal.data(),src_size, cudaMemcpyHostToDevice));
+
+	dim3 block(256,1);
+	dim3 grid(divUp(src_vertex.rows(), block.x));
+	device::FindCorresponceKernel<<<grid, block, 0, stream>>>(
+		d_src_vertex,
+		d_src_normal,
+		m_live_maps.vertex_map,
+		m_live_maps.normal_map,
+		m_image_width,
+		m_image_height,
+		src_vertex.rows(),
+		correspondences,
+		m_project_intrinsic,
+		d_correspondence_count
+	);
+
+	int h_correspondence_count;
+	cudaSafeCall(cudaMemcpy(&h_correspondence_count, d_correspondence_count, sizeof(int), cudaMemcpyDeviceToHost));
+	correspondence_count = h_correspondence_count;
+	cudaFree(d_correspondence_count);
+}