├── src
    ├── TriMesh.cc
    ├── TriMesh.h
    ├── DeformGraph.h
    ├── DeformGraphTest.cpp
    └── DeformGraph.cpp
├── DeformationGraph.vcxproj.user
├── .gitignore
├── README.md
├── DeformationGraph.vcxproj.filters
└── DeformationGraph.vcxproj


/src/TriMesh.cc:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/JiaxiangZheng/DeformationGraph/HEAD/src/TriMesh.cc


--------------------------------------------------------------------------------
/DeformationGraph.vcxproj.user:
--------------------------------------------------------------------------------
1 | <?xml version="1.0" encoding="utf-8"?>
2 | <Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
3 | </Project>


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
 1 | # Compiled Object files
 2 | *.slo
 3 | *.lo
 4 | *.o
 5 | 
 6 | # Compiled Dynamic libraries
 7 | *.so
 8 | *.dylib
 9 | 
10 | # Compiled Static libraries
11 | *.lai
12 | *.la
13 | *.a
14 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | DeformationGraph
 2 | ================
 3 | 
 4 | A implemetation of deformation graph which was widely used in mesh deformation and non-rigid reconstruction. I write this for the reason that I will use it in later animation reconstruction work.
 5 | 
 6 | ##about the code##
 7 | [Eigen](http://eigen.tuxfamily.org), [FLANN](http://www.cs.ubc.ca/~mariusm/index.php/FLANN/FLANN) and [PCL](http://www.pointclouds.org/) are used as third party library.
 8 | 
 9 | `DeformGraph.h` and `DeformGraph.cpp` contains the core code of deformation
10 | graph, and `DeformGraphTest.cpp` is an example for how to use the code. It
11 | should be note that the initial data should be aligned together and normalized.
12 | 
13 | 
14 | 


--------------------------------------------------------------------------------
/DeformationGraph.vcxproj.filters:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="utf-8"?>
 2 | <Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
 3 |   <ItemGroup>
 4 |     <Filter Include="Source Files">
 5 |       <UniqueIdentifier>{4FC737F1-C7A5-4376-A066-2A32D752A2FF}</UniqueIdentifier>
 6 |       <Extensions>cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx</Extensions>
 7 |     </Filter>
 8 |     <Filter Include="Header Files">
 9 |       <UniqueIdentifier>{93995380-89BD-4b04-88EB-625FBE52EBFB}</UniqueIdentifier>
10 |       <Extensions>h;hpp;hxx;hm;inl;inc;xsd</Extensions>
11 |     </Filter>
12 |     <Filter Include="Resource Files">
13 |       <UniqueIdentifier>{67DA6AB6-F800-4c08-8B7A-83BB121AAD01}</UniqueIdentifier>
14 |       <Extensions>rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms</Extensions>
15 |     </Filter>
16 |   </ItemGroup>
17 |   <ItemGroup>
18 |     <ClCompile Include="src\TriMesh.cc">
19 |       <Filter>Source Files</Filter>
20 |     </ClCompile>
21 |     <ClCompile Include="src\DeformGraph.cpp">
22 |       <Filter>Source Files</Filter>
23 |     </ClCompile>
24 |     <ClCompile Include="src\DeformGraphTest.cpp">
25 |       <Filter>Source Files</Filter>
26 |     </ClCompile>
27 |   </ItemGroup>
28 |   <ItemGroup>
29 |     <ClInclude Include="src\TriMesh.h">
30 |       <Filter>Header Files</Filter>
31 |     </ClInclude>
32 |     <ClInclude Include="src\DeformGraph.h">
33 |       <Filter>Header Files</Filter>
34 |     </ClInclude>
35 |   </ItemGroup>
36 | </Project>


--------------------------------------------------------------------------------
/src/TriMesh.h:
--------------------------------------------------------------------------------
 1 | #ifndef _TRI_MESH_HH_
 2 | #define _TRI_MESH_HH_
 3 | #include <vector>
 4 | #include <cmath>
 5 | using namespace std;
 6 | #include <Eigen/Dense>
 7 | 
 8 | #define ERROR_LOG(ERROR_INFO)		\
 9 | 	fprintf(stderr, "%s\t%s\t%d\n",	\
10 | 	ERROR_INFO, __FILE__, __LINE__)
11 | 
12 | #define DATA_09
13 | typedef enum DisplayMode {POINT_MODE, EDGE_MODE, FACE_MODE} DisplayMode;
14 | 
15 | class TriMesh
16 | {
17 | public:
18 | 	struct PolyIndex
19 | 	{
20 | 		unsigned int vert_index[3];
21 | 		unsigned int norm_index[3];
22 | 	};
23 | 	vector<Eigen::Vector3d> vertex_coord;//
24 | 	vector<Eigen::Vector3d> norm_coord;
25 | 	vector<PolyIndex> polyIndex;
26 | 	vector<Eigen::Vector3d> face_norm;
27 | 	Eigen::Vector3d BoundingBox_Min, BoundingBox_Max;	//min max
28 | 	int vert_num;
29 | 	int poly_num;
30 | 
31 | 	TriMesh() : vert_num(0), poly_num(0) {}
32 | 	TriMesh(const char* filename);
33 | 	void updateNorm();
34 | 	void render(DisplayMode mode);
35 | 	void prepareFaceNeighbours(std::vector<std::vector<int>>& neighbours);
36 | 	void saveOBJ(const char* filename);
37 | 	void savePLY(const char* filename);
38 | 	void getBoundingBox(Eigen::Vector3d& Min, Eigen::Vector3d& Max);
39 | private:
40 | 	void readPLY(const char* filename);
41 | 	void readOBJ(const char* filename);
42 | };
43 | 
44 | struct Skeleton
45 | {
46 | 	std::vector<Eigen::Vector3d> pos;
47 | 	std::vector<int>			 parents;
48 | 	Skeleton() {}
49 | 	Skeleton(const char* filename) {read(filename);}
50 | 	void read(const char* filename);
51 | 	void render(float sphereSize = 0.05, float lineWidth = 3.0);
52 | };
53 | 
54 | #endif/*_TRI_MESH_HH_*/
55 | 
56 | 
57 | 
58 | 


--------------------------------------------------------------------------------
/src/DeformGraph.h:
--------------------------------------------------------------------------------
 1 | #include <Eigen/Core>
 2 | #include <Eigen/Sparse>
 3 | #include <Eigen/SparseCholesky>
 4 | #include "TriMesh.h"
 5 | 
 6 | #define dprintf fprintf
 7 | 
 8 | struct DGraph {
 9 |     const TriMesh* p_mesh;                            // for data backup 
10 |     std::vector<int> node_index;
11 | 
12 |     std::vector<Eigen::Vector3d> node_pos;
13 |     std::vector<Eigen::Vector3d> node_norm;
14 | 
15 |     std::vector<std::vector<int>> node_neigh;
16 |     std::vector<Eigen::Matrix3d> node_rots;
17 |     std::vector<Eigen::Vector3d> node_trans;
18 | 
19 |     double max_dist_neigh;      // used for building neighbours
20 |     int    k_neigh;
21 |     DGraph() {
22 |         p_mesh = NULL;
23 |         max_dist_neigh = 0.0;
24 |         k_neigh = 0;
25 |     }
26 |     void build_graph(const TriMesh& mesh, std::vector<int>& sample_index, 
27 |         double _max_dist_neigh = 0.1, int k_nn = 10);
28 |     void build_neigh();
29 | 	void update_graph(const Eigen::VectorXd& X);
30 | 	void deform(TriMesh& d_mesh);
31 | };
32 | 
33 | struct Deformer {
34 |     DGraph* p_graph;
35 |     std::vector<double> weights;    //rot, smooth, regular
36 |     std::vector<int> constraints_index;
37 |     std::vector<Eigen::Vector3d> node_constraints;
38 | 
39 |     void init(DGraph& graph, const std::vector<int>& corr_indexs, 
40 |         const std::vector<Eigen::Vector3d>& cons);
41 |     void set_weights(double* _weights, int n) {
42 |         weights.resize(n); 
43 | 		for (int i=0; i<n; ++i) weights[i] = _weights[i];
44 |     }
45 |     void build_jacobi(const Eigen::VectorXd& x, Eigen::MatrixXd& fJac);
46 |     void build_jacobi(const Eigen::VectorXd& x, Eigen::SparseMatrix<double>& fJac);
47 |     void build_values(const Eigen::VectorXd& x, Eigen::VectorXd& fVec);
48 | 
49 |     int rows, cols;
50 | };
51 | 
52 | void mesh_sampling(const TriMesh& mesh, std::vector<int>& sample_index, double max_dist);
53 | double minimize_guass_newton(Deformer& deformer, Eigen::VectorXd& X);
54 | double minimize_levenberg_marquardt(Deformer& deformer, Eigen::VectorXd& X);
55 | double optimize_once(DGraph& graph, std::vector<int>& constraints_index,
56 | 	const std::vector<Eigen::Vector3d>& cons); //constraints_index is in respect to graph nodes' index
57 | 
58 | 


--------------------------------------------------------------------------------
/DeformationGraph.vcxproj:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="utf-8"?>
 2 | <Project DefaultTargets="Build" ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
 3 |   <ItemGroup Label="ProjectConfigurations">
 4 |     <ProjectConfiguration Include="Debug|Win32">
 5 |       <Configuration>Debug</Configuration>
 6 |       <Platform>Win32</Platform>
 7 |     </ProjectConfiguration>
 8 |     <ProjectConfiguration Include="Release|Win32">
 9 |       <Configuration>Release</Configuration>
10 |       <Platform>Win32</Platform>
11 |     </ProjectConfiguration>
12 |   </ItemGroup>
13 |   <ItemGroup>
14 |     <ClCompile Include="src\DeformGraph.cpp" />
15 |     <ClCompile Include="src\DeformGraphTest.cpp" />
16 |     <ClCompile Include="src\TriMesh.cc" />
17 |   </ItemGroup>
18 |   <ItemGroup>
19 |     <ClInclude Include="src\DeformGraph.h" />
20 |     <ClInclude Include="src\TriMesh.h" />
21 |   </ItemGroup>
22 |   <PropertyGroup Label="Globals">
23 |     <ProjectGuid>{ACB93890-4121-4A2E-952F-73CAD23E3736}</ProjectGuid>
24 |     <Keyword>Win32Proj</Keyword>
25 |     <RootNamespace>DeformationGraph</RootNamespace>
26 |   </PropertyGroup>
27 |   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
28 |   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
29 |     <ConfigurationType>Application</ConfigurationType>
30 |     <UseDebugLibraries>true</UseDebugLibraries>
31 |     <CharacterSet>Unicode</CharacterSet>
32 |   </PropertyGroup>
33 |   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
34 |     <ConfigurationType>Application</ConfigurationType>
35 |     <UseDebugLibraries>false</UseDebugLibraries>
36 |     <WholeProgramOptimization>true</WholeProgramOptimization>
37 |     <CharacterSet>Unicode</CharacterSet>
38 |   </PropertyGroup>
39 |   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
40 |   <ImportGroup Label="ExtensionSettings">
41 |   </ImportGroup>
42 |   <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
43 |     <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
44 |   </ImportGroup>
45 |   <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
46 |     <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
47 |   </ImportGroup>
48 |   <PropertyGroup Label="UserMacros" />
49 |   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
50 |     <LinkIncremental>true</LinkIncremental>
51 |     <EmbedManifest>false</EmbedManifest>
52 |   </PropertyGroup>
53 |   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
54 |     <LinkIncremental>false</LinkIncremental>
55 |   </PropertyGroup>
56 |   <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
57 |     <ClCompile>
58 |       <PrecompiledHeader>
59 |       </PrecompiledHeader>
60 |       <WarningLevel>Level3</WarningLevel>
61 |       <Optimization>Disabled</Optimization>
62 |       <PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
63 |       <AdditionalIncludeDirectories>D:\Program Files\Eigen\include;D:\Program Files\flann\include;D:\Program Files\PCL 1.6.0\include\pcl-1.6;D:\Program Files\Boost\include;</AdditionalIncludeDirectories>
64 |     </ClCompile>
65 |     <Link>
66 |       <SubSystem>Console</SubSystem>
67 |       <GenerateDebugInformation>true</GenerateDebugInformation>
68 |       <AdditionalLibraryDirectories>D:\Program Files\flann\lib;D:\Program Files\PCL 1.6.0\lib;D:\Program Files\Boost\lib;</AdditionalLibraryDirectories>
69 |       <AdditionalDependencies>flann.lib;pcl_keypoints_debug.lib;pcl_kdtree_debug.lib;pcl_common_debug.lib;pcl_search_debug.lib</AdditionalDependencies>
70 |     </Link>
71 |   </ItemDefinitionGroup>
72 |   <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
73 |     <ClCompile>
74 |       <WarningLevel>Level3</WarningLevel>
75 |       <PrecompiledHeader>
76 |       </PrecompiledHeader>
77 |       <Optimization>MaxSpeed</Optimization>
78 |       <FunctionLevelLinking>true</FunctionLevelLinking>
79 |       <IntrinsicFunctions>true</IntrinsicFunctions>
80 |       <PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
81 |       <AdditionalIncludeDirectories>D:\Program Files\Eigen\include;D:\Program Files\flann\include;D:\Program Files\PCL 1.6.0\include\pcl-1.6;D:\Program Files\Boost\include;</AdditionalIncludeDirectories>
82 |     </ClCompile>
83 |     <Link>
84 |       <SubSystem>Console</SubSystem>
85 |       <GenerateDebugInformation>true</GenerateDebugInformation>
86 |       <EnableCOMDATFolding>true</EnableCOMDATFolding>
87 |       <OptimizeReferences>true</OptimizeReferences>
88 |       <AdditionalLibraryDirectories>D:\Program Files\flann\lib;D:\Program Files\PCL 1.6.0\lib;D:\Program Files\Boost\lib;</AdditionalLibraryDirectories>
89 |       <AdditionalDependencies>flann.lib;pcl_keypoints_release.lib;pcl_kdtree_release.lib;pcl_common_release.lib;pcl_search_release.lib;</AdditionalDependencies>
90 |     </Link>
91 |   </ItemDefinitionGroup>
92 |   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
93 |   <ImportGroup Label="ExtensionTargets">
94 |   </ImportGroup>
95 | </Project>


--------------------------------------------------------------------------------
/src/DeformGraphTest.cpp:
--------------------------------------------------------------------------------
  1 | #include <flann/flann.hpp>
  2 | #include "TriMesh.h"
  3 | #include "DeformGraph.h"
  4 | /* debug utility functions */
  5 | ////////////////////////////////////////////////////////////////////////////////
  6 | static void _write_graphs(const char* filename, const std::vector<std::vector<int>>& neighs) {
  7 | 	FILE* fp = fopen(filename, "w");
  8 | 	if (!fp) return;
  9 | 	fprintf(fp, "%d\n", neighs.size());
 10 | 	for (int i=0; i<neighs.size(); ++i) {
 11 | 		fprintf(fp, "%d ", neighs[i].size());
 12 | 		for (int j=0; j<neighs[i].size(); ++j)
 13 | 			fprintf(fp, "%d ", neighs[i][j]);
 14 | 		fprintf(fp, "\n");
 15 | 	}
 16 | 	fclose(fp);
 17 | 	return;
 18 | }
 19 | static void _save_pcd2ply(std::vector<Eigen::Vector3d> node_pos,
 20 | std::vector<Eigen::Vector3d> node_norm, const char* filename) {
 21 |     assert (node_pos.size() == node_norm.size());
 22 |     FILE* fp = fopen(filename, "w");
 23 |     if (!fp) {
 24 |         fprintf(stderr, "warning : unable to open %s when saving pcd to ply file.\n", filename);
 25 |         return;
 26 |     }
 27 |     fprintf(fp, "ply\n");
 28 |     fprintf(fp, "format ascii 1.0\n");
 29 |     fprintf(fp, "element vertex %d\n", node_pos.size());
 30 |     fprintf(fp, "property float x\n");
 31 |     fprintf(fp, "property float y\n");
 32 |     fprintf(fp, "property float z\n");
 33 |     fprintf(fp, "property float nx\n");
 34 |     fprintf(fp, "property float ny\n");
 35 |     fprintf(fp, "property float nz\n");
 36 |     fprintf(fp, "property uchar red\n");
 37 |     fprintf(fp, "property uchar green\n");
 38 |     fprintf(fp, "property uchar blue\n");
 39 |     fprintf(fp, "end_header\n");
 40 |     
 41 |     for (size_t i=0; i<node_pos.size(); ++i) {
 42 |         fprintf(fp, "%f %f %f %f %f %f %d %d %d\n", 
 43 |             node_pos[i][0], node_pos[i][1], node_pos[i][2],
 44 |             node_norm[i][0], node_norm[i][1], node_norm[i][2],
 45 |             0, 255, 255);
 46 |     }
 47 |     fclose(fp);
 48 | }
 49 | static void _save_pcd2obj(std::vector<Eigen::Vector3d> node_pos,
 50 | std::vector<Eigen::Vector3d> node_norm, const char* filename) {
 51 | 	TriMesh mesh;
 52 | 	mesh.vert_num = node_norm.size();
 53 | 	mesh.vertex_coord = node_pos;
 54 | 	mesh.norm_coord = node_norm;
 55 | 	mesh.saveOBJ(filename);
 56 | }
 57 | ////////////////////////////////////////////////////////////////////////////////
 58 | 
 59 | int main() {
 60 |     // make sure these meshes are normalized, aligned rigidly and all vertex is valid (normal is valid)
 61 | 	const char* src_filename = "./data/hand_000.obj";
 62 | 	const char* dst_filename = "./data/hand_050.obj";
 63 |     TriMesh mesh_in(src_filename);
 64 |     TriMesh mesh_out(dst_filename);
 65 | 
 66 |     std::vector<int> sampling_index;
 67 | 	double sampling_dist = 0.03;
 68 |     mesh_sampling(mesh_in, sampling_index, sampling_dist);
 69 | 
 70 |     DGraph graph;
 71 |     graph.build_graph(mesh_in, sampling_index, 2.0*sampling_dist, 8);
 72 | 	_write_graphs("graph.txt", graph.node_neigh);	// for debug
 73 | 
 74 | 	double *raw_dataset = new double[3*mesh_out.vertex_coord.size()];
 75 |     for (size_t i=0; i<mesh_out.vertex_coord.size(); ++i) {
 76 |         raw_dataset[3*i+0] = mesh_out.vertex_coord[i][0];
 77 |         raw_dataset[3*i+1] = mesh_out.vertex_coord[i][1];
 78 |         raw_dataset[3*i+2] = mesh_out.vertex_coord[i][2];
 79 |     }
 80 |     flann::Matrix<double> flann_dataset(raw_dataset, mesh_out.vertex_coord.size(), 3);
 81 |     flann::Index< flann::L2<double> > flann_index(flann_dataset, flann::KDTreeIndexParams(1));
 82 |     flann_index.buildIndex();
 83 |     flann::Matrix<double>   query_node(new double[3], 1, 3);	// num of querys * dimension
 84 |     flann::Matrix<int>      indices_node(new int[1], 1, 1);		// num of querys * knn
 85 |     flann::Matrix<double>   dists_node(new double[1], 1, 1);
 86 |     
 87 | 	double max_dist_neigh = 2.0*sampling_dist;
 88 | 	char save_filename[512];
 89 | 	int iter = 0;
 90 | 	double pre_energy = DBL_MAX;
 91 | 	do {
 92 | 		// 1. build nearest correspondence from graph to mesh_out
 93 | 		std::vector<int> corres_indexs; std::vector<Eigen::Vector3d> corres_constraints;
 94 | 		for (size_t i=0; i<graph.node_pos.size(); ++i) {
 95 | 			query_node[0][0] = graph.node_pos[i][0]; query_node[0][1] = graph.node_pos[i][1]; query_node[0][2] = graph.node_pos[i][2];
 96 | 			flann_index.knnSearch(query_node, indices_node, dists_node, 1, flann::SearchParams(flann::FLANN_CHECKS_UNLIMITED));
 97 | 			if (dists_node[0][0] > max_dist_neigh*max_dist_neigh) continue;	//TODO: replace with a stronger condition for matching pairs
 98 | 			corres_indexs.push_back(i);
 99 | 			corres_constraints.push_back(mesh_out.vertex_coord[indices_node[0][0]]);
100 | 		}
101 | 		dprintf(stdout, "%d:\tfind pairs %d\n", iter, corres_indexs.size());
102 | 
103 |         // 2. use the correspondence to initialize a Deformer and call optimize_once and update graph
104 | 		double cur_energy = optimize_once(graph, corres_indexs, corres_constraints);
105 | 		dprintf(stdout, "min_energy = %lf\n", cur_energy);
106 | 		if (fabs(pre_energy-cur_energy) < 1e-5) break;
107 | 		pre_energy = cur_energy;
108 | 	} while (++iter < 50);
109 | 	sprintf(save_filename, "d_graph.ply");
110 | 	_save_pcd2ply(graph.node_pos, graph.node_norm, save_filename);
111 | 
112 | 	TriMesh source_mesh(src_filename);
113 | 	graph.deform(source_mesh);
114 | 	source_mesh.savePLY("deformed_mesh.ply");
115 | 	
116 | //	FILE* fp = fopen("trans.txt", "w");
117 | //	for (int i=0; i<graph.node_rots.size(); ++i) {
118 | //		fprintf(fp, "%d\n", i);
119 | //		for (int k=0; k<3; ++k) {
120 | //			fprintf(fp, "%lf %lf %lf %lf\n", graph.node_rots[i](k, 0), graph.node_rots[i](k, 1), graph.node_rots[i](k, 2), graph.node_trans[i][k]);
121 | //		}
122 | //		fprintf(fp, "\n");
123 | //	}
124 | //    fclose(fp);
125 | 
126 |     delete []query_node.ptr();
127 |     delete []dists_node.ptr();
128 |     delete []indices_node.ptr();
129 |     delete []raw_dataset;
130 |     return 0;
131 | }
132 | 


--------------------------------------------------------------------------------
/src/DeformGraph.cpp:
--------------------------------------------------------------------------------
  1 | #include <iterator>
  2 | #include <vector>
  3 | #include <algorithm>
  4 | #include "DeformGraph.h"
  5 | 
  6 | //for debug the graph
  7 | static void _save_pcd2ply(std::vector<Eigen::Vector3d> node_pos,
  8 | std::vector<Eigen::Vector3d> node_norm, const char* filename) {
  9 |     assert (node_pos.size() == node_norm.size());
 10 |     FILE* fp = fopen(filename, "w");
 11 |     if (!fp) {
 12 |         fprintf(stderr, "warning : unable to open %s when saving pcd to ply file.\n", filename);
 13 |         return;
 14 |     }
 15 |     fprintf(fp, "ply\n");
 16 |     fprintf(fp, "format ascii 1.0\n");
 17 |     fprintf(fp, "element vertex %d\n", node_pos.size());
 18 |     fprintf(fp, "property float x\n");
 19 |     fprintf(fp, "property float y\n");
 20 |     fprintf(fp, "property float z\n");
 21 |     fprintf(fp, "property float nx\n");
 22 |     fprintf(fp, "property float ny\n");
 23 |     fprintf(fp, "property float nz\n");
 24 |     fprintf(fp, "property uchar red\n");
 25 |     fprintf(fp, "property uchar green\n");
 26 |     fprintf(fp, "property uchar blue\n");
 27 |     fprintf(fp, "end_header\n");
 28 |     
 29 |     for (size_t i=0; i<node_pos.size(); ++i) {
 30 |         fprintf(fp, "%f %f %f %f %f %f %d %d %d\n", 
 31 |             node_pos[i][0], node_pos[i][1], node_pos[i][2],
 32 |             node_norm[i][0], node_norm[i][1], node_norm[i][2],
 33 |             0, 255, 255);
 34 |     }
 35 |     fclose(fp);
 36 | }
 37 | static void _save_pcd2obj(std::vector<Eigen::Vector3d> node_pos,
 38 | std::vector<Eigen::Vector3d> node_norm, const char* filename) {
 39 | 	TriMesh mesh;
 40 | 	mesh.vert_num = node_norm.size();
 41 | 	mesh.vertex_coord = node_pos;
 42 | 	mesh.norm_coord = node_norm;
 43 | 	mesh.saveOBJ(filename);
 44 | }
 45 | void DGraph::build_graph(const TriMesh& mesh, std::vector<int>& sample_index, double _max_dist_neigh/* = 0.1 */, int k_nn/*  = 10 */) {
 46 |     p_mesh = &mesh;
 47 | 	node_index.resize(sample_index.size()); 
 48 | 	std::copy(sample_index.begin(), sample_index.end(), node_index.begin());
 49 | 	node_pos.resize(node_index.size());
 50 | 	node_norm.resize(node_index.size());
 51 |     for (size_t i=0; i<node_index.size(); ++i) {
 52 |         node_pos[i] = p_mesh->vertex_coord[node_index[i]];
 53 |         node_norm[i] = p_mesh->norm_coord[node_index[i]];
 54 |     }
 55 |     node_rots.resize(node_index.size(), Eigen::Matrix3d::Identity());
 56 |     node_trans.resize(node_index.size(), Eigen::Vector3d::Zero());
 57 | 
 58 |     max_dist_neigh = _max_dist_neigh;
 59 |     k_neigh = k_nn;
 60 |     build_neigh();
 61 |     _save_pcd2ply(node_pos, node_norm, "graph.ply");    // save the graph for debugging
 62 | 	_save_pcd2obj(node_pos, node_norm, "graph.obj");
 63 | }
 64 | #include <flann/flann.hpp>
 65 | void DGraph::build_neigh() {
 66 |     if (k_neigh == 0 || fabs(max_dist_neigh) <= 1e-9) {
 67 |         fprintf(stderr, "error : set neigh parameters first!!!\n");
 68 |         return;
 69 |     }
 70 |     double *raw_dataset = new double[3*node_pos.size()];
 71 |     for (size_t i=0; i<node_pos.size(); ++i) {
 72 |         raw_dataset[3*i+0] = node_pos[i][0];
 73 |         raw_dataset[3*i+1] = node_pos[i][1];
 74 |         raw_dataset[3*i+2] = node_pos[i][2];
 75 |     }
 76 |     flann::Matrix<double> flann_dataset(raw_dataset, node_pos.size(), 3);
 77 |     flann::Index< flann::L2<double> > flann_index(flann_dataset, flann::KDTreeIndexParams(1));
 78 |     flann_index.buildIndex();
 79 | 
 80 |     flann::Matrix<double>   query_node(new double[3], 1, 3);
 81 |     flann::Matrix<int>      indices_node(new int[k_neigh+1], 1, k_neigh+1);
 82 |     flann::Matrix<double>   dists_node(new double[k_neigh+1], 1, k_neigh+1);
 83 |     
 84 |     int node_index = 0; 
 85 |     node_neigh.resize(node_pos.size());
 86 |     for (size_t i=0; i<node_pos.size(); ++i) {
 87 |         query_node[0][0] = node_pos[i][0]; query_node[0][1] = node_pos[i][1]; query_node[0][2] = node_pos[i][2];
 88 |         int n = flann_index.knnSearch(query_node, indices_node, dists_node, k_neigh+1, flann::SearchParams(flann::FLANN_CHECKS_UNLIMITED));
 89 |         for (int j=1; j<n; ++j) {  //ignore itself
 90 |             if (dists_node[0][j] > max_dist_neigh*max_dist_neigh) break;
 91 |             node_neigh[i].push_back(indices_node[0][j]);
 92 |         }
 93 |     }
 94 |     
 95 |     delete []query_node.ptr();
 96 |     delete []dists_node.ptr();
 97 |     delete []indices_node.ptr();
 98 |     delete []raw_dataset;
 99 | }
100 | void DGraph::update_graph(const Eigen::VectorXd& X) {
101 | 	int vn = node_pos.size();
102 | 	std::vector<Eigen::Matrix3d> rots(vn); std::vector<Eigen::Vector3d> trans(vn);
103 | 	for (int i=0; i<vn; ++i) {
104 | 		for (int r=0; r<3; ++r) {
105 | 			for (int c=0; c<3; ++c)
106 | 				rots[i](r, c) = X[12*i+3*r+c];
107 | 			trans[i][r] = X[12*i+9+r];
108 | 		}
109 | 		Eigen::Matrix3d r = rots[i].inverse(); rots[i] = r;
110 | 		rots[i].transposeInPlace();
111 | 		node_pos[i] = node_pos[i] + trans[i];
112 | 		node_norm[i] = rots[i]*node_norm[i];
113 | 	}
114 | 	for (int i=0; i<vn; ++i) {
115 | 		node_rots[i] = rots[i]*node_rots[i];
116 | 		node_trans[i] = rots[i]*node_trans[i] + trans[i];
117 | 	}
118 | }
119 | void DGraph::deform(TriMesh& d_mesh) {
120 | 	if (&d_mesh == this->p_mesh) {
121 | 		fprintf(stderr, "error : address conflict, not allow to deform the mesh.\n");
122 | 		return;
123 | 	}
124 | 
125 |     double *raw_dataset = new double[3*node_pos.size()];
126 |     for (size_t i=0; i<node_pos.size(); ++i) {
127 |         raw_dataset[3*i+0] = node_pos[i][0];
128 |         raw_dataset[3*i+1] = node_pos[i][1];
129 |         raw_dataset[3*i+2] = node_pos[i][2];
130 |     }
131 |     flann::Matrix<double> flann_dataset(raw_dataset, node_pos.size(), 3);
132 |     flann::Index< flann::L2<double> > flann_index(flann_dataset, flann::KDTreeIndexParams(1));
133 |     flann_index.buildIndex();
134 | 
135 | 	int knn = this->k_neigh; double max_dist = this->max_dist_neigh;
136 | 	flann::Matrix<double>   query_node(new double[3], 1, 3);
137 | 	flann::Matrix<int>      indices_node(new int[knn+1], 1, knn+1);
138 | 	flann::Matrix<double>   dists_node(new double[knn+1], 1, knn+1);
139 | 
140 | 	FILE* fp = fopen("weights.txt", "w");
141 | 	std::vector<double> weights;
142 |     std::vector<Eigen::Vector3d>& verts = d_mesh.vertex_coord;
143 |     std::vector<Eigen::Vector3d>& norms = d_mesh.norm_coord;
144 |     std::vector<Eigen::Vector3d> vs(this->node_pos.size());
145 |     for (size_t i=0; i<vs.size(); ++i) {
146 |         vs[i] = this->p_mesh->vertex_coord[this->node_index[i]];
147 |     }
148 | 	for (size_t i=0; i<verts.size(); ++i) {
149 |         weights.clear();
150 |         query_node[0][0] = verts[i][0]; query_node[0][1] = verts[i][1]; query_node[0][2] = verts[i][2];
151 |         int n_neigh = flann_index.knnSearch(query_node, indices_node, dists_node, knn+1, flann::SearchParams(flann::FLANN_CHECKS_UNLIMITED));
152 | 		if (n_neigh < 2) {
153 | 			fprintf(stderr, "neighour size is less than 2, which might incur unwished deformation\n");
154 | 		}
155 | 		assert (n_neigh >= 2);
156 |         weights.resize(n_neigh - 1);    // last one for the max dist
157 |         double d_max = sqrt(dists_node[0][n_neigh-1]);
158 |         n_neigh -= 1; double weight_sum = 0;
159 |         for (int j=0; j<n_neigh; ++j) {
160 |             weights[j] = (1-sqrt(dists_node[0][j]) / d_max) ;
161 |             weights[j] = weights[j]*weights[j];
162 |             weight_sum += weights[j];
163 |         }
164 |         for (int j=0; j<n_neigh; ++j)
165 |             weights[j] /= weight_sum;
166 |         Eigen::Vector3d vert = verts[i], norm = norms[i]; 
167 | 		Eigen::Matrix3d rot = Eigen::Matrix3d::Zero(); verts[i] = Eigen::Vector3d(0.0, 0.0, 0.0);
168 |         for (int j=0; j<n_neigh; ++j) {
169 |             int index_cur_node = indices_node[0][j];
170 |             verts[i] += (weights[j]*(this->node_rots[index_cur_node]*(vert - vs[index_cur_node]) + this->node_trans[index_cur_node] + vs[index_cur_node]));
171 |             rot += weights[j]*this->node_rots[index_cur_node].inverse().transpose();
172 |         }
173 |         norms[i] = rot * norms[i];
174 |         norms[i].normalize();
175 | 		for (int j=0; j<n_neigh; ++j) {
176 | 			fprintf(fp, "%lf\t", weights[j]);
177 | 		}
178 | 		fprintf(fp, "\n");
179 |     }
180 | 	fclose(fp);
181 | 	delete []query_node.ptr();
182 | 	delete []indices_node.ptr();
183 | 	delete []dists_node.ptr();
184 |     delete []raw_dataset;
185 | }
186 | void Deformer::init(DGraph& graph, const std::vector<int>& corr_indexs, const std::vector<Eigen::Vector3d>& cons) {
187 |     p_graph = &graph;
188 | 	if (corr_indexs.size() != cons.size()) {
189 | 		fprintf(stderr, "fatal error : cons_index size should be equal to cons_vert size\n");
190 | 		exit(-1);
191 | 	}
192 |     constraints_index = corr_indexs;
193 |     node_constraints = cons;
194 |     rows = 0;
195 |     int unknown_vn = p_graph->node_pos.size();
196 |     rows += 6*unknown_vn;
197 |     for (int i=0; i<unknown_vn; ++i) 
198 |         rows += 3*p_graph->node_neigh[i].size();
199 |     rows += 3*constraints_index.size();
200 | 
201 |     cols = 12*unknown_vn;
202 | }
203 | void Deformer::build_values(const Eigen::VectorXd& x, Eigen::VectorXd& fVec) {
204 |     int unknown_vn = p_graph->node_pos.size();
205 |     int row = 0;
206 |     for (int i=0; i<unknown_vn; ++i) {
207 | 		int col = 12*i;
208 |         Eigen::Vector3d v[3] = {Eigen::Vector3d(x(col+0), x(col+3), x(col+6)), 
209 |                                 Eigen::Vector3d(x(col+1), x(col+4), x(col+7)), 
210 |                                 Eigen::Vector3d(x(col+2), x(col+5), x(col+8))};
211 |         fVec[row+0] = weights[0]*(v[0].dot(v[1]));
212 |         fVec[row+1] = weights[0]*(v[0].dot(v[2]));
213 |         fVec[row+2] = weights[0]*(v[1].dot(v[2]));
214 |         fVec[row+3] = weights[0]*(v[0].dot(v[0])) - weights[0];
215 |         fVec[row+4] = weights[0]*(v[1].dot(v[1])) - weights[0];
216 |         fVec[row+5] = weights[0]*(v[2].dot(v[2])) - weights[0];
217 |         row += 6;
218 | 	}
219 |     
220 |     for (int i=0; i<unknown_vn; ++i) {
221 |         int col_i = 12*i;
222 |         Eigen::Matrix3d R_i; 
223 |         Eigen::Vector3d t_i(x(col_i+9), x(col_i+10), x(col_i+11));
224 |         R_i <<  x(col_i+0), x(col_i+1), x(col_i+2),
225 |                 x(col_i+3), x(col_i+4), x(col_i+5),  
226 |                 x(col_i+6), x(col_i+7), x(col_i+8);
227 |         for (size_t _j=0; _j<p_graph->node_neigh[i].size(); ++_j) {
228 |             int j = p_graph->node_neigh[i][_j];
229 | 			int col_j = 12*j; Eigen::Vector3d t_j(x(col_j+9), x(col_j+10), x(col_j+11));
230 |             Eigen::Vector3d g = p_graph->node_pos[i] - p_graph->node_pos[j];  //g_i - g_j
231 |             g = (-R_i*g + g + t_i-t_j);
232 | 			g = weights[1]*g;
233 | 			fVec[row+0] = g[0]; fVec[row+1] = g[1]; fVec[row+2] = g[2];
234 | 			row += 3;
235 |         }
236 |     }
237 | 
238 |     for (size_t _i=0; _i<constraints_index.size(); ++_i) {
239 |         int i = constraints_index[_i];
240 |         int col = 12*i;
241 |         Eigen::Vector3d t(x(col+9), x(col+10), x(col+11));
242 | 		Eigen::Vector3d new_pos = weights[2]*(p_graph->node_pos[i] + t - node_constraints[_i]);
243 | 		fVec[row + 0] = new_pos[0]; fVec[row + 1] = new_pos[1]; fVec[row + 2] = new_pos[2];
244 | 		row += 3;
245 |     }
246 |     assert (row == rows);
247 |     return;
248 | }
249 | 
250 | void Deformer::build_jacobi(const Eigen::VectorXd& x, Eigen::SparseMatrix<double>& fJac) {
251 |     int unknown_vn = p_graph->node_pos.size();
252 | 	fJac.resize(this->rows, this->cols);	//re-initialize
253 | 	fJac.reserve(Eigen::VectorXi::Constant(this->cols, 3*this->p_graph->k_neigh/*3+2*iMaxNeigb*/)); /*  */
254 |     int row = 0;
255 |     //build the rot part
256 |     double weight_rot = weights[0];
257 |     for (int i=0; i<unknown_vn; ++i) {
258 |         int col = 12*i;
259 |         //c1*c2
260 |         fJac.coeffRef(row, col) = weight_rot*x[col+1]; fJac.coeffRef(row, col+3) = weight_rot*x[col+4];
261 |         fJac.coeffRef(row, col+6) = weight_rot*x[col+7]; fJac.coeffRef(row, col+1) = weight_rot*x[col];
262 |         fJac.coeffRef(row, col+4) = weight_rot*x[col+3]; fJac.coeffRef(row, col+7) = weight_rot*x[col+6]; 
263 |         ++row;
264 |         //c1*c3
265 |         fJac.coeffRef(row, col) = weight_rot*x[col+2]; fJac.coeffRef(row, col+3) = weight_rot*x[col+5];
266 |         fJac.coeffRef(row, col+6) = weight_rot*x[col+8]; fJac.coeffRef(row, col+2) = weight_rot*x[col];
267 |         fJac.coeffRef(row, col+5) = weight_rot*x[col+3]; fJac.coeffRef(row, col+8) = weight_rot*x[col+6]; 
268 |         ++row;
269 |         //c2*c3
270 |         fJac.coeffRef(row, col+1) = weight_rot*x[col+2]; fJac.coeffRef(row, col+4) = weight_rot*x[col+5];
271 |         fJac.coeffRef(row, col+7) = weight_rot*x[col+8]; fJac.coeffRef(row, col+2) = weight_rot*x[col+1];
272 |         fJac.coeffRef(row, col+5) = weight_rot*x[col+4]; fJac.coeffRef(row, col+8) = weight_rot*x[col+7]; 
273 |         ++row;
274 |         //c1*c1-1
275 |         fJac.coeffRef(row, col) = 2.0*weight_rot*x[col];
276 |         fJac.coeffRef(row, col+3) = 2.0*weight_rot*x[col+3];
277 |         fJac.coeffRef(row, col+6) = 2.0*weight_rot*x[col+6]; 
278 |         ++row;
279 |         //c2*c2-1
280 |         fJac.coeffRef(row, col+1) = 2.0*weight_rot*x[col+1];
281 |         fJac.coeffRef(row, col+4) = 2.0*weight_rot*x[col+4];
282 |         fJac.coeffRef(row, col+7) = 2.0*weight_rot*x[col+7]; 
283 |         ++row;
284 |         //c3*c3-1
285 |         fJac.coeffRef(row, col+2) = 2.0*weight_rot*x[col+2];
286 |         fJac.coeffRef(row, col+5) = 2.0*weight_rot*x[col+5];
287 |         fJac.coeffRef(row, col+8) = 2.0*weight_rot*x[col+8]; 
288 |         ++row;
289 |     }
290 | 
291 |     //build smooth part
292 |     double weight_smooth = weights[1];  //is the reg part of original part
293 |     for (int i=0; i<unknown_vn; ++i) {
294 | 		//min \sum_{i=0}^N\sum_{j\in~Neigh(i)}R_i*(v_j - v_i) + v_i - (v_j + t_j)
295 |         for (size_t _j=0; _j<p_graph->node_neigh[i].size(); ++_j) {
296 |             int j = p_graph->node_neigh[i][_j];
297 |             int col_i = 12*i, col_j = 12*j;
298 |             Eigen::Vector3d g = p_graph->node_pos[j] - p_graph->node_pos[i];  //v_j - v_i
299 |             fJac.coeffRef(row, col_i+0) = weight_smooth*g[0];
300 |             fJac.coeffRef(row+1, col_i+3) = weight_smooth*g[0];
301 |             fJac.coeffRef(row+2, col_i+6) = weight_smooth*g[0];
302 | 
303 |             fJac.coeffRef(row, col_i+1) = weight_smooth*g[1];
304 |             fJac.coeffRef(row+1, col_i+4) = weight_smooth*g[1];
305 |             fJac.coeffRef(row+2, col_i+7) = weight_smooth*g[1];
306 | 
307 |             fJac.coeffRef(row, col_i+2) = weight_smooth*g[2];
308 |             fJac.coeffRef(row+1, col_i+5) = weight_smooth*g[2];
309 |             fJac.coeffRef(row+2, col_i+8) = weight_smooth*g[2];
310 | 
311 | 			//t_i t_j
312 |             fJac.coeffRef(row, col_i+9) = weight_smooth;  
313 |             fJac.coeffRef(row+1, col_i+10) = weight_smooth;
314 |             fJac.coeffRef(row+2, col_i+11) = weight_smooth;
315 | 
316 |             fJac.coeffRef(row, col_j+9) = -weight_smooth;
317 |             fJac.coeffRef(row+1, col_j+10) = -weight_smooth;
318 |             fJac.coeffRef(row+2, col_j+11) = -weight_smooth;
319 |             row += 3;
320 |         }
321 |     }
322 | 
323 |     //build regular part
324 |     double weight_regular = weights[2]; //is the con part of orginal paper
325 |     for (size_t _i=0; _i<constraints_index.size(); ++_i) {
326 | 		int i = constraints_index[_i];
327 | 		int col = 12*i;
328 | 		fJac.coeffRef(row, col+9 ) = weight_regular;
329 | 		fJac.coeffRef(row+1, col+10) = weight_regular;
330 | 		fJac.coeffRef(row+2, col+11) = weight_regular;
331 | 		row += 3;
332 |     }
333 |     assert (row == rows);
334 |     return;
335 | }
336 | 
337 | void Deformer::build_jacobi(const Eigen::VectorXd& x, Eigen::MatrixXd& fJac) {
338 |     int unknown_vn = p_graph->node_pos.size();
339 |     fJac.resize(rows, cols);
340 |     fJac.setZero(rows, cols);
341 | 
342 |     int row = 0;
343 |     //build the rot part
344 |     double weight_rot = weights[0];
345 |     for (int i=0; i<unknown_vn; ++i) {
346 |         int col = 12*i;
347 |         //c1*c2
348 |         fJac(row, col) = weight_rot*x[col+1]; fJac(row, col+3) = weight_rot*x[col+4];
349 |         fJac(row, col+6) = weight_rot*x[col+7]; fJac(row, col+1) = weight_rot*x[col];
350 |         fJac(row, col+4) = weight_rot*x[col+3]; fJac(row, col+7) = weight_rot*x[col+6]; 
351 |         ++row;
352 |         //c1*c3
353 |         fJac(row, col) = weight_rot*x[col+2]; fJac(row, col+3) = weight_rot*x[col+5];
354 |         fJac(row, col+6) = weight_rot*x[col+8]; fJac(row, col+2) = weight_rot*x[col];
355 |         fJac(row, col+5) = weight_rot*x[col+3]; fJac(row, col+8) = weight_rot*x[col+6]; 
356 |         ++row;
357 |         //c2*c3
358 |         fJac(row, col+1) = weight_rot*x[col+2]; fJac(row, col+4) = weight_rot*x[col+5];
359 |         fJac(row, col+7) = weight_rot*x[col+8]; fJac(row, col+2) = weight_rot*x[col+1];
360 |         fJac(row, col+5) = weight_rot*x[col+4]; fJac(row, col+8) = weight_rot*x[col+7]; 
361 |         ++row;
362 |         //c1*c1-1
363 |         fJac(row, col) = 2.0*weight_rot*x[col];
364 |         fJac(row, col+3) = 2.0*weight_rot*x[col+3];
365 |         fJac(row, col+6) = 2.0*weight_rot*x[col+6]; 
366 |         ++row;
367 |         //c2*c2-1
368 |         fJac(row, col+1) = 2.0*weight_rot*x[col+1];
369 |         fJac(row, col+4) = 2.0*weight_rot*x[col+4];
370 |         fJac(row, col+7) = 2.0*weight_rot*x[col+7]; 
371 |         ++row;
372 |         //c3*c3-1
373 |         fJac(row, col+2) = 2.0*weight_rot*x[col+2];
374 |         fJac(row, col+5) = 2.0*weight_rot*x[col+5];
375 |         fJac(row, col+8) = 2.0*weight_rot*x[col+8]; 
376 |         ++row;
377 |     }
378 | 
379 |     //build smooth part
380 |     double weight_smooth = weights[1];  //is the reg part of original part
381 |     for (int i=0; i<unknown_vn; ++i) {
382 | 		//min \sum_{i=0}^N\sum_{j\in~Neigh(i)}R_i*(v_j - v_i) + v_i - (v_j + t_j)
383 |         for (size_t _j=0; _j<p_graph->node_neigh[i].size(); ++_j) {
384 |             int j = p_graph->node_neigh[i][_j];
385 |             int col_i = 12*i, col_j = 12*j;
386 |             Eigen::Vector3d g = p_graph->node_pos[j] - p_graph->node_pos[i];  //v_j - v_i
387 |             fJac(row, col_i+0) = weight_smooth*g[0];
388 |             fJac(row+1, col_i+3) = weight_smooth*g[0];
389 |             fJac(row+2, col_i+6) = weight_smooth*g[0];
390 | 
391 |             fJac(row, col_i+1) = weight_smooth*g[1];
392 |             fJac(row+1, col_i+4) = weight_smooth*g[1];
393 |             fJac(row+2, col_i+7) = weight_smooth*g[1];
394 | 
395 |             fJac(row, col_i+2) = weight_smooth*g[2];
396 |             fJac(row+1, col_i+5) = weight_smooth*g[2];
397 |             fJac(row+2, col_i+8) = weight_smooth*g[2];
398 | 
399 | 			//t_i t_j
400 |             fJac(row, col_i+9) = weight_smooth;  
401 |             fJac(row+1, col_i+10) = weight_smooth;
402 |             fJac(row+2, col_i+11) = weight_smooth;
403 | 
404 |             fJac(row, col_j+9) = -weight_smooth;
405 |             fJac(row+1, col_j+10) = -weight_smooth;
406 |             fJac(row+2, col_j+11) = -weight_smooth;
407 |             row += 3;
408 |         }
409 |     }
410 | 
411 |     //build regular part
412 |     double weight_regular = weights[2]; //is the con part of orginal paper
413 |     for (size_t _i=0; _i<constraints_index.size(); ++_i) {
414 | 		int i = constraints_index[_i];
415 | 		int col = 12*i;
416 | 		fJac(row, col+9 ) = weight_regular;
417 | 		fJac(row+1, col+10) = weight_regular;
418 | 		fJac(row+2, col+11) = weight_regular;
419 | 		row += 3;
420 |     }
421 |     assert (row == rows);
422 |     return;
423 | }
424 | 
425 | #define NOMINMAX	
426 | #include <Windows.h>
427 | class Timer {
428 | public:
429 |     inline void tic() {
430 |         _tic = getTime();
431 |     }
432 |     inline long toc() {
433 |         _toc = getTime();
434 |         return _toc - _tic;
435 |     }
436 | private:
437 |     inline long getTime() {
438 |         SYSTEMTIME t;
439 |         GetSystemTime(&t);
440 |         return ((t.wHour*60 + t.wMinute)*60 +t.wSecond)*1000 + t.wMilliseconds;
441 |     }
442 | private:
443 |     long _tic, _toc;
444 | };
445 | double minimize_guass_newton(Deformer& deformer, Eigen::VectorXd& X) {
446 | 	Eigen::VectorXd fVec(deformer.rows); fVec.setZero();
447 | 	Eigen::SparseMatrix<double> fJac(deformer.rows, deformer.cols);
448 | 	fJac.reserve(Eigen::VectorXi::Constant(deformer.cols, 3*deformer.p_graph->k_neigh)); /* or reserve bigger space for each column */
449 | 	Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver;
450 |     double eps_break_condition[3] = {1e-6, 1e-2, 1e-3}; 
451 | 	double pre_energy = 0, cur_energy = 0, best_energy = 1e10;
452 | 	Eigen::VectorXd pre_X, best_X;
453 | 	Eigen::VectorXd pre_fVec, best_fVec;
454 |     
455 |     deformer.build_values(X, fVec);
456 |     cur_energy = best_energy = sqrt(fVec.dot(fVec))*0.5;
457 | 
458 |     int iter = 0;
459 | 	while (iter < 20) {
460 | //		printf("\titer = %d\n", iter);
461 |         deformer.build_jacobi(X, fJac);
462 |         Eigen::SparseMatrix<double> A = fJac.transpose();
463 |         Eigen::VectorXd            g = A*fVec;
464 |         A = A*fJac;
465 |         solver.compute(A);
466 |         if (solver.info() != Eigen::Success) {
467 |             fprintf(stdout, "unable to defactorize fJac^T*fJac\n");
468 |             break;
469 |         }
470 |         Eigen::VectorXd h = solver.solve(-g);	// ['-' is request]
471 | 
472 | 		X = X + h;
473 |         pre_fVec = fVec;
474 |         pre_energy = cur_energy;
475 | 
476 | 		deformer.build_values(X, fVec);
477 |         cur_energy = sqrt(fVec.dot(fVec))*0.5;
478 | 
479 |         if (cur_energy < best_energy) {
480 |             best_energy = cur_energy;
481 |             best_X = X;
482 |         }
483 | 
484 | //		printf("best_energy = %lf\tcur_energy : %lf\n", best_energy, cur_energy);
485 | 		++iter;
486 | 		if (iter >= 2) {
487 | 			if (fabs(cur_energy - pre_energy) < eps_break_condition[0]*(1+cur_energy)) break;
488 | 			Eigen::VectorXd gradient = fJac.transpose() * pre_fVec;
489 | 			double gradient_max = gradient.maxCoeff();
490 | 			if (gradient_max < eps_break_condition[1]*(1+cur_energy)) break;
491 | 		}
492 | 		double delta_max = h.maxCoeff();
493 | 		if (delta_max < eps_break_condition[2]*(1+delta_max)) break;
494 |     }
495 | 	printf("iter = %d\tbest_energy = %lf\n", iter, best_energy);
496 | 	X = best_X;
497 |     return best_energy;
498 | }
499 | static double _getMaxDiagnalCoeff(const Eigen::SparseMatrix<double>& sp_mat) 
500 | {
501 | 	double max_coeff = DBL_MIN;
502 | 	assert (sp_mat.rows() == sp_mat.cols());
503 | 	for (int k=0; k<sp_mat.outerSize(); ++k) {
504 | 		for (Eigen::SparseMatrix<double>::InnerIterator it(sp_mat, k); it; ++it) {
505 | 			if (it.row() == it.col() && max_coeff < it.value())
506 | 				max_coeff = it.value();
507 | 		}
508 | 	}
509 | 	return max_coeff;
510 | }
511 | double minimize_levenberg_marquardt(Deformer& deformer, Eigen::VectorXd& X) {
512 | 	Timer tt;
513 | 	Eigen::VectorXd fVec(deformer.rows); fVec.setZero();
514 | 	Eigen::SparseMatrix<double> fJac(deformer.rows, deformer.cols);
515 | 	fJac.reserve(Eigen::VectorXi::Constant(deformer.cols, 3*deformer.p_graph->k_neigh/*3+2*iMaxNeigb*/)); /*  */
516 | 	Eigen::SparseMatrix<double> _Identity(deformer.cols, deformer.cols);
517 | 	_Identity.reserve(Eigen::VectorXi::Constant(deformer.cols, 1)); /*  */
518 | 	for (int k=0; k<deformer.cols; ++k)  _Identity.coeffRef(k, k) = 1.0;
519 | 
520 | 	deformer.build_values(X, fVec);
521 | 	deformer.build_jacobi(X, fJac);
522 | 	Eigen::SparseMatrix<double> A = fJac.transpose();
523 | 	Eigen::VectorXd            g = A*fVec;
524 | 	A = A*fJac;
525 |     double best_energy = sqrt(fVec.dot(fVec)); best_energy *= 0.5;
526 | 	double cur_energy = best_energy;
527 |     const double epsilon_1 = 1e-8, epsilon_2 = 1e-6;
528 |     const double tau = 1e-3;    //if init guess is a good approximation, tau should be small, else tau = 10^-3 or even 1
529 |     const int ITER_MAX = 100;
530 |     double mu = tau*_getMaxDiagnalCoeff(A);
531 |     double nu = 2.0;
532 |     int iter = 0; bool found = g.maxCoeff() < epsilon_1;
533 | 	Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver;
534 | 	tt.tic();
535 |     while (!found && iter++ < ITER_MAX) {
536 | 		//TODO: after uncomment this line, the min energy is bigger than comment condition, however, this condition can speedup
537 | 		//each factorization time.
538 | 		A = A + _Identity*mu;
539 | 		solver.compute(A);
540 | 		if (solver.info() != Eigen::Success)
541 | 		{
542 | 			printf("!!!Cholesky factorization on Jac failed. solver.info() = %d\n", solver.info());
543 | 			break;
544 | 		}
545 | 		Eigen::VectorXd h = solver.solve(-g);
546 | 		if (sqrt(h.dot(h)) <= epsilon_2*(sqrt(X.dot(X))+epsilon_2)) {
547 | 			found = true;
548 | 			break;
549 | 		}
550 |         tt.tic();
551 | 		Eigen::VectorXd _vX = X + h;     //new X
552 |         deformer.build_values(_vX, fVec);
553 | 		cur_energy = sqrt(fVec.dot(fVec));  cur_energy *= 0.5f;	//F(x_new)
554 | 
555 | //		dprintf(stdout, "solver: iter = %d\t", iter);
556 | //		dprintf(stdout, "energy_min = %lf\tenergy_cur = %lf\n", best_energy, cur_energy);
557 | 
558 |         double varrho = (best_energy - cur_energy) / (0.5*(h.dot(mu*h-g)));
559 | //		dprintf(stdout, "varrho = %lf\n", varrho);
560 |         if (varrho > 0) {   //accept
561 |             best_energy = cur_energy;
562 | 			X = _vX;
563 |             deformer.build_jacobi(X, fJac);
564 | 			A = (fJac).transpose();
565 |             g = A*fVec;
566 |             A = A*fJac;
567 | 
568 |             found = g.maxCoeff() < epsilon_1;
569 |             mu = mu*std::max(1.0/3, 1-(2*varrho-1)*(2*varrho-1)*(2*varrho-1));
570 |             nu = 2.0;
571 |         } else {
572 |             mu = mu*nu; nu = 2*nu;
573 |         }
574 |     }	
575 | 	printf("iter = %d\tbest_energy = %lf\n", iter, best_energy);
576 | 	return best_energy;
577 | }
578 | 
579 | double optimize_once(DGraph& graph, std::vector<int>& constraints_index,
580 | 	const std::vector<Eigen::Vector3d>& cons) {
581 |     Deformer* p_deformer = new Deformer;
582 |     p_deformer->init(graph, constraints_index, cons);
583 |     double weights[3] = {1.0, 10.0*sqrt(100.0), 10*sqrt(100.0)};
584 |     p_deformer->set_weights(weights, 3);
585 | 
586 | 	Eigen::VectorXd x(p_deformer->cols);x.setZero();
587 | 	for (int i=0; i<graph.node_pos.size(); ++i) {
588 | 		x[12*i] = 1.0; x[12*i+4] = 1.0; x[12*i+8] = 1.0;
589 | 	}
590 | 	Timer tt;
591 | #if 0
592 | 	// start to minimize x using levenberg marquardt method
593 | 	tt.tic();
594 | 	double min_energy = minimize_levenberg_marquardt(*p_deformer, x);
595 |     dprintf(stderr, "minimize_levenberg_marquardt time useage %ldms\n", tt.toc());
596 | #else
597 | 	// start to minimize x using gauss-newton method
598 | 	tt.tic();
599 | 	double min_energy = minimize_guass_newton(*p_deformer, x);
600 |     dprintf(stderr, "minimize_guass_newton time useage %ldms\n", tt.toc());
601 | #endif
602 | 	// update the graph	
603 | 	graph.update_graph(x);
604 | 
605 | 	delete p_deformer;
606 | 	return min_energy;
607 | }
608 | #pragma warning(push)
609 | #pragma warning(disable: 4819)
610 | #pragma warning(disable: 4521)
611 | #include <pcl/keypoints/impl/uniform_sampling.hpp>
612 | #include <pcl/filters/impl/normal_space.hpp>
613 | #include <pcl/search/kdtree.h>
614 | #include <pcl/common/common.h>
615 | #pragma warning(pop)
616 | 
617 | //convert the trimesh to the point cloud data with positions and normals.
618 | static pcl::PointCloud<pcl::PointNormal>::Ptr
619 | _trimesh2pcl_data(const TriMesh& trimesh) {	
620 |     pcl::PointCloud<pcl::PointNormal>::Ptr out(new pcl::PointCloud<pcl::PointNormal>);
621 | 	out->points.reserve(trimesh.vert_num);
622 |     int vi = 0;
623 |     for (int i=0; i<trimesh.vert_num; ++i) {
624 |         pcl::PointNormal temp_v;
625 |         temp_v.x = trimesh.vertex_coord[i][0];
626 |         temp_v.y = trimesh.vertex_coord[i][1];
627 |         temp_v.z = trimesh.vertex_coord[i][2];
628 |         temp_v.normal_x = trimesh.norm_coord[i][0];
629 |         temp_v.normal_y = trimesh.norm_coord[i][1];
630 |         temp_v.normal_z = trimesh.norm_coord[i][2];
631 |         out->points.push_back(temp_v);
632 |         ++vi;
633 |     }
634 |     out->width = vi; out->height = 1; out->is_dense = false;
635 |     return out;
636 | }
637 | 
638 | void mesh_sampling(const TriMesh& mesh, std::vector<int>& sample_index, double max_dist) {
639 |     pcl::PointCloud<pcl::PointNormal>::Ptr src_pcd = _trimesh2pcl_data(mesh);
640 | 	std::vector<int> normal_sampling_indices;
641 | 	std::vector<int> uniform_sampling_indices;
642 | 
643 | 	const int NORMAL_SAMPLE_BIN_NUMBER = 30; 
644 | 	const int NORMAL_SAMPLE_NUMBER = 200;
645 | 	if (1)
646 | 	{
647 | 		pcl::NormalSpaceSampling<pcl::PointNormal, pcl::Normal> normal_sampler;
648 | 		pcl::PointCloud<pcl::Normal>::Ptr tri_normal(new pcl::PointCloud<pcl::Normal>);
649 | 		{
650 | 			tri_normal->resize(src_pcd->points.size());
651 | 			for (int i=0; i<src_pcd->points.size(); ++i) {
652 | 				tri_normal->points[i].normal_x = src_pcd->points[i].normal_x;
653 | 				tri_normal->points[i].normal_y = src_pcd->points[i].normal_y;
654 | 				tri_normal->points[i].normal_z = src_pcd->points[i].normal_z;
655 | 			}
656 | 		}
657 | 		normal_sampler.setSeed(time(NULL));
658 | 		normal_sampler.setInputCloud(src_pcd);
659 | 		normal_sampler.setNormals(tri_normal);
660 | 		normal_sampler.setBins(NORMAL_SAMPLE_BIN_NUMBER, NORMAL_SAMPLE_BIN_NUMBER, NORMAL_SAMPLE_BIN_NUMBER);
661 | 		normal_sampler.setSample(NORMAL_SAMPLE_NUMBER);
662 | 		normal_sampler.filter(normal_sampling_indices);
663 | 	}
664 | 	{
665 | 		typedef pcl::PointXYZ PointType;
666 | 		pcl::UniformSampling<PointType> uniform_sampler;
667 | 		pcl::PointCloud<PointType>::Ptr tri_vertex(new pcl::PointCloud<PointType>);
668 | 		{
669 | 			tri_vertex->resize(src_pcd->points.size());
670 | 			for (int i=0; i<src_pcd->points.size(); ++i) {
671 | 				tri_vertex->points[i].x = src_pcd->points[i].x;
672 | 				tri_vertex->points[i].y = src_pcd->points[i].y;
673 | 				tri_vertex->points[i].z = src_pcd->points[i].z;
674 | 			}
675 | 		}
676 | 		pcl::search::KdTree<PointType>::Ptr tree(new pcl::search::KdTree<PointType>());
677 | 		uniform_sampler.setInputCloud(tri_vertex);
678 | 		uniform_sampler.setSearchMethod(tree);
679 | 		uniform_sampler.setRadiusSearch(max_dist);
680 | 		{
681 | 			pcl::PointCloud<int> keypoints_src_idx;
682 | 			uniform_sampler.compute(keypoints_src_idx);
683 | 			uniform_sampling_indices.clear();
684 | 			uniform_sampling_indices.resize(keypoints_src_idx.size());
685 | 			std::copy(keypoints_src_idx.begin(), keypoints_src_idx.end(), uniform_sampling_indices.begin());
686 | 		}
687 | 	}
688 | 
689 | 	//merge the sampling result to output
690 | 	sample_index.clear();
691 | 	sample_index.resize(uniform_sampling_indices.size() + normal_sampling_indices.size());
692 | 	std::sort(uniform_sampling_indices.begin(), uniform_sampling_indices.end());
693 | 	std::sort(normal_sampling_indices.begin(), normal_sampling_indices.end());
694 | 
695 | 	std::merge(uniform_sampling_indices.begin(), uniform_sampling_indices.end(),
696 | 		normal_sampling_indices.begin(), normal_sampling_indices.end(), 
697 | 		sample_index.begin());
698 | 	std::vector<int>::iterator last_it = std::unique(sample_index.begin(), sample_index.end());
699 |     sample_index.erase(last_it, sample_index.end());
700 | 	printf("****uniform sampling count : %d\n****normal sampling count : %d\n", uniform_sampling_indices.size(),
701 | 		normal_sampling_indices.size());
702 | 	printf("****sampling count : %d\n", sample_index.size());
703 | }
704 | 


--------------------------------------------------------------------------------