Initial commit: add index.html

.gitattributes

@@ -0,0 +1,39 @@
+*.7z filter=lfs diff=lfs merge=lfs -text
+*.arrow filter=lfs diff=lfs merge=lfs -text
+*.bin filter=lfs diff=lfs merge=lfs -text
+*.bz2 filter=lfs diff=lfs merge=lfs -text
+*.ckpt filter=lfs diff=lfs merge=lfs -text
+*.ftz filter=lfs diff=lfs merge=lfs -text
+*.gz filter=lfs diff=lfs merge=lfs -text
+*.h5 filter=lfs diff=lfs merge=lfs -text
+*.joblib filter=lfs diff=lfs merge=lfs -text
+*.lfs.* filter=lfs diff=lfs merge=lfs -text
+*.mlmodel filter=lfs diff=lfs merge=lfs -text
+*.model filter=lfs diff=lfs merge=lfs -text
+*.msgpack filter=lfs diff=lfs merge=lfs -text
+*.npy filter=lfs diff=lfs merge=lfs -text
+*.npz filter=lfs diff=lfs merge=lfs -text
+*.onnx filter=lfs diff=lfs merge=lfs -text
+*.ot filter=lfs diff=lfs merge=lfs -text
+*.parquet filter=lfs diff=lfs merge=lfs -text
+*.pb filter=lfs diff=lfs merge=lfs -text
+*.pickle filter=lfs diff=lfs merge=lfs -text
+*.pkl filter=lfs diff=lfs merge=lfs -text
+*.pt filter=lfs diff=lfs merge=lfs -text
+*.pth filter=lfs diff=lfs merge=lfs -text
+*.rar filter=lfs diff=lfs merge=lfs -text
+*.safetensors filter=lfs diff=lfs merge=lfs -text
+saved_model/**/* filter=lfs diff=lfs merge=lfs -text
+*.tar.* filter=lfs diff=lfs merge=lfs -text
+*.tar filter=lfs diff=lfs merge=lfs -text
+*.tflite filter=lfs diff=lfs merge=lfs -text
+*.tgz filter=lfs diff=lfs merge=lfs -text
+*.wasm filter=lfs diff=lfs merge=lfs -text
+*.xz filter=lfs diff=lfs merge=lfs -text
+*.zip filter=lfs diff=lfs merge=lfs -text
+*.zst filter=lfs diff=lfs merge=lfs -text
+*tfevents* filter=lfs diff=lfs merge=lfs -text
+*.so filter=lfs diff=lfs merge=lfs -text
+*.stl filter=lfs diff=lfs merge=lfs -text
+*.pm filter=lfs diff=lfs merge=lfs -text
+*.om filter=lfs diff=lfs merge=lfs -text

.gitignore

@@ -0,0 +1,28 @@
+# 构建目录
+build/
+
+# CMake 生成文件
+CMakeFiles/
+CMakeCache.txt
+cmake_install.cmake
+Makefile
+
+# Visual Studio 编译产物
+*.o
+*.exe
+*.pdb
+*.lib
+*.dll
+.vs/
+*.vcxproj*
+*.sln
+
+# Build outputs
+build/
+*.pdb
+*.lib
+*.ipch
+.vs/
+
+# VSCode 配置目录
+.vscode/

3rdparty/nanoflann.hpp

@@ -0,0 +1,1305 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja ([email protected]). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe ([email protected]). All rights reserved.
+ * Copyright 2011 Jose Luis Blanco ([email protected]).
+ *   All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *************************************************************************/
+
+#ifndef  NANOFLANN_HPP_
+#define  NANOFLANN_HPP_
+
+#include <vector>
+#include <cassert>
+#include <algorithm>
+#include <stdexcept>
+#include <cstdio>  // for fwrite()
+#include <cmath>   // for fabs(),...
+#include <limits>
+
+// Avoid conflicting declaration of min/max macros in windows headers
+#if !defined(NOMINMAX) && (defined(_WIN32) || defined(_WIN32_)  || defined(WIN32) || defined(_WIN64))
+# define NOMINMAX
+# ifdef max
+#  undef   max
+#  undef   min
+# endif
+#endif
+
+namespace nanoflann
+{
+/** @addtogroup nanoflann_grp nanoflann C++ library for ANN
+  *  @{ */
+
+  	/** Library version: 0xMmP (M=Major,m=minor,P=path) */
+	#define NANOFLANN_VERSION 0x113
+
+	/** @addtogroup result_sets_grp Result set classes
+	  *  @{ */
+	template <typename DistanceType, typename IndexType = size_t, typename CountType = size_t>
+	class KNNResultSet
+	{
+		IndexType * indices;
+		DistanceType* dists;
+		CountType capacity;
+		CountType count;
+
+	public:
+		inline KNNResultSet(CountType capacity_) : capacity(capacity_), count(0)
+		{
+		}
+
+		inline void init(IndexType* indices_, DistanceType* dists_)
+		{
+			indices = indices_;
+			dists = dists_;
+			count = 0;
+			dists[capacity-1] = (std::numeric_limits<DistanceType>::max)();
+		}
+
+		inline CountType size() const
+		{
+			return count;
+		}
+
+		inline bool full() const
+		{
+			return count == capacity;
+		}
+
+
+		inline void addPoint(DistanceType dist, IndexType index)
+		{
+			CountType i;
+			for (i=count; i>0; --i) {
+#ifdef NANOFLANN_FIRST_MATCH   // If defined and two poins have the same distance, the one with the lowest-index will be returned first.
+				if ( (dists[i-1]>dist) || ((dist==dists[i-1])&&(indices[i-1]>index)) ) {
+#else
+				if (dists[i-1]>dist) {
+#endif
+					if (i<capacity) {
+						dists[i] = dists[i-1];
+						indices[i] = indices[i-1];
+					}
+				}
+				else break;
+			}
+			if (i<capacity) {
+				dists[i] = dist;
+				indices[i] = index;
+			}
+			if (count<capacity) count++;
+		}
+
+		inline DistanceType worstDist() const
+		{
+			return dists[capacity-1];
+		}
+	};
+
+
+	/**
+	 * A result-set class used when performing a radius based search.
+	 */
+	template <typename DistanceType, typename IndexType = size_t>
+	class RadiusResultSet
+	{
+	public:
+		const DistanceType radius;
+
+		std::vector<std::pair<IndexType,DistanceType> >& m_indices_dists;
+
+		inline RadiusResultSet(DistanceType radius_, std::vector<std::pair<IndexType,DistanceType> >& indices_dists) : radius(radius_), m_indices_dists(indices_dists)
+		{
+			init();
+		}
+
+		inline ~RadiusResultSet() { }
+
+		inline void init() { clear(); }
+		inline void clear() { m_indices_dists.clear(); }
+
+		inline size_t size() const { return m_indices_dists.size(); }
+
+		inline bool full() const { return true; }
+
+		inline void addPoint(DistanceType dist, IndexType index)
+		{
+			if (dist<radius)
+				m_indices_dists.push_back(std::make_pair<IndexType,DistanceType>(index,dist));
+		}
+
+		inline DistanceType worstDist() const { return radius; }
+
+		/** Clears the result set and adjusts the search radius. */
+		inline void set_radius_and_clear( const DistanceType r )
+		{
+			radius = r;
+			clear();
+		}
+
+		/**
+		 * Find the worst result (furtherest neighbor) without copying or sorting
+		 * Pre-conditions: size() > 0
+		 */
+		std::pair<IndexType,DistanceType> worst_item() const
+		{
+		   if (m_indices_dists.empty()) throw std::runtime_error("Cannot invoke RadiusResultSet::worst_item() on an empty list of results.");
+		   typedef typename std::vector<std::pair<IndexType,DistanceType> >::const_iterator DistIt;
+		   DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end());
+		   return *it;
+		}
+	};
+
+	/** operator "<" for std::sort() */
+	struct IndexDist_Sorter
+	{
+		/** PairType will be typically: std::pair<IndexType,DistanceType> */
+		template <typename PairType>
+		inline bool operator()(const PairType &p1, const PairType &p2) const {
+			return p1.second < p2.second;
+		}
+	};
+
+	/** @} */
+
+
+	/** @addtogroup loadsave_grp Load/save auxiliary functions
+	  * @{ */
+	template<typename T>
+	void save_value(FILE* stream, const T& value, size_t count = 1)
+	{
+		fwrite(&value, sizeof(value),count, stream);
+	}
+
+	template<typename T>
+	void save_value(FILE* stream, const std::vector<T>& value)
+	{
+		size_t size = value.size();
+		fwrite(&size, sizeof(size_t), 1, stream);
+		fwrite(&value[0], sizeof(T), size, stream);
+	}
+
+	template<typename T>
+	void load_value(FILE* stream, T& value, size_t count = 1)
+	{
+		size_t read_cnt = fread(&value, sizeof(value), count, stream);
+		if (read_cnt != count) {
+			throw std::runtime_error("Cannot read from file");
+		}
+	}
+
+
+	template<typename T>
+	void load_value(FILE* stream, std::vector<T>& value)
+	{
+		size_t size;
+		size_t read_cnt = fread(&size, sizeof(size_t), 1, stream);
+		if (read_cnt!=1) {
+			throw std::runtime_error("Cannot read from file");
+		}
+		value.resize(size);
+		read_cnt = fread(&value[0], sizeof(T), size, stream);
+		if (read_cnt!=size) {
+			throw std::runtime_error("Cannot read from file");
+		}
+	}
+	/** @} */
+
+
+	/** @addtogroup metric_grp Metric (distance) classes
+	  * @{ */
+
+	template<typename T> inline T abs(T x) { return (x<0) ? -x : x; }
+	template<> inline int abs<int>(int x) { return ::abs(x); }
+	template<> inline float abs<float>(float x) { return fabsf(x); }
+	template<> inline double abs<double>(double x) { return fabs(x); }
+	template<> inline long double abs<long double>(long double x) { return fabsl(x); }
+
+	/** Manhattan distance functor (generic version, optimized for high-dimensionality data sets).
+	  *  Corresponding distance traits: nanoflann::metric_L1
+	  * \tparam T Type of the elements (e.g. double, float, uint8_t)
+	  * \tparam DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t)
+	  */
+	template<class T, class DataSource, typename _DistanceType = T>
+	struct L1_Adaptor
+	{
+		typedef T ElementType;
+		typedef _DistanceType DistanceType;
+
+		const DataSource &data_source;
+
+		L1_Adaptor(const DataSource &_data_source) : data_source(_data_source) { }
+
+		inline DistanceType operator()(const T* a, const size_t b_idx, size_t size, DistanceType worst_dist = -1) const
+		{
+			DistanceType result = DistanceType();
+			const T* last = a + size;
+			const T* lastgroup = last - 3;
+			size_t d = 0;
+
+			/* Process 4 items with each loop for efficiency. */
+			while (a < lastgroup) {
+				const DistanceType diff0 = nanoflann::abs(a[0] - data_source.kdtree_get_pt(b_idx,d++));
+				const DistanceType diff1 = nanoflann::abs(a[1] - data_source.kdtree_get_pt(b_idx,d++));
+				const DistanceType diff2 = nanoflann::abs(a[2] - data_source.kdtree_get_pt(b_idx,d++));
+				const DistanceType diff3 = nanoflann::abs(a[3] - data_source.kdtree_get_pt(b_idx,d++));
+				result += diff0 + diff1 + diff2 + diff3;
+				a += 4;
+				if ((worst_dist>0)&&(result>worst_dist)) {
+					return result;
+				}
+			}
+			/* Process last 0-3 components.  Not needed for standard vector lengths. */
+			while (a < last) {
+				result += nanoflann::abs( *a++ - data_source.kdtree_get_pt(b_idx,d++) );
+			}
+			return result;
+		}
+
+		template <typename U, typename V>
+		inline DistanceType accum_dist(const U a, const V b, int dim) const
+		{
+			return (a-b)*(a-b);
+		}
+	};
+
+	/** Squared Euclidean distance functor (generic version, optimized for high-dimensionality data sets).
+	  *  Corresponding distance traits: nanoflann::metric_L2
+	  * \tparam T Type of the elements (e.g. double, float, uint8_t)
+	  * \tparam DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t)
+	  */
+	template<class T, class DataSource, typename _DistanceType = T>
+	struct L2_Adaptor
+	{
+		typedef T ElementType;
+		typedef _DistanceType DistanceType;
+
+		const DataSource &data_source;
+
+		L2_Adaptor(const DataSource &_data_source) : data_source(_data_source) { }
+
+		inline DistanceType operator()(const T* a, const size_t b_idx, size_t size, DistanceType worst_dist = -1) const
+		{
+			DistanceType result = DistanceType();
+			const T* last = a + size;
+			const T* lastgroup = last - 3;
+			size_t d = 0;
+
+			/* Process 4 items with each loop for efficiency. */
+			while (a < lastgroup) {
+				const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx,d++);
+				const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx,d++);
+				const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx,d++);
+				const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx,d++);
+				result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
+				a += 4;
+				if ((worst_dist>0)&&(result>worst_dist)) {
+					return result;
+				}
+			}
+			/* Process last 0-3 components.  Not needed for standard vector lengths. */
+			while (a < last) {
+				const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx,d++);
+				result += diff0 * diff0;
+			}
+			return result;
+		}
+
+		template <typename U, typename V>
+		inline DistanceType accum_dist(const U a, const V b, int dim) const
+		{
+			return (a-b)*(a-b);
+		}
+	};
+
+	/** Squared Euclidean distance functor (suitable for low-dimensionality datasets, like 2D or 3D point clouds)
+	  *  Corresponding distance traits: nanoflann::metric_L2_Simple
+	  * \tparam T Type of the elements (e.g. double, float, uint8_t)
+	  * \tparam DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t)
+	  */
+	template<class T, class DataSource, typename _DistanceType = T>
+	struct L2_Simple_Adaptor
+	{
+		typedef T ElementType;
+		typedef _DistanceType DistanceType;
+
+		const DataSource &data_source;
+
+		L2_Simple_Adaptor(const DataSource &_data_source) : data_source(_data_source) { }
+
+		inline DistanceType operator()(const T* a, const size_t b_idx, size_t size) const {
+			return data_source.kdtree_distance(a,b_idx,size);
+		}
+
+		template <typename U, typename V>
+		inline DistanceType accum_dist(const U a, const V b, int dim) const
+		{
+            dim = dim;
+			return (a-b)*(a-b);
+		}
+	};
+
+	/** Metaprogramming helper traits class for the L1 (Manhattan) metric */
+	struct metric_L1 {
+		template<class T, class DataSource>
+		struct traits {
+			typedef L1_Adaptor<T,DataSource> distance_t;
+		};
+	};
+	/** Metaprogramming helper traits class for the L2 (Euclidean) metric */
+	struct metric_L2 {
+		template<class T, class DataSource>
+		struct traits {
+			typedef L2_Adaptor<T,DataSource> distance_t;
+		};
+	};
+	/** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */
+	struct metric_L2_Simple {
+		template<class T, class DataSource>
+		struct traits {
+			typedef L2_Simple_Adaptor<T,DataSource> distance_t;
+		};
+	};
+
+	/** @} */
+
+
+
+	/** @addtogroup param_grp Parameter structs
+	  * @{ */
+
+	/**  Parameters (see http://code.google.com/p/nanoflann/ for help choosing the parameters)
+	  */
+	struct KDTreeSingleIndexAdaptorParams
+	{
+		KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10, int dim_ = -1) :
+			leaf_max_size(_leaf_max_size), dim(dim_)
+		{}
+
+		size_t leaf_max_size;
+		int dim;
+	};
+
+	/** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */
+	struct SearchParams
+	{
+		/** Note: The first argument (checks_IGNORED_) is ignored, but kept for compatibility with the FLANN interface */
+		SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true ) :
+            eps(eps_), sorted(sorted_) {
+            checks_IGNORED_ = checks_IGNORED_;
+        }
+
+		int   checks;  //!< Ignored parameter (Kept for compatibility with the FLANN interface).
+		float eps;  //!< search for eps-approximate neighbours (default: 0)
+		bool sorted; //!< only for radius search, require neighbours sorted by distance (default: true)
+	};
+	/** @} */
+
+
+	/** @addtogroup memalloc_grp Memory allocation
+	  * @{ */
+
+	/**
+	 * Allocates (using C's malloc) a generic type T.
+	 *
+	 * Params:
+	 *     count = number of instances to allocate.
+	 * Returns: pointer (of type T*) to memory buffer
+	 */
+	template <typename T>
+	inline T* allocate(size_t count = 1)
+	{
+		T* mem = (T*) ::malloc(sizeof(T)*count);
+		return mem;
+	}
+
+
+	/**
+	 * Pooled storage allocator
+	 *
+	 * The following routines allow for the efficient allocation of storage in
+	 * small chunks from a specified pool.  Rather than allowing each structure
+	 * to be freed individually, an entire pool of storage is freed at once.
+	 * This method has two advantages over just using malloc() and free().  First,
+	 * it is far more efficient for allocating small objects, as there is
+	 * no overhead for remembering all the information needed to free each
+	 * object or consolidating fragmented memory.  Second, the decision about
+	 * how long to keep an object is made at the time of allocation, and there
+	 * is no need to track down all the objects to free them.
+	 *
+	 */
+
+	const size_t     WORDSIZE=16;
+	const size_t     BLOCKSIZE=8192;
+
+	class PooledAllocator
+	{
+		/* We maintain memory alignment to word boundaries by requiring that all
+		    allocations be in multiples of the machine wordsize.  */
+		/* Size of machine word in bytes.  Must be power of 2. */
+		/* Minimum number of bytes requested at a time from	the system.  Must be multiple of WORDSIZE. */
+
+
+		size_t  remaining;  /* Number of bytes left in current block of storage. */
+		void*   base;     /* Pointer to base of current block of storage. */
+		void*   loc;      /* Current location in block to next allocate memory. */
+		size_t  blocksize;
+
+
+	public:
+		size_t  usedMemory;
+		size_t  wastedMemory;
+
+		/**
+		    Default constructor. Initializes a new pool.
+		 */
+		PooledAllocator(const size_t blocksize = BLOCKSIZE)
+		{
+			this->blocksize = blocksize;
+			remaining = 0;
+			base = NULL;
+
+			usedMemory = 0;
+			wastedMemory = 0;
+		}
+
+		/**
+		 * Destructor. Frees all the memory allocated in this pool.
+		 */
+		~PooledAllocator()
+		{
+			while (base != NULL) {
+				void *prev = *((void**) base); /* Get pointer to prev block. */
+				::free(base);
+				base = prev;
+			}
+		}
+
+		/**
+		 * Returns a pointer to a piece of new memory of the given size in bytes
+		 * allocated from the pool.
+		 */
+		void* malloc(const size_t req_size)
+		{
+			/* Round size up to a multiple of wordsize.  The following expression
+			    only works for WORDSIZE that is a power of 2, by masking last bits of
+			    incremented size to zero.
+			 */
+			const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1);
+
+			/* Check whether a new block must be allocated.  Note that the first word
+			    of a block is reserved for a pointer to the previous block.
+			 */
+			if (size > remaining) {
+
+				wastedMemory += remaining;
+
+				/* Allocate new storage. */
+				const size_t blocksize = (size + sizeof(void*) + (WORDSIZE-1) > BLOCKSIZE) ?
+							size + sizeof(void*) + (WORDSIZE-1) : BLOCKSIZE;
+
+				// use the standard C malloc to allocate memory
+				void* m = ::malloc(blocksize);
+				if (!m) {
+					fprintf(stderr,"Failed to allocate memory.\n");
+					return NULL;
+				}
+
+				/* Fill first word of new block with pointer to previous block. */
+				((void**) m)[0] = base;
+				base = m;
+
+				size_t shift = 0;
+				//int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & (WORDSIZE-1))) & (WORDSIZE-1);
+
+				remaining = blocksize - sizeof(void*) - shift;
+				loc = ((char*)m + sizeof(void*) + shift);
+			}
+			void* rloc = loc;
+			loc = (char*)loc + size;
+			remaining -= size;
+
+			usedMemory += size;
+
+			return rloc;
+		}
+
+		/**
+		 * Allocates (using this pool) a generic type T.
+		 *
+		 * Params:
+		 *     count = number of instances to allocate.
+		 * Returns: pointer (of type T*) to memory buffer
+		 */
+		template <typename T>
+		T* allocate(const size_t count = 1)
+		{
+			T* mem = (T*) this->malloc(sizeof(T)*count);
+			return mem;
+		}
+
+	};
+	/** @} */
+
+
+	/** @addtogroup kdtrees_grp KD-tree classes and adaptors
+	  * @{ */
+
+	/** kd-tree index
+	 *
+	 * Contains the k-d trees and other information for indexing a set of points
+	 * for nearest-neighbor matching.
+	 *
+	 *  The class "DatasetAdaptor" must provide the following interface (can be non-virtual, inlined methods):
+	 *
+	 *  \code
+	 *   // Must return the number of data points
+	 *   inline size_t kdtree_get_point_count() const { ... }
+	 *
+	 *   // Must return the Euclidean (L2) distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
+	 *   inline DistanceType kdtree_distance(const T *p1, const size_t idx_p2,size_t size) const { ... }
+	 *
+	 *   // Must return the dim'th component of the idx'th point in the class:
+	 *   inline T kdtree_get_pt(const size_t idx, int dim) const { ... }
+	 *
+	 *   // Optional bounding-box computation: return false to default to a standard bbox computation loop.
+	 *   //   Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
+	 *   //   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
+	 *   template <class BBOX>
+	 *   bool kdtree_get_bbox(BBOX &bb) const
+	 *   {
+	 *      bb[0].low = ...; bb[0].high = ...;  // 0th dimension limits
+	 *      bb[1].low = ...; bb[1].high = ...;  // 1st dimension limits
+	 *      ...
+	 *      return true;
+	 *   }
+	 *
+	 *  \endcode
+	 *
+	 * \tparam IndexType Will be typically size_t or int
+	 */
+	template <typename Distance, class DatasetAdaptor,int DIM = -1, typename IndexType = size_t>
+	class KDTreeSingleIndexAdaptor
+	{
+	public:
+		typedef typename Distance::ElementType  ElementType;
+		typedef typename Distance::DistanceType DistanceType;
+	protected:
+
+		/**
+		 *  Array of indices to vectors in the dataset.
+		 */
+		std::vector<IndexType> vind;
+
+		size_t m_leaf_max_size;
+
+
+		/**
+		 * The dataset used by this index
+		 */
+		const DatasetAdaptor &dataset; //!< The source of our data
+
+		const KDTreeSingleIndexAdaptorParams index_params;
+
+		size_t m_size;
+		int dim;  //!< Dimensionality of each data point
+
+
+		/*--------------------- Internal Data Structures --------------------------*/
+		struct Node
+		{
+			union {
+				struct
+				{
+					/**
+					 * Indices of points in leaf node
+					 */
+					IndexType left, right;
+				} lr;
+				struct
+				{
+					/**
+					 * Dimension used for subdivision.
+					 */
+					int divfeat;
+					/**
+					 * The values used for subdivision.
+					 */
+					DistanceType divlow, divhigh;
+				} sub;
+			};
+			/**
+			 * The child nodes.
+			 */
+			Node* child1, * child2;
+		};
+		typedef Node* NodePtr;
+
+
+		struct Interval
+		{
+			ElementType low, high;
+		};
+
+		typedef std::vector<Interval> BoundingBox;
+
+
+		/** This record represents a branch point when finding neighbors in
+			the tree.  It contains a record of the minimum distance to the query
+			point, as well as the node at which the search resumes.
+		 */
+		template <typename T, typename DistanceType>
+		struct BranchStruct
+		{
+			T node;           /* Tree node at which search resumes */
+			DistanceType mindist;     /* Minimum distance to query for all nodes below. */
+
+			BranchStruct() {}
+			BranchStruct(const T& aNode, DistanceType dist) : node(aNode), mindist(dist) {}
+
+			inline bool operator<(const BranchStruct<T, DistanceType>& rhs) const
+			{
+				return mindist<rhs.mindist;
+			}
+		};
+
+		/**
+		 * Array of k-d trees used to find neighbours.
+		 */
+		NodePtr root_node;
+		typedef BranchStruct<NodePtr, DistanceType> BranchSt;
+		typedef BranchSt* Branch;
+
+		BoundingBox root_bbox;
+
+		/**
+		 * Pooled memory allocator.
+		 *
+		 * Using a pooled memory allocator is more efficient
+		 * than allocating memory directly when there is a large
+		 * number small of memory allocations.
+		 */
+		PooledAllocator pool;
+
+	public:
+
+		Distance distance;
+
+		/**
+		 * KDTree constructor
+		 *
+		 * Params:
+		 *          inputData = dataset with the input features
+		 *          params = parameters passed to the kdtree algorithm (see http://code.google.com/p/nanoflann/ for help choosing the parameters)
+		 */
+		KDTreeSingleIndexAdaptor(const int dimensionality, const DatasetAdaptor& inputData, const KDTreeSingleIndexAdaptorParams& params = KDTreeSingleIndexAdaptorParams() ) :
+			dataset(inputData), index_params(params), distance(inputData)
+		{
+			m_size = dataset.kdtree_get_point_count();
+			dim = dimensionality;
+			if (DIM>0) dim=DIM;
+			else {
+				if (params.dim>0) dim = params.dim;
+			}
+			m_leaf_max_size = params.leaf_max_size;
+
+			// Create a permutable array of indices to the input vectors.
+			init_vind();
+		}
+
+		/**
+		 * Standard destructor
+		 */
+		~KDTreeSingleIndexAdaptor()
+		{
+		}
+
+		/**
+		 * Builds the index
+		 */
+		void buildIndex()
+		{
+			init_vind();
+			computeBoundingBox(root_bbox);
+			root_node = divideTree(0, m_size, root_bbox );   // construct the tree
+		}
+
+		/**
+		 *  Returns size of index.
+		 */
+		size_t size() const
+		{
+			return m_size;
+		}
+
+		/**
+		 * Returns the length of an index feature.
+		 */
+		size_t veclen() const
+		{
+			return static_cast<size_t>(DIM>0 ? DIM : dim);
+		}
+
+		/**
+		 * Computes the inde memory usage
+		 * Returns: memory used by the index
+		 */
+		size_t usedMemory() const
+		{
+			return pool.usedMemory+pool.wastedMemory+dataset.kdtree_get_point_count()*sizeof(IndexType);  // pool memory and vind array memory
+		}
+
+		/** \name Query methods
+		  * @{ */
+
+		/**
+		 * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored inside
+		 * the result object.
+		 *
+		 * Params:
+		 *     result = the result object in which the indices of the nearest-neighbors are stored
+		 *     vec = the vector for which to search the nearest neighbors
+		 *
+		 * \tparam RESULTSET Should be any ResultSet<DistanceType>
+		 * \sa knnSearch, radiusSearch
+		 */
+		template <typename RESULTSET>
+		void findNeighbors(RESULTSET& result, const ElementType* vec, const SearchParams& searchParams) const
+		{
+			assert(vec);
+			float epsError = 1+searchParams.eps;
+
+			std::vector<DistanceType> dists( (DIM>0 ? DIM : dim) ,0);
+			DistanceType distsq = computeInitialDistances(vec, dists);
+			searchLevel(result, vec, root_node, distsq, dists, epsError);  // "count_leaf" parameter removed since was neither used nor returned to the user.
+		}
+
+		/**
+		 * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. Their indices are stored inside
+		 * the result object.
+		 *  \sa radiusSearch, findNeighbors
+		 * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface.
+		 */
+        inline void knnSearch(const ElementType *query_point, const size_t num_closest, IndexType *out_indices, DistanceType *out_distances_sq) const
+		{
+			nanoflann::KNNResultSet<DistanceType,IndexType> resultSet(num_closest);
+			resultSet.init(out_indices, out_distances_sq);
+			this->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
+		}
+
+		/**
+		 * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius.
+		 *  The output is given as a vector of pairs, of which the first element is a point index and the second the corresponding distance.
+		 *  Previous contents of \a IndicesDists are cleared.
+		 *
+		 *  If searchParams.sorted==true, the output list is sorted by ascending distances.
+		 *
+		 *  For a better performance, it is advisable to do a .reserve() on the vector if you have any wild guess about the number of expected matches.
+		 *
+		 *  \sa knnSearch, findNeighbors
+		 * \return The number of points within the given radius (i.e. indices.size() or dists.size() )
+		 */
+		size_t radiusSearch(const ElementType *query_point,const DistanceType radius, std::vector<std::pair<IndexType,DistanceType> >& IndicesDists, const SearchParams& searchParams) const
+		{
+			RadiusResultSet<DistanceType,IndexType> resultSet(radius,IndicesDists);
+			this->findNeighbors(resultSet, query_point, searchParams);
+
+			if (searchParams.sorted)
+				std::sort(IndicesDists.begin(),IndicesDists.end(), IndexDist_Sorter() );
+
+			return resultSet.size();
+		}
+
+		/** @} */
+
+	private:
+		/** Make sure the auxiliary list \a vind has the same size than the current dataset, and re-generate if size has changed. */
+		void init_vind()
+		{
+			// Create a permutable array of indices to the input vectors.
+			m_size = dataset.kdtree_get_point_count();
+			if (vind.size()!=m_size)
+			{
+				vind.resize(m_size);
+				for (size_t i = 0; i < m_size; i++) vind[i] = i;
+			}
+		}
+
+		/// Helper accessor to the dataset points:
+		inline ElementType dataset_get(size_t idx, int component) const {
+			return dataset.kdtree_get_pt(idx,component);
+		}
+
+
+		void save_tree(FILE* stream, NodePtr tree)
+		{
+			save_value(stream, *tree);
+			if (tree->child1!=NULL) {
+				save_tree(stream, tree->child1);
+			}
+			if (tree->child2!=NULL) {
+				save_tree(stream, tree->child2);
+			}
+		}
+
+
+		void load_tree(FILE* stream, NodePtr& tree)
+		{
+			tree = pool.allocate<Node>();
+			load_value(stream, *tree);
+			if (tree->child1!=NULL) {
+				load_tree(stream, tree->child1);
+			}
+			if (tree->child2!=NULL) {
+				load_tree(stream, tree->child2);
+			}
+		}
+
+
+		void computeBoundingBox(BoundingBox& bbox)
+		{
+			bbox.resize((DIM>0 ? DIM : dim));
+			if (dataset.kdtree_get_bbox(bbox))
+			{
+				// Done! It was implemented in derived class
+			}
+			else
+			{
+				for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
+					bbox[i].low =
+						bbox[i].high = dataset_get(0,i);
+				}
+				const size_t N = dataset.kdtree_get_point_count();
+				for (size_t k=1; k<N; ++k) {
+					for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
+						if (dataset_get(k,i)<bbox[i].low) bbox[i].low = dataset_get(k,i);
+						if (dataset_get(k,i)>bbox[i].high) bbox[i].high = dataset_get(k,i);
+					}
+				}
+			}
+		}
+
+
+		/**
+		 * Create a tree node that subdivides the list of vecs from vind[first]
+		 * to vind[last].  The routine is called recursively on each sublist.
+		 * Place a pointer to this new tree node in the location pTree.
+		 *
+		 * Params: pTree = the new node to create
+		 *                  first = index of the first vector
+		 *                  last = index of the last vector
+		 */
+		NodePtr divideTree(const IndexType left, const IndexType right, BoundingBox& bbox)
+		{
+			NodePtr node = pool.allocate<Node>(); // allocate memory
+
+			/* If too few exemplars remain, then make this a leaf node. */
+			if ( (right-left) <= m_leaf_max_size) {
+				node->child1 = node->child2 = NULL;    /* Mark as leaf node. */
+				node->lr.left = left;
+				node->lr.right = right;
+
+				// compute bounding-box of leaf points
+				for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
+					bbox[i].low = dataset_get(vind[left],i);
+					bbox[i].high = dataset_get(vind[left],i);
+				}
+				for (IndexType k=left+1; k<right; ++k) {
+					for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
+						if (bbox[i].low>dataset_get(vind[k],i)) bbox[i].low=dataset_get(vind[k],i);
+						if (bbox[i].high<dataset_get(vind[k],i)) bbox[i].high=dataset_get(vind[k],i);
+					}
+				}
+			}
+			else {
+				IndexType idx;
+				int cutfeat;
+				DistanceType cutval;
+				middleSplit_(&vind[0]+left, right-left, idx, cutfeat, cutval, bbox);
+
+				node->sub.divfeat = cutfeat;
+
+				BoundingBox left_bbox(bbox);
+				left_bbox[cutfeat].high = cutval;
+				node->child1 = divideTree(left, left+idx, left_bbox);
+
+				BoundingBox right_bbox(bbox);
+				right_bbox[cutfeat].low = cutval;
+				node->child2 = divideTree(left+idx, right, right_bbox);
+
+				node->sub.divlow = left_bbox[cutfeat].high;
+				node->sub.divhigh = right_bbox[cutfeat].low;
+
+				for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
+					bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low);
+					bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high);
+				}
+			}
+
+			return node;
+		}
+
+		void computeMinMax(IndexType* ind, IndexType count, int element, ElementType& min_elem, ElementType& max_elem)
+		{
+			min_elem = dataset_get(ind[0],element);
+			max_elem = dataset_get(ind[0],element);
+			for (IndexType i=1; i<count; ++i) {
+				ElementType val = dataset_get(ind[i],element);
+				if (val<min_elem) min_elem = val;
+				if (val>max_elem) max_elem = val;
+			}
+		}
+
+		void middleSplit(IndexType* ind, IndexType count, IndexType& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox)
+		{
+			// find the largest span from the approximate bounding box
+			ElementType max_span = bbox[0].high-bbox[0].low;
+			cutfeat = 0;
+			cutval = (bbox[0].high+bbox[0].low)/2;
+			for (int i=1; i<(DIM>0 ? DIM : dim); ++i) {
+				ElementType span = bbox[i].low-bbox[i].low;
+				if (span>max_span) {
+					max_span = span;
+					cutfeat = i;
+					cutval = (bbox[i].high+bbox[i].low)/2;
+				}
+			}
+
+			// compute exact span on the found dimension
+			ElementType min_elem, max_elem;
+			computeMinMax(ind, count, cutfeat, min_elem, max_elem);
+			cutval = (min_elem+max_elem)/2;
+			max_span = max_elem - min_elem;
+
+			// check if a dimension of a largest span exists
+			size_t k = cutfeat;
+			for (size_t i=0; i<(DIM>0 ? DIM : dim); ++i) {
+				if (i==k) continue;
+				ElementType span = bbox[i].high-bbox[i].low;
+				if (span>max_span) {
+					computeMinMax(ind, count, i, min_elem, max_elem);
+					span = max_elem - min_elem;
+					if (span>max_span) {
+						max_span = span;
+						cutfeat = i;
+						cutval = (min_elem+max_elem)/2;
+					}
+				}
+			}
+			IndexType lim1, lim2;
+			planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
+
+			if (lim1>count/2) index = lim1;
+			else if (lim2<count/2) index = lim2;
+			else index = count/2;
+		}
+
+
+		void middleSplit_(IndexType* ind, IndexType count, IndexType& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox)
+		{
+			const DistanceType EPS=static_cast<DistanceType>(0.00001);
+			ElementType max_span = bbox[0].high-bbox[0].low;
+			for (int i=1; i<(DIM>0 ? DIM : dim); ++i) {
+				ElementType span = bbox[i].high-bbox[i].low;
+				if (span>max_span) {
+					max_span = span;
+				}
+			}
+			ElementType max_spread = -1;
+			cutfeat = 0;
+			for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
+				ElementType span = bbox[i].high-bbox[i].low;
+				if (span>(1-EPS)*max_span) {
+					ElementType min_elem, max_elem;
+					computeMinMax(ind, count, cutfeat, min_elem, max_elem);
+					ElementType spread = max_elem-min_elem;;
+					if (spread>max_spread) {
+						cutfeat = i;
+						max_spread = spread;
+					}
+				}
+			}
+			// split in the middle
+			DistanceType split_val = (bbox[cutfeat].low+bbox[cutfeat].high)/2;
+			ElementType min_elem, max_elem;
+			computeMinMax(ind, count, cutfeat, min_elem, max_elem);
+
+			if (split_val<min_elem) cutval = min_elem;
+			else if (split_val>max_elem) cutval = max_elem;
+			else cutval = split_val;
+
+			IndexType lim1, lim2;
+			planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
+
+			if (lim1>count/2) index = lim1;
+			else if (lim2<count/2) index = lim2;
+			else index = count/2;
+		}
+
+
+		/**
+		 *  Subdivide the list of points by a plane perpendicular on axe corresponding
+		 *  to the 'cutfeat' dimension at 'cutval' position.
+		 *
+		 *  On return:
+		 *  dataset[ind[0..lim1-1]][cutfeat]<cutval
+		 *  dataset[ind[lim1..lim2-1]][cutfeat]==cutval
+		 *  dataset[ind[lim2..count]][cutfeat]>cutval
+		 */
+		void planeSplit(IndexType* ind, const IndexType count, int cutfeat, DistanceType cutval, IndexType& lim1, IndexType& lim2)
+		{
+			/* Move vector indices for left subtree to front of list. */
+			IndexType left = 0;
+			IndexType right = count-1;
+			for (;; ) {
+				while (left<=right && dataset_get(ind[left],cutfeat)<cutval) ++left;
+				while (right && left<=right && dataset_get(ind[right],cutfeat)>=cutval) --right;
+				if (left>right || !right) break;  // "!right" was added to support unsigned Index types
+				std::swap(ind[left], ind[right]);
+				++left;
+				--right;
+			}
+			/* If either list is empty, it means that all remaining features
+			 * are identical. Split in the middle to maintain a balanced tree.
+			 */
+			lim1 = left;
+			right = count-1;
+			for (;; ) {
+				while (left<=right && dataset_get(ind[left],cutfeat)<=cutval) ++left;
+				while (right && left<=right && dataset_get(ind[right],cutfeat)>cutval) --right;
+				if (left>right || !right) break;  // "!right" was added to support unsigned Index types
+				std::swap(ind[left], ind[right]);
+				++left;
+				--right;
+			}
+			lim2 = left;
+		}
+
+		DistanceType computeInitialDistances(const ElementType* vec, std::vector<DistanceType>& dists) const
+		{
+			assert(vec);
+			DistanceType distsq = 0.0;
+
+			for (int i = 0; i < (DIM>0 ? DIM : dim); ++i) {
+				if (vec[i] < root_bbox[i].low) {
+					dists[i] = distance.accum_dist(vec[i], root_bbox[i].low, i);
+					distsq += dists[i];
+				}
+				if (vec[i] > root_bbox[i].high) {
+					dists[i] = distance.accum_dist(vec[i], root_bbox[i].high, i);
+					distsq += dists[i];
+				}
+			}
+
+			return distsq;
+		}
+
+		/**
+		 * Performs an exact search in the tree starting from a node.
+		 * \tparam RESULTSET Should be any ResultSet<DistanceType>
+		 */
+		template <class RESULTSET>
+		void searchLevel(RESULTSET& result_set, const ElementType* vec, const NodePtr node, DistanceType mindistsq,
+						 std::vector<DistanceType>& dists, const float epsError) const
+		{
+			/* If this is a leaf node, then do check and return. */
+			if ((node->child1 == NULL)&&(node->child2 == NULL)) {
+				//count_leaf += (node->lr.right-node->lr.left);  // Removed since was neither used nor returned to the user.
+				DistanceType worst_dist = result_set.worstDist();
+				for (IndexType i=node->lr.left; i<node->lr.right; ++i) {
+					const IndexType index = vind[i];// reorder... : i;
+					DistanceType dist = distance(vec, index, (DIM>0 ? DIM : dim));
+					if (dist<worst_dist) {
+						result_set.addPoint(dist,vind[i]);
+					}
+				}
+				return;
+			}
+
+			/* Which child branch should be taken first? */
+			int idx = node->sub.divfeat;
+			ElementType val = vec[idx];
+			DistanceType diff1 = val - node->sub.divlow;
+			DistanceType diff2 = val - node->sub.divhigh;
+
+			NodePtr bestChild;
+			NodePtr otherChild;
+			DistanceType cut_dist;
+			if ((diff1+diff2)<0) {
+				bestChild = node->child1;
+				otherChild = node->child2;
+				cut_dist = distance.accum_dist(val, node->sub.divhigh, idx);
+			}
+			else {
+				bestChild = node->child2;
+				otherChild = node->child1;
+				cut_dist = distance.accum_dist( val, node->sub.divlow, idx);
+			}
+
+			/* Call recursively to search next level down. */
+			searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError);
+
+			DistanceType dst = dists[idx];
+			mindistsq = mindistsq + cut_dist - dst;
+			dists[idx] = cut_dist;
+			if (mindistsq*epsError<=result_set.worstDist()) {
+				searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError);
+			}
+			dists[idx] = dst;
+		}
+
+
+		void saveIndex(FILE* stream)
+		{
+			save_value(stream, m_size);
+			save_value(stream, dim);
+			save_value(stream, root_bbox);
+			save_value(stream, m_leaf_max_size);
+			save_value(stream, vind);
+			save_tree(stream, root_node);
+		}
+
+		void loadIndex(FILE* stream)
+		{
+			load_value(stream, m_size);
+			load_value(stream, dim);
+			load_value(stream, root_bbox);
+			load_value(stream, m_leaf_max_size);
+			load_value(stream, vind);
+			load_tree(stream, root_node);
+		}
+
+	};   // class KDTree
+
+
+	/** A simple KD-tree adaptor for working with data directly stored in an Eigen Matrix, without duplicating the data storage.
+	  *  Each row in the matrix represents a point in the state space.
+	  *
+	  *  Example of usage:
+	  * \code
+	  * 	Eigen::Matrix<num_t,Dynamic,Dynamic>  mat;
+	  * 	// Fill out "mat"...
+	  *
+	  * 	typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix<num_t,Dynamic,Dynamic> >  my_kd_tree_t;
+	  * 	const int max_leaf = 10;
+	  * 	my_kd_tree_t   mat_index(dimdim, mat, max_leaf );
+	  * 	mat_index.index->buildIndex();
+	  * 	mat_index.index->...
+	  * \endcode
+	  *
+	  *  \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations.
+	  *  \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.
+	  *  \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int)
+	  */
+	template <class MatrixType, int DIM = -1, class Distance = nanoflann::metric_L2, typename IndexType = size_t>
+	struct KDTreeEigenMatrixAdaptor
+	{
+		typedef KDTreeEigenMatrixAdaptor<MatrixType,DIM,Distance> self_t;
+		typedef typename MatrixType::Scalar              num_t;
+		typedef typename Distance::template traits<num_t,self_t>::distance_t metric_t;
+		typedef KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType>  index_t;
+
+		index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index.
+
+		/// Constructor: takes a const ref to the matrix object with the data points
+		KDTreeEigenMatrixAdaptor(const int dimensionality, const MatrixType &mat, const int leaf_max_size = 10) : m_data_matrix(mat)
+		{
+			const size_t dims = mat.cols();
+			if (DIM>0 && static_cast<int>(dims)!=DIM)
+				throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument");
+			index = new index_t( dims, *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size, dims ) );
+			index->buildIndex();
+		}
+
+		~KDTreeEigenMatrixAdaptor() {
+			delete index;
+		}
+
+		const MatrixType &m_data_matrix;
+
+		/** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]).
+		  *  Note that this is a short-cut method for index->findNeighbors().
+		  *  The user can also call index->... methods as desired.
+		  * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface.
+		  */
+		inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int nChecks_IGNORED = 10) const
+		{
+			nanoflann::KNNResultSet<typename MatrixType::Scalar,IndexType> resultSet(num_closest);
+			resultSet.init(out_indices, out_distances_sq);
+			index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
+		}
+
+		/** @name Interface expected by KDTreeSingleIndexAdaptor
+		  * @{ */
+
+		const self_t & derived() const {
+			return *this;
+		}
+		self_t & derived()       {
+			return *this;
+		}
+
+		// Must return the number of data points
+		inline size_t kdtree_get_point_count() const {
+			return m_data_matrix.rows();
+		}
+
+		// Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
+		inline num_t kdtree_distance(const num_t *p1, const size_t idx_p2,size_t size) const
+		{
+			num_t s=0;
+			for (size_t i=0; i<size; i++) {
+				const num_t d= p1[i]-m_data_matrix.coeff(idx_p2,i);
+				s+=d*d;
+			}
+			return s;
+		}
+
+		// Returns the dim'th component of the idx'th point in the class:
+		inline num_t kdtree_get_pt(const size_t idx, int dim) const {
+			return m_data_matrix.coeff(idx,dim);
+		}
+
+		// Optional bounding-box computation: return false to default to a standard bbox computation loop.
+		//   Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
+		//   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
+		template <class BBOX>
+		bool kdtree_get_bbox(BBOX &bb) const {
+			return false;
+		}
+
+		/** @} */
+
+	}; // end of KDTreeEigenMatrixAdaptor
+	/** @} */
+
+/** @} */ // end of grouping
+} // end of NS
+
+
+#endif /* NANOFLANN_HPP_ */

CMakeLists.txt

@@ -0,0 +1,156 @@
+cmake_minimum_required(VERSION 3.16)
+project(Lightweight3DRegLib)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Release)
+endif()
+
+# 第三方库路径
+list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
+
+# --- 第三方库
+find_package(Eigen3 3.4.0 REQUIRED)
+message(STATUS "Found Eigen3 version: ${EIGEN3_VERSION_STRING}")
+find_package(OpenMesh REQUIRED)
+find_package(OpenMP REQUIRED)
+
+# --- 添加 pybind11 支持 ---
+find_package(pybind11 CONFIG REQUIRED)
+
+option(USEPARDISO "Use Pardiso solver" ON)
+
+# --- 源文件
+file(GLOB_RECURSE SOURCES
+    "${CMAKE_CURRENT_SOURCE_DIR}/cpp/core/*.cpp"
+    "${CMAKE_CURRENT_SOURCE_DIR}/cpp/io/*.cpp"
+    "${CMAKE_CURRENT_SOURCE_DIR}/cpp/tools/*.cpp"
+    "${CMAKE_CURRENT_SOURCE_DIR}/cpp/tools/geodesic/*.cpp"
+    "${CMAKE_CURRENT_SOURCE_DIR}/cpp/tools/preprocess/*.cpp"
+)
+
+file(GLOB_RECURSE HEADERS
+    "${CMAKE_CURRENT_SOURCE_DIR}/cpp/*.h"
+    "${CMAKE_CURRENT_SOURCE_DIR}/cpp/core/*.h"
+    "${CMAKE_CURRENT_SOURCE_DIR}/cpp/io/*.h"
+    "${CMAKE_CURRENT_SOURCE_DIR}/cpp/tools/*.h"
+    "${CMAKE_CURRENT_SOURCE_DIR}/cpp/tools/geodesic/*.h"
+    "${CMAKE_CURRENT_SOURCE_DIR}/cpp/tools/preprocess/*.h"
+    "${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/*.h"
+)
+
+# --- 创建静态库
+add_library(Lightweight3DRegLib STATIC ${SOURCES} ${HEADERS})
+
+target_precompile_headers(Lightweight3DRegLib PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/cpp/pch.h")
+
+set_target_properties(Lightweight3DRegLib PROPERTIES POSITION_INDEPENDENT_CODE ON)
+
+# --- include 路径
+target_include_directories(Lightweight3DRegLib
+    PUBLIC 
+        "${CMAKE_CURRENT_SOURCE_DIR}/cpp"
+        "${CMAKE_CURRENT_SOURCE_DIR}/cpp/core"
+        "${CMAKE_CURRENT_SOURCE_DIR}/cpp/io"
+        "${CMAKE_CURRENT_SOURCE_DIR}/cpp/tools"
+        "${CMAKE_CURRENT_SOURCE_DIR}/cpp/tools/geodesic"
+        "${CMAKE_CURRENT_SOURCE_DIR}/cpp/tools/preprocess"
+        "${CMAKE_CURRENT_SOURCE_DIR}/3rdparty"
+        ${EIGEN3_INCLUDE_DIRS}
+)
+
+# --- 链接库
+target_link_libraries(Lightweight3DRegLib
+    PUBLIC OpenMeshCore OpenMeshTools
+    PUBLIC OpenMP::OpenMP_CXX
+)
+
+# --- Pardiso / MKL
+if(USEPARDISO AND ${CMAKE_HOST_SYSTEM_NAME} MATCHES "Linux")
+    if(EXISTS /opt/intel/oneapi/mkl/latest/include/mkl_pardiso.h)
+        target_compile_definitions(Lightweight3DRegLib PUBLIC USE_PARDISO)
+        target_include_directories(Lightweight3DRegLib SYSTEM PUBLIC "/opt/intel/oneapi/mkl/latest/include")
+        target_compile_options(Lightweight3DRegLib PUBLIC "-m64")
+        target_link_libraries(Lightweight3DRegLib PRIVATE
+            "-Wl,--start-group /opt/intel/oneapi/mkl/latest/lib/intel64/libmkl_intel_lp64.a"
+            "/opt/intel/oneapi/mkl/latest/lib/intel64/libmkl_gnu_thread.a"
+            "/opt/intel/oneapi/mkl/latest/lib/intel64/libmkl_core.a"
+            "-Wl,--end-group -lgomp -lpthread -lm -ldl"
+        )
+    else()
+        message("Cannot find Intel MKL at /opt/intel/oneapi/mkl. Using Eigen LDLT.")
+    endif()
+endif()
+
+# --- 添加 Python 模块 ---
+pybind11_add_module(pyregister ${CMAKE_CURRENT_SOURCE_DIR}/python/pyregister.cpp)
+
+target_link_libraries(pyregister 
+    PRIVATE 
+        Lightweight3DRegLib  
+        OpenMeshCore 
+        OpenMeshTools
+        OpenMP::OpenMP_CXX
+)
+
+target_include_directories(pyregister 
+    PRIVATE
+        "${CMAKE_CURRENT_SOURCE_DIR}/cpp"
+        "${CMAKE_CURRENT_SOURCE_DIR}/cpp/core"
+        "${CMAKE_CURRENT_SOURCE_DIR}/cpp/io"
+        "${CMAKE_CURRENT_SOURCE_DIR}/cpp/tools"
+        "${CMAKE_CURRENT_SOURCE_DIR}/cpp/tools/geodesic"
+        "${CMAKE_CURRENT_SOURCE_DIR}/cpp/tools/preprocess"
+        "${CMAKE_CURRENT_SOURCE_DIR}/3rdparty"
+        ${EIGEN3_INCLUDE_DIRS}
+)
+
+target_precompile_headers(pyregister REUSE_FROM Lightweight3DRegLib)
+
+if(USEPARDISO AND ${CMAKE_HOST_SYSTEM_NAME} MATCHES "Linux")
+    if(EXISTS /opt/intel/oneapi/mkl/latest/include/mkl_pardiso.h)
+        target_compile_definitions(pyregister PRIVATE USE_PARDISO)
+        target_include_directories(pyregister SYSTEM PRIVATE "/opt/intel/oneapi/mkl/latest/include")
+        target_compile_options(pyregister PRIVATE "-m64")
+        target_link_libraries(pyregister PRIVATE
+            "-Wl,--start-group /opt/intel/oneapi/mkl/latest/lib/intel64/libmkl_intel_lp64.a"
+            "/opt/intel/oneapi/mkl/latest/lib/intel64/libmkl_gnu_thread.a"
+            "/opt/intel/oneapi/mkl/latest/lib/intel64/libmkl_core.a"
+            "-Wl,--end-group -lgomp -lpthread -lm -ldl"
+        )
+    endif()
+endif()
+
+set_target_properties(pyregister PROPERTIES 
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/python
+)
+
+
+# =====================================================================
+# --- 添加示例可执行程序 (examples/c++/main.cpp) ---
+# =====================================================================
+file(GLOB EXAMPLE_SOURCES
+    "${CMAKE_CURRENT_SOURCE_DIR}/examples/c++/*.cpp"
+)
+
+
+add_executable(Lightweight3DRegDemo ${EXAMPLE_SOURCES})
+
+target_precompile_headers(Lightweight3DRegDemo PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/cpp/pch.h")
+
+
+target_link_libraries(Lightweight3DRegDemo
+    PRIVATE
+        Lightweight3DRegLib
+        OpenMeshCore 
+        OpenMeshTools
+        OpenMP::OpenMP_CXX
+)
+
+
+
+set_target_properties(Lightweight3DRegDemo PROPERTIES
+    RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/examples
+)

README.md

@@ -0,0 +1,14 @@
+---
+title: Lowweight Register
+emoji: 🚀
+colorFrom: purple
+colorTo: red
+sdk: gradio
+sdk_version: 5.47.2
+app_file: app.py
+pinned: false
+license: apache-2.0
+short_description: '00000'
+---
+
+Check out the configuration reference at https://huggingface.co/docs/hub/spaces-config-reference

app.py

@@ -0,0 +1,638 @@
+# app.py
+import os
+import sys
+import glob
+import numpy as np
+import trimesh
+import plotly.graph_objs as go
+import gradio as gr
+from plyfile import PlyData
+
+# ------------------ Load pyregister module ------------------
+so_candidates = glob.glob("build/python/pyregister*.so")
+if not so_candidates:
+    os.system("bash build.sh")
+    so_candidates = glob.glob("build/python/pyregister*.so")
+if not so_candidates:
+    raise FileNotFoundError("pyregister build failed, no .so found in build/python")
+sys.path.append("build/python")
+
+import pyregister  
+
+# ------------------ Example data ------------------
+EXAMPLES = {
+    "Rigid":    {"target": "./examples/data/fricp/target.ply", "source": "./examples/data/fricp/source.ply"},
+    "NonRigid": {"target": "./examples/data/spare/target.obj", "source": "./examples/data/spare/source.obj"},
+}
+
+FRICP_PARAS = {
+    "useinit": False,
+    "fileinit": "",
+    "maxiter": 100,
+    "stop": 1e-5
+}
+
+SPARE_PARAS = {
+    "iters": 30,
+    "stopcoarse": 1e-3,
+    "stopfine": 1e-4,
+    "use_landmark": False,
+    "src": [],
+    "tar": []
+}
+
+def load_example(example_type):
+    target_path = EXAMPLES[example_type]["target"]
+    source_path = EXAMPLES[example_type]["source"]
+    method = example_type
+    return source_path, target_path, method
+
+def read_mesh(file_path):
+    ext = os.path.splitext(file_path)[1].lower()
+    if ext == ".ply":
+        plydata = PlyData.read(file_path)
+        vertex = plydata["vertex"]
+        vertices = np.stack([vertex["x"], vertex["y"], vertex["z"]], axis=-1)
+        faces = None
+        if "face" in plydata and len(plydata["face"].data) > 0:
+            face_list = [f[0] for f in plydata["face"].data]
+            if len(face_list) > 0:
+                faces = np.array(face_list)
+                if faces.ndim == 1:
+                    faces = faces.reshape(-1, 3)
+        return vertices, faces
+    elif ext == ".obj":
+        mesh = trimesh.load(file_path, process=False)
+        return np.asarray(mesh.vertices), (np.asarray(mesh.faces) if mesh.faces.size > 0 else None)
+    else:
+        raise ValueError(f"Unsupported file type: {ext}")
+
+def plot_source_target_mesh(target_file, source_file, alpha_target=1.0, alpha_source=1.0, scatter_mode=False):
+    target_v, target_f = read_mesh(target_file)
+    source_v, source_f = read_mesh(source_file)
+
+    fig = go.Figure()
+    lighting_opts = dict(ambient=0.5, diffuse=0.8, specular=0.6, roughness=0.25)
+    light_pos = dict(x=100, y=200, z=50)
+
+    target_color = "crimson"
+
+    if scatter_mode:
+
+        fig.add_trace(go.Scatter3d(
+            x=target_v[:, 0], y=target_v[:, 1], z=target_v[:, 2],
+            mode="markers",
+  
+            marker=dict(size=0.6, color=target_color, opacity=1.0),
+            name="Target Points",
+            hoverinfo="text",
+            text=[
+                f"<b><span style='color:black;'>Target ID:</span></b> {i}"
+                f"<br><span style='font-size:12px;'>x={x:.3f}, y={y:.3f}, z={z:.3f}</span>"
+                for i, (x, y, z) in enumerate(target_v)
+            ],
+            hoverlabel=dict(
+                bgcolor="white",
+                bordercolor="black",
+                font=dict(color="black", size=13, family="Arial", weight="bold")
+            )
+        ))
+
+        fig.add_trace(go.Scatter3d(
+            x=source_v[:, 0], y=source_v[:, 1], z=source_v[:, 2],
+            mode="markers",
+
+            marker=dict(size=0.6, color="limegreen", opacity=1.0),
+            name="Source Points",
+            hoverinfo="text",
+            text=[
+                f"<b><span style='color:red;'>Source ID:</span></b> {i}"
+                f"<br><span style='font-size:12px;'>x={x:.3f}, y={y:.3f}, z={z:.3f}</span>"
+                for i, (x, y, z) in enumerate(source_v)
+            ],
+            hoverlabel=dict(
+                bgcolor="white",
+                bordercolor="limegreen",
+                font=dict(color="red", size=13, family="Arial", weight="bold")
+            )
+        ))
+
+    else:
+
+        if target_f is not None:
+            fig.add_trace(go.Mesh3d(
+                x=target_v[:, 0], y=target_v[:, 1], z=target_v[:, 2],
+                i=target_f[:, 0], j=target_f[:, 1], k=target_f[:, 2],
+                color="khaki", opacity=alpha_target, name="Target Mesh",
+                lighting=lighting_opts, lightposition=light_pos,
+                hovertemplate="<b>Target Surface</b><br>x=%{x:.3f}<br>y=%{y:.3f}<br>z=%{z:.3f}<extra></extra>",
+            ))
+        else:
+            fig.add_trace(go.Scatter3d(
+                x=target_v[:, 0], y=target_v[:, 1], z=target_v[:, 2],
+                mode="markers",
+                marker=dict(size=0.6, color=target_color, opacity=alpha_target),  
+                name="Target Points"
+            ))
+
+        if source_f is not None:
+            fig.add_trace(go.Mesh3d(
+                x=source_v[:, 0], y=source_v[:, 1], z=source_v[:, 2],
+                i=source_f[:, 0], j=source_f[:, 1], k=source_f[:, 2],
+                color="darkseagreen", opacity=alpha_source, name="Source Mesh",
+                lighting=lighting_opts, lightposition=light_pos,
+                hovertemplate="<b>Source Surface</b><br>x=%{x:.3f}<br>y=%{y:.3f}<br>z=%{z:.3f}<extra></extra>",
+            ))
+        else:
+            fig.add_trace(go.Scatter3d(
+                x=source_v[:, 0], y=source_v[:, 1], z=source_v[:, 2],
+                mode="markers",
+                marker=dict(size=0.6, color="limegreen", opacity=alpha_source),  
+                name="Source Points"
+            ))
+
+    fig.update_layout(
+        height=600, width=600, margin=dict(l=0, r=0, t=40, b=0),
+        scene=dict(aspectmode="data"), showlegend=True
+    )
+    return fig
+
+
+
+def plot_result_target_mesh(target_file, result_file):
+    target_v, target_f = read_mesh(target_file)
+    result_v, result_f = read_mesh(result_file)
+
+    fig = go.Figure()
+    lighting_opts = dict(ambient=0.5, diffuse=0.8, specular=0.6, roughness=0.25)
+    light_pos = dict(x=100, y=200, z=50)
+
+    target_mesh_color = "khaki"
+    result_mesh_color = "lightblue"
+
+    if target_f is not None:
+        fig.add_trace(go.Mesh3d(
+            x=target_v[:, 0], y=target_v[:, 1], z=target_v[:, 2],
+            i=target_f[:, 0], j=target_f[:, 1], k=target_f[:, 2],
+            color=target_mesh_color, opacity=1.0, name="Target Mesh",
+            lighting=lighting_opts, lightposition=light_pos
+        ))
+    else:
+        fig.add_trace(go.Scatter3d(
+            x=target_v[:, 0], y=target_v[:, 1], z=target_v[:, 2],
+            mode="markers",
+            marker=dict(size=0.6, color="crimson", opacity=1.0),
+            name="Target Points (Red)"
+        ))
+
+    if result_f is not None:
+        fig.add_trace(go.Mesh3d(
+            x=result_v[:, 0], y=result_v[:, 1], z=result_v[:, 2],
+            i=result_f[:, 0], j=result_f[:, 1], k=result_f[:, 2],
+            color=result_mesh_color, opacity=1.0, name="Result Mesh",
+            lighting=lighting_opts, lightposition=light_pos
+        ))
+    else:
+        fig.add_trace(go.Scatter3d(
+            x=result_v[:, 0], y=result_v[:, 1], z=result_v[:, 2],
+            mode="markers",
+            marker=dict(size=0.6, color="royalblue", opacity=1.0),
+            name="Result Points (Blue)"
+        ))
+
+    fig.update_layout(height=600, width=600, margin=dict(l=0, r=0, t=40, b=0),
+                      scene=dict(aspectmode="data"), showlegend=True)
+    return fig
+
+# ------------------ Registration ------------------
+def register_and_visualize_with_zip(target_file, source_file, output_dir, method,
+                                    useinit=False, fileinit=None, maxiter=100, stop=1e-5,
+                                    iters=30, stopcoarse=1e-3, stopfine=1e-4):
+    if target_file is None or source_file is None:
+        raise gr.Error("Please upload both target and source point cloud files first!")
+
+    target_path = target_file.name
+    source_path = source_file.name
+    os.makedirs(output_dir, exist_ok=True)
+
+    # === Rigid ===
+    if method == "Rigid":
+        reg = pyregister.RigidFricpRegistration()
+        FRICP_PARAS["useinit"] = bool(useinit)
+        FRICP_PARAS["fileinit"] = fileinit.name if useinit and fileinit else ""
+        FRICP_PARAS["maxiter"] = int(maxiter)
+        FRICP_PARAS["stop"] = float(stop)
+
+        reg.Paras_init(FRICP_PARAS["useinit"],
+                       FRICP_PARAS["fileinit"],
+                       FRICP_PARAS["maxiter"],
+                       FRICP_PARAS["stop"])
+        output_file = "FRICP_res.ply"
+
+    # === NonRigid ===
+    else:
+        reg = pyregister.NonrigidSpareRegistration()
+        SPARE_PARAS["iters"] = int(iters)
+        SPARE_PARAS["stopcoarse"] = float(stopcoarse)
+        SPARE_PARAS["stopfine"] = float(stopfine)
+
+        reg.Paras_init(
+            SPARE_PARAS["iters"],
+            SPARE_PARAS["stopcoarse"],
+            SPARE_PARAS["stopfine"],
+            SPARE_PARAS["use_landmark"],
+            SPARE_PARAS["src"],
+            SPARE_PARAS["tar"]
+        )
+        output_file = "spare_res.ply"
+
+
+    reg.Reg(target_path, source_path, output_dir)
+
+    result_path = os.path.join(output_dir, output_file)
+    fig_result = plot_result_target_mesh(target_path, result_path)
+
+    import zipfile
+    zip_path = os.path.join(output_dir, "results.zip")
+    with zipfile.ZipFile(zip_path, "w", zipfile.ZIP_DEFLATED) as zipf:
+        for f in os.listdir(output_dir):
+            if f.endswith(".ply"):
+                zipf.write(os.path.join(output_dir, f), arcname=f)
+
+    return fig_result, zip_path
+
+
+# ------------------ Reregister ------------------
+def reuse_last_result_as_source(target_file, output_dir):
+    if target_file is None:
+        raise gr.Error("Please upload the target point cloud file first!")
+    if not os.path.exists(output_dir):
+        raise gr.Error("Output directory does not exist!")
+
+    result_candidates = []
+    for fname in ["FRICP_res.ply", "spare_res.ply"]:
+        fpath = os.path.join(output_dir, fname)
+        if os.path.exists(fpath):
+            result_candidates.append(fpath)
+    if not result_candidates:
+        raise gr.Error("No previous registration result found. Please run registration first!")
+
+    result_candidates.sort(key=os.path.getmtime, reverse=True)
+    latest_result = result_candidates[0]
+
+    fig = plot_source_target_mesh(target_file.name, latest_result)
+    return latest_result, fig
+
+# ------------------ Utility ------------------
+def reset_parameters():
+    """
+    Reset all registration parameters (Rigid + NonRigid + Landmarks)
+    and clear any manual inputs in the UI.
+    This also resets the 'Use Landmarks' checkbox and hides the landmark UI group.
+    """
+
+    # --- Reset Rigid (FRICP) parameters ---
+    FRICP_PARAS.update({
+        "useinit": False,
+        "fileinit": "",
+        "maxiter": 100,
+        "stop": 1e-5
+    })
+
+    # --- Reset NonRigid (Spare) parameters ---
+    SPARE_PARAS.update({
+        "iters": 30,
+        "stopcoarse": 1e-3,
+        "stopfine": 1e-4,
+        "use_landmark": False,
+        "src": [],
+        "tar": []
+    })
+
+    print("[reset] All parameters reset.")
+    print("[reset] FRICP_PARAS:", FRICP_PARAS)
+    print("[reset] SPARE_PARAS:", SPARE_PARAS)
+
+
+    return (
+        FRICP_PARAS["useinit"],     # Checkbox
+        None,                       # File input reset
+        FRICP_PARAS["maxiter"],
+        FRICP_PARAS["stop"],
+        SPARE_PARAS["iters"],
+        SPARE_PARAS["stopcoarse"],
+        SPARE_PARAS["stopfine"],
+        gr.update(value=""),        
+        gr.update(value=""),      
+        gr.update(value=False),    
+        gr.update(visible=False)   
+    )
+
+
+def clear_all():
+    """
+    Full reset for all UI elements:
+    - Clears uploaded files
+    - Resets plots
+    - Resets all parameters (Rigid + NonRigid + Landmark)
+    - Resets landmark checkbox & hides group
+    """
+
+    FRICP_PARAS.update({
+        "useinit": False,
+        "fileinit": "",
+        "maxiter": 100,
+        "stop": 1e-5
+    })
+    SPARE_PARAS.update({
+        "iters": 30,
+        "stopcoarse": 1e-3,
+        "stopfine": 1e-4,
+        "use_landmark": False,
+        "src": [],
+        "tar": []
+    })
+
+    print("[clear] All files, parameters, and landmarks cleared.")
+    return (
+        None,  # target_input
+        None,  # source_input
+        None,  # upload_plot
+        None,  # result_plot
+        FRICP_PARAS["useinit"],  # fricp_useinit
+        None,  # fricp_fileinit
+        FRICP_PARAS["maxiter"],
+        FRICP_PARAS["stop"],
+        SPARE_PARAS["iters"],
+        SPARE_PARAS["stopcoarse"],
+        SPARE_PARAS["stopfine"],
+        gr.update(value=""),    
+        gr.update(value=""),      
+        gr.update(value=False),   
+        gr.update(visible=False)  
+    )
+
+
+def visualize_and_store(target_file, source_file, scatter_mode=False):
+    if target_file is None or source_file is None:
+        return None, None, None
+    target_v, _ = read_mesh(target_file.name)
+    source_v, _ = read_mesh(source_file.name)
+    fig = plot_source_target_mesh(target_file.name, source_file.name, scatter_mode=scatter_mode)
+
+    return fig, source_v, target_v
+
+def clear_landmarks():
+    return [], [], "", ""
+def highlight_landmarks_on_mesh(target_file, source_file, src_text, tar_text):
+    src_v, _ = read_mesh(source_file.name)
+    tar_v, _ = read_mesh(target_file.name)
+    src_ids = [int(i) for i in src_text.split(",") if i.strip().isdigit()]
+    tar_ids = [int(i) for i in tar_text.split(",") if i.strip().isdigit()]
+   
+
+    fig = plot_source_target_mesh(target_file.name, source_file.name)
+
+    # === Source landmarks ===
+    if len(src_ids) > 0:
+        pts = src_v[src_ids]
+        fig.add_trace(go.Scatter3d(
+            x=pts[:, 0], y=pts[:, 1], z=pts[:, 2],
+            mode="markers+text",
+            text=[str(i) for i in src_ids],
+            textposition="top center",
+            textfont=dict(color="black", size=10, family="Arial"),
+            marker=dict(
+                size=6,
+                color="limegreen",
+                opacity=0.9,
+                line=dict(width=2, color="white"),
+                symbol="circle"
+            ),
+            name="Source Landmarks",
+            hoverinfo="text",
+            hovertext=[
+                f"<b>Source ID:</b> {i}<br>x={x:.3f}, y={y:.3f}, z={z:.3f}"
+                for i, (x, y, z) in zip(src_ids, pts)
+            ]
+        ))
+
+    # === Target landmarks ===
+    if len(tar_ids) > 0:
+        pts = tar_v[tar_ids]
+        fig.add_trace(go.Scatter3d(
+            x=pts[:, 0], y=pts[:, 1], z=pts[:, 2],
+            mode="markers+text",
+            text=[str(i) for i in tar_ids],
+            textposition="top center",
+            textfont=dict(color="black", size=10, family="Arial"),
+            marker=dict(
+                size=6,
+                color="crimson",
+                opacity=0.9,
+                line=dict(width=2, color="white"),
+                symbol="circle"
+            ),
+            name="Target Landmarks",
+            hoverinfo="text",
+            hovertext=[
+                f"<b>Target ID:</b> {i}<br>x={x:.3f}, y={y:.3f}, z={z:.3f}"
+                for i, (x, y, z) in zip(tar_ids, pts)
+            ]
+        ))
+
+
+    fig.update_layout(
+        scene=dict(
+            xaxis=dict(visible=False),
+            yaxis=dict(visible=False),
+            zaxis=dict(visible=False)
+        ),
+        legend=dict(bgcolor="rgba(255,255,255,0.6)"),
+        margin=dict(l=0, r=0, t=40, b=0),
+        title="Landmark Highlight View"
+    )
+
+    return fig
+
+def start_landmark_selection(target_file, source_file):
+    if target_file is None or source_file is None:
+        raise gr.Error("Please upload both Source and Target first!")
+
+    fig = plot_source_target_mesh(target_file.name, source_file.name, scatter_mode=True)
+    return fig
+def update_landmark_ids(src_text, tar_text):
+    src_ids = [int(i) for i in src_text.split(",") if i.strip().isdigit()]
+    tar_ids = [int(i) for i in tar_text.split(",") if i.strip().isdigit()]
+    return src_ids, tar_ids, src_text, tar_text
+
+
+def set_landmarks_to_registration(src_ids, tar_ids):
+    if not src_ids or not tar_ids:
+        raise gr.Error("Please select both Source and Target points first!")
+    if len(src_ids) != len(tar_ids):
+        raise gr.Error(f"Landmark count mismatch: Source={len(src_ids)} vs Target={len(tar_ids)}")
+
+    SPARE_PARAS["use_landmark"] = True
+    SPARE_PARAS["src"] = [int(i) for i in src_ids]
+    SPARE_PARAS["tar"] = [int(i) for i in tar_ids]
+
+    return f"✅ Landmarks staged: {len(src_ids)} Source ↔ {len(tar_ids)} Target. Will be used in NonRigid registration."
+
+# ------------------ UI ------------------
+CSS_FIX = """
+html, body, .gradio-container {
+    min-height: 100vh;
+    overflow-y: auto;
+}
+"""
+
+
+
+with gr.Blocks(css=CSS_FIX) as demo:
+    gr.Markdown("## Point Cloud / Mesh Registration Visualization Tool")
+
+    src_ids_state = gr.State([])
+    tar_ids_state = gr.State([])
+    src_vertices_state = gr.State(None)
+    tar_vertices_state = gr.State(None)
+
+    with gr.Row():
+        with gr.Column(scale=1):
+
+            source_input = gr.File(label="Source File", file_types=[".ply", ".obj"])
+            target_input = gr.File(label="Target File", file_types=[".ply", ".obj"])
+            method_dropdown = gr.Dropdown(label="Registration Method",
+                                          choices=["Rigid", "NonRigid"], value="Rigid")
+
+            with gr.Accordion("Rigid Parameters", open=False):
+                fricp_useinit = gr.Checkbox(label="Use initial transform?", value=False)
+                fricp_fileinit = gr.File(label="Initial transform file", file_types=[".txt"])
+                fricp_maxiter = gr.Number(label="Max iterations", value=100)
+                fricp_stop = gr.Number(label="Stop threshold", value=1e-5)
+
+
+            with gr.Accordion("NonRigid Parameters", open=False):
+                spare_iters = gr.Number(label="Iterations", value=30)
+                spare_stopcoarse = gr.Number(label="Stop Coarse", value=1e-3)
+                spare_stopfine = gr.Number(label="Stop Fine", value=1e-4)
+
+
+                use_landmark_cb = gr.Checkbox(label="Use Landmarks", value=False)
+
+
+                with gr.Group(visible=False) as landmark_group:
+                    gr.Markdown("### Landmark Point Selection")
+                    selected_src = gr.Textbox(
+                        label="Selected Source IDs (comma-separated)",
+                        placeholder="e.g. 12,45,89", interactive=True)
+                    selected_tar = gr.Textbox(
+                        label="Selected Target IDs (comma-separated)",
+                        placeholder="e.g. 7,42,105", interactive=True)
+
+                    with gr.Row():
+                        start_landmark_btn = gr.Button("Start Landmark Selection", elem_classes="primary")
+                        highlight_btn = gr.Button("Highlight Landmarks")
+                        clear_landmark_btn = gr.Button("🧹 Clear Selections")
+
+                    select_button = gr.Button("Confirm Landmarks to Registration", elem_classes="primary")
+
+
+            output_dir = gr.Textbox(label="Output Directory", value="./output/", placeholder="./output/")
+            with gr.Row():
+                run_button = gr.Button("Run Registration", elem_classes="primary")
+                rerun_button = gr.Button("Reregister (Use Last Result as Source)", elem_classes="primary")
+                clear_button = gr.Button("Clear", elem_classes="secondary")
+                reset_button = gr.Button("Reset Parameters", elem_classes="primary")
+
+            
+            with gr.Row():
+                example_dropdown = gr.Dropdown(label="Load Example Data",
+                                               choices=["Rigid", "NonRigid"], value="Rigid")
+                example_button = gr.Button("Load Example")
+
+            example_button.click(fn=load_example,
+                                 inputs=[example_dropdown],
+                                 outputs=[source_input, target_input, method_dropdown])
+
+        
+        with gr.Column(scale=2):
+            upload_plot = gr.Plot(label="Source vs Target Mesh")
+            result_plot = gr.Plot(label="Result vs Target Mesh")
+            download_button = gr.File(label="Download Result Directory",
+                                      file_types=[".zip"], interactive=False)
+
+
+    reset_button.click(
+        fn=reset_parameters,
+        inputs=None,
+        outputs=[
+            fricp_useinit, fricp_fileinit, fricp_maxiter, fricp_stop,
+            spare_iters, spare_stopcoarse, spare_stopfine,
+            selected_src, selected_tar,
+            use_landmark_cb, landmark_group
+        ]
+    )
+
+    def toggle_landmark_visibility(use_landmark):
+        SPARE_PARAS["use_landmark"] = bool(use_landmark)
+        print(f"[UI] use_landmark set to {SPARE_PARAS['use_landmark']}")
+        return gr.update(visible=use_landmark)
+
+    use_landmark_cb.change(fn=toggle_landmark_visibility,
+                           inputs=[use_landmark_cb],
+                           outputs=[landmark_group])
+
+    clear_button.click(
+    fn=clear_all,
+    inputs=None,
+    outputs=[
+        target_input, source_input, upload_plot, result_plot,
+        fricp_useinit, fricp_fileinit, fricp_maxiter, fricp_stop,
+        spare_iters, spare_stopcoarse, spare_stopfine,
+        selected_src, selected_tar,
+        use_landmark_cb, landmark_group
+    ]
+)
+
+
+    target_input.change(fn=visualize_and_store,
+                        inputs=[target_input, source_input],
+                        outputs=[upload_plot, src_vertices_state, tar_vertices_state])
+    source_input.change(fn=visualize_and_store,
+                        inputs=[target_input, source_input],
+                        outputs=[upload_plot, src_vertices_state, tar_vertices_state])
+
+    start_landmark_btn.click(fn=start_landmark_selection,
+                             inputs=[target_input, source_input],
+                             outputs=[upload_plot])
+
+    highlight_btn.click(fn=highlight_landmarks_on_mesh,
+                        inputs=[target_input, source_input, selected_src, selected_tar],
+                        outputs=[upload_plot])
+
+    clear_landmark_btn.click(fn=clear_landmarks,
+                             inputs=None,
+                             outputs=[src_ids_state, tar_ids_state, selected_src, selected_tar])
+
+    select_button.click(fn=update_landmark_ids,
+                        inputs=[selected_src, selected_tar],
+                        outputs=[src_ids_state, tar_ids_state, selected_src, selected_tar]
+                        ).then(fn=set_landmarks_to_registration,
+                               inputs=[src_ids_state, tar_ids_state],
+                               outputs=[gr.Textbox(label="Landmark Setup Info")])
+
+    run_button.click(fn=register_and_visualize_with_zip,
+                     inputs=[target_input, source_input, output_dir, method_dropdown,
+                             fricp_useinit, fricp_fileinit, fricp_maxiter, fricp_stop,
+                             spare_iters, spare_stopcoarse, spare_stopfine],
+                     outputs=[result_plot, download_button])
+
+    rerun_button.click(fn=reuse_last_result_as_source,
+                       inputs=[target_input, output_dir],
+                       outputs=[source_input, upload_plot])
+
+if __name__ == "__main__":
+    demo.launch(server_name="0.0.0.0", server_port=7860)
+
+

apt.txt

@@ -0,0 +1,16 @@
+
+build-essential
+cmake
+g++
+git
+wget
+curl
+make
+pkg-config
+libeigen3-dev
+libopenmesh-dev
+pybind11-dev
+python3-dev
+python3-pip
+
+

build.sh

@@ -0,0 +1,22 @@
+#!/usr/bin/env bash
+set -e
+
+# 安装系统依赖
+apt-get update
+xargs -a apt.txt apt-get install -y
+
+# 清理并创建 build 目录
+rm -rf build
+mkdir build
+cd build
+
+# 生成 Makefile，指定 Python
+cmake .. -DPYTHON_EXECUTABLE=$(which python3)
+
+# 编译
+cmake --build . --config Release -j$(nproc)
+
+# 回到主目录
+cd ..
+
+echo "✅ Build complete. .so is in build/python/"

cmake/FindEigen3.cmake

@@ -0,0 +1,19 @@
+FIND_PATH( EIGEN3_INCLUDE_DIRS Eigen/Geometry
+    $ENV{EIGEN3DIR}/include
+    /usr/local/include/eigen3
+    /usr/local/include
+    /usr/local/X11R6/include
+    /usr/X11R6/include
+    /usr/X11/include
+    /usr/include/X11
+    /usr/include/eigen3/
+    /usr/include
+    /opt/X11/include
+    /opt/include 
+    ${CMAKE_SOURCE_DIR}/external/eigen/include
+    ${CMAKE_SOURCE_DIR}/include/eigen3)
+
+SET(EIGEN3_FOUND "NO")
+IF(EIGEN3_INCLUDE_DIRS)
+    SET(EIGEN3_FOUND "YES")
+ENDIF()

cmake/FindNanoFlann.cmake

@@ -0,0 +1,24 @@
+FIND_PATH(
+    # filled variable
+    NANOFLANN_INCLUDE_DIR
+    # what I am looking for?
+    nanoflann.hpp
+    # where should I look?
+    $ENV{NANOFLANN_DIR}
+    ${CMAKE_SOURCE_DIR}/include)
+
+IF(NANOFLANN_INCLUDE_DIR)
+   SET(NANOFLANN_FOUND TRUE)
+else()
+   SET(NANOFLANN_FOUND FALSE)
+ENDIF()
+
+IF(NANOFLANN_FOUND)
+    IF(NOT CMAKE_FIND_QUIETLY)
+        MESSAGE(STATUS "Found NanoFlann: ${NANOFLANN_INCLUDE_DIR}")
+    ENDIF()
+ELSE()
+    IF(NANOFLANN_FOUND_REQUIRED)
+        MESSAGE(FATAL_ERROR "Could not find NanoFlann")
+    ENDIF()
+ENDIF()

cmake/FindOpenMesh.cmake

@@ -0,0 +1,27 @@
+# - Try to find OpenMesh
+# This is a minimal FindOpenMesh.cmake for systems with libopenmesh-dev installed
+
+# 查找头文件
+find_path(OPENMESH_INCLUDE_DIR OpenMesh/Core/IO/MeshIO.hh
+          PATHS /usr/include /usr/include/OpenMesh
+          PATH_SUFFIXES OpenMesh)
+
+# 查找库文件
+find_library(OPENMESH_CORE_LIBRARY OpenMeshCore
+             PATHS /usr/lib /usr/lib/x86_64-linux-gnu)
+
+find_library(OPENMESH_TOOLS_LIBRARY OpenMeshTools
+             PATHS /usr/lib /usr/lib/x86_64-linux-gnu)
+
+# 设置变量给 CMake 使用
+set(OPENMESH_LIBRARIES ${OPENMESH_CORE_LIBRARY} ${OPENMESH_TOOLS_LIBRARY})
+set(OPENMESH_FOUND TRUE)
+
+# 输出信息
+if(OPENMESH_FOUND)
+    message(STATUS "Found OpenMesh:")
+    message(STATUS "  Include dir: ${OPENMESH_INCLUDE_DIR}")
+    message(STATUS "  Libraries: ${OPENMESH_LIBRARIES}")
+else()
+    message(FATAL_ERROR "OpenMesh not found")
+endif()

cpp/core/nonrigid_spare_registration.cpp

@@ -0,0 +1,1013 @@
+
+#include "nonrigid_spare_registration.h"
+#include "tools.h"
+#include "robust_norm.h"
+#include <median.h>
+
+#if (__cplusplus >= 201402L) || (defined(_MSC_VER) && _MSC_VER >= 1800)
+#define MAKE_UNIQUE std::make_unique
+#else
+#define MAKE_UNIQUE company::make_unique
+#endif
+
+/// @param Source (one 3D point per column)
+/// @param Target (one 3D point per column)
+/// @param Confidence weights
+
+
+NonrigidSpareRegistration::NonrigidSpareRegistration() {
+};
+
+NonrigidSpareRegistration::~NonrigidSpareRegistration()
+{
+}
+
+void NonrigidSpareRegistration::Initialize()
+{
+
+    //welsch weight paramete
+    InitWelschParam();
+
+
+    int knn_num_neighbor = 6;
+
+    if(!pars_.use_geodesic_dist)
+    {
+        src_knn_indices_.resize(knn_num_neighbor, n_src_vertex_);
+        KDtree* src_tree = new KDtree(src_points_);
+#pragma omp parallel for
+        for(int i = 0; i < n_src_vertex_; i++)
+        {
+            int* out_indices = new int[knn_num_neighbor+1];
+            Scalar *out_dists = new Scalar[knn_num_neighbor+1];
+            src_tree->query(src_points_.col(i).data(), knn_num_neighbor+1, out_indices, out_dists);
+            for(int j = 0; j < knn_num_neighbor; j++)
+            {
+                src_knn_indices_(j, i) = out_indices[j+1];
+            }
+            delete[] out_indices;
+            delete[] out_dists;
+        }
+        delete src_tree;
+    }
+
+
+    Scalar sample_radius;
+
+    if(pars_.use_geodesic_dist)
+        sample_radius = src_sample_nodes.SampleAndConstuct(*src_mesh_, pars_.uni_sample_radio,  src_points_);
+    else
+        sample_radius = src_sample_nodes.SampleAndConstuctFPS(*src_mesh_, pars_.uni_sample_radio, src_points_, src_knn_indices_, 4, 8);
+
+
+    num_sample_nodes = src_sample_nodes.nodeSize();
+    pars_.num_sample_nodes = num_sample_nodes;
+
+    X_.resize(12 * num_sample_nodes); X_.setZero();
+    align_coeff_PV0_.resize(3 * n_src_vertex_, 12 * num_sample_nodes);
+    nodes_P_.resize(n_src_vertex_ * 3);
+
+    nodes_R_.resize(9 * num_sample_nodes); nodes_R_.setZero();
+    rigid_coeff_L_.resize(12 * num_sample_nodes, 12 * num_sample_nodes);
+    rigid_coeff_J_.resize(12 * num_sample_nodes, 9 * num_sample_nodes);
+
+    std::vector<Triplet> coeffv(4 * num_sample_nodes);
+    std::vector<Triplet> coeffL(9 * num_sample_nodes);
+    std::vector<Triplet> coeffJ(9 * num_sample_nodes);
+    for (int i = 0; i < num_sample_nodes; i++)
+    {
+        // X_
+        X_[12 * i] = 1.0;
+        X_[12 * i + 4] = 1.0;
+        X_[12 * i + 8] = 1.0;
+
+        // nodes_R_
+        nodes_R_[9 * i] = 1.0;
+        nodes_R_[9 * i + 4] = 1.0;
+        nodes_R_[9 * i + 8] = 1.0;
+
+        for (int j = 0; j < 9; j++)
+        {
+            // rigid_coeff_L_
+            coeffL[9 * i + j] = Triplet(12 * i + j, 12 * i + j, 1.0);
+            // rigid_coeff_J_
+            coeffJ[9 * i + j] = Triplet(12 * i + j, 9 * i + j, 1.0);
+        }
+    }
+    rigid_coeff_L_.setFromTriplets(coeffL.begin(), coeffL.end());
+    rigid_coeff_J_.setFromTriplets(coeffJ.begin(), coeffJ.end());
+
+
+    // update coefficient matrices
+    src_sample_nodes.initWeight(align_coeff_PV0_, nodes_P_,
+                                reg_coeff_B_, reg_right_D_, reg_cwise_weights_);
+
+    num_graph_edges = reg_cwise_weights_.rows();
+
+
+
+    // update ARAP coeffs
+    FullInARAPCoeff();
+
+    local_rotations_.resize(3, n_src_vertex_ * 3);
+
+    if(pars_.use_geodesic_dist)
+    {
+        num_edges = src_mesh_->n_halfedges();
+        arap_right_.resize(3*src_mesh_->n_halfedges());
+        arap_right_fine_.resize(3 * src_mesh_->n_halfedges());
+    }
+    else{
+        num_edges = knn_num_neighbor*n_src_vertex_;
+        arap_right_.resize(3*knn_num_neighbor*n_src_vertex_);
+        arap_right_fine_.resize(3 * knn_num_neighbor*n_src_vertex_);
+    }
+
+
+
+    InitRotations();
+
+
+
+    sampling_indices_.clear();
+
+    // start points
+    size_t startIndex = 0;
+    sampling_indices_.push_back(startIndex);
+
+
+    // FPS to get sampling points in align term
+
+    VectorX minDistances(n_src_vertex_);
+    minDistances.setConstant(std::numeric_limits<Scalar>::max());
+    minDistances[startIndex] = 0;
+
+    vertex_sample_indices_.resize(n_src_vertex_, -1);
+    vertex_sample_indices_[startIndex] = 0;
+
+    // repeat select farthest points
+    while (sampling_indices_.size() < align_sampling_num_) {
+// calculate the distance between each point with the sampling points set.
+#pragma omp parallel for
+        for (size_t i = 0; i < n_src_vertex_; ++i) {
+            if(i==startIndex)
+                continue;
+
+            Scalar dist = (src_points_.col(startIndex) - src_points_.col(i)).norm();
+            if(dist < minDistances[i])
+                minDistances[i] = dist;
+        }
+
+        // choose farthest point
+        int maxDistanceIndex;
+        minDistances.maxCoeff(&maxDistanceIndex);
+        minDistances[maxDistanceIndex] = 0;
+
+        // add the farthest point into the sampling points set.
+        sampling_indices_.push_back(maxDistanceIndex);
+        startIndex= maxDistanceIndex;
+        vertex_sample_indices_[startIndex] = sampling_indices_.size()-1;
+    }
+	if(pars_.use_landmark && pars_.landmark_src.size() > 0)
+    {
+       CalcLandmarkCoeff();
+	   vertex_landmark_indices_.resize(n_src_vertex_, -1);
+	   for(int i = 0; i < pars_.landmark_src.size(); i++)
+	   {
+			vertex_landmark_indices_[pars_.landmark_src[i]] = i; 
+	   }
+   	}
+}
+void NonrigidSpareRegistration::CalcLandmarkCoeff()
+{
+	size_t n_landmarks = pars_.landmark_src.size();
+	tar_landmarks_.resize(n_landmarks*3); 
+	RowMajorSparseMatrix landmark_coeff(n_landmarks*3, 12*num_sample_nodes);
+	std::vector<Triplet> coeffs;
+	for(int idx = 0; idx < n_landmarks; idx++)
+	{
+		int src_idx = pars_.landmark_src[idx];
+		for (int k = 0; k < 3; k++)
+		{
+			for (RowMajorSparseMatrix::InnerIterator it(align_coeff_PV0_, src_idx*3+k); it; ++it)
+			{
+				coeffs.push_back(Triplet(idx*3+k, it.col(), it.value()));
+			}
+			tar_landmarks_[idx*3+k] = tar_points_(k, pars_.landmark_tar[idx]) - nodes_P_[src_idx*3+k];
+		}
+	}
+	landmark_coeff.setFromTriplets(coeffs.begin(), coeffs.end());
+	landmark_mul_ = landmark_coeff.transpose() * landmark_coeff; 
+	landmark_right_ = landmark_coeff.transpose() * tar_landmarks_; 
+
+	std::vector<Triplet> coeffs_fine;
+	landmark_right_fine_.resize(n_src_vertex_*3);
+	landmark_mul_fine_.resize(n_src_vertex_*3, n_src_vertex_*3);
+	landmark_right_fine_.setZero();
+	for(int idx = 0; idx < n_landmarks; idx++)
+	{
+		int src_idx = pars_.landmark_src[idx];
+		for (int k = 0; k < 3; k++)
+		{
+			coeffs_fine.push_back(Triplet(src_idx*3+k, src_idx*3+k, 1.0));
+			landmark_right_fine_[src_idx*3+k] = tar_points_(k, pars_.landmark_tar[idx]);
+		}
+	}
+	landmark_mul_fine_.setFromTriplets(coeffs_fine.begin(), coeffs_fine.end());
+}
+
+void NonrigidSpareRegistration::InitWelschParam()
+{
+    // welsch parameters
+    weight_d_.resize(n_src_vertex_*3);
+    weight_d_.setOnes();
+
+    // Initialize correspondences
+    InitCorrespondence(correspondence_pairs_);
+
+    VectorX init_nus(correspondence_pairs_.size());
+#pragma omp parallel for
+    for(size_t i = 0; i < correspondence_pairs_.size(); i++)
+    {
+        Vector3 closet = correspondence_pairs_[i].position;
+        init_nus[i] = (src_mesh_->point(src_mesh_->vertex_handle(correspondence_pairs_[i].src_idx))
+                       - Vec3(closet[0], closet[1], closet[2])).norm();
+    }
+    igl::median(init_nus, pars_.Data_nu);
+
+    if(pars_.calc_gt_err&&n_src_vertex_ == n_tar_vertex_)
+    {
+        VectorX gt_err(n_src_vertex_);
+        for(int i = 0; i < n_src_vertex_; i++)
+        {
+            gt_err[i] = (src_mesh_->point(src_mesh_->vertex_handle(i)) - tar_mesh_->point(tar_mesh_->vertex_handle(i))).norm();
+        }
+        pars_.init_gt_mean_errs = std::sqrt(gt_err.squaredNorm()/n_src_vertex_);
+        pars_.init_gt_max_errs = gt_err.maxCoeff();
+    }
+}
+void NonrigidSpareRegistration::InitwithLandmark()
+{
+	// Scalar energy=0., landmark_err=0., reg_err=0., rot_err=0., arap_err=0.;
+	VectorX prevV = VectorX::Zero(n_src_vertex_ * 3);
+
+	// welsch_sweight
+ 	bool run_once = true;
+
+	Timer time;
+	Timer::EventID begin_time, run_time;
+
+
+	w_smo = optimize_w_smo;
+
+	// pars_.each_energys.push_back(0.0);
+	// pars_.each_gt_max_errs.push_back(pars_.init_gt_max_errs);
+	// pars_.each_gt_mean_errs.push_back(pars_.init_gt_mean_errs);
+	// pars_.each_iters.push_back(0);
+	// pars_.each_times.push_back(pars_.non_rigid_init_time);
+	// pars_.each_term_energy.push_back(Vector4(0, 0, 0, 0));
+
+	Scalar gt_err;
+
+	VectorX prev_X = X_;
+
+	begin_time = time.get_time();
+	
+	#ifdef DEBUG
+	double find_cp_time = 0.0;
+	double construct_mat_time = 0.0;
+	double solve_eq_time = 0.0;
+	double calc_energy_time = 0.0;
+	double robust_weight_time = 0.0;
+	double update_r_time = 0.0;
+	#endif
+
+	std::cout << "init A" << std::endl;
+	RowMajorSparseMatrix A_fixed_coeff = optimize_w_smo * reg_coeff_B_.transpose() * reg_cwise_weights_.asDiagonal() * reg_coeff_B_  + optimize_w_rot * rigid_coeff_L_ + optimize_w_landmark * landmark_mul_; // + optimize_w_arap * arap_coeff_mul_;
+
+	int out_iter = 0;
+	while (out_iter < pars_.max_outer_iters)
+	{
+		#ifdef DEBUG
+		Timer::EventID inner_start_time = time.get_time();
+		#endif
+
+		// int welsch_iter;
+		int total_inner_iters = 0;
+
+		// std::cout << "out_iter" << out_iter << std::endl;
+
+		mat_A0_ = A_fixed_coeff; 
+		vec_b_ = optimize_w_smo * reg_coeff_B_.transpose() * reg_cwise_weights_.asDiagonal() * reg_right_D_ + optimize_w_rot * rigid_coeff_J_ * nodes_R_ + optimize_w_landmark * landmark_right_; // + optimize_w_arap * arap_coeff_.transpose() * arap_right_;
+
+		#ifdef DEBUG
+		Timer::EventID end_construct_eq = time.get_time();
+		eps_time1 = time.elapsed_time(end_robust_weight, end_construct_eq);
+		construct_mat_time += eps_time1;	
+		#endif
+
+		if (run_once)
+		{
+			solver_.analyzePattern(mat_A0_);
+			run_once = false;
+		}
+		solver_.factorize(mat_A0_);
+		X_ = solver_.solve(vec_b_);		
+
+		run_time = time.get_time();
+		double eps_time = time.elapsed_time(begin_time, run_time);
+	
+		#ifdef DEBUG
+		eps_time1 = time.elapsed_time(end_construct_eq, run_time);
+		solve_eq_time += eps_time1;
+		#endif
+
+
+		#ifdef DEBUG
+		energy = CalcEnergy(align_err, reg_err, rot_err, arap_err, reg_cwise_weights_);
+		// std::cout << "energy = " << energy << std::endl;
+		#endif 
+		// std::cout << "X = " << X_ << X_.sum() << std::endl;
+		deformed_points_ = align_coeff_PV0_ * X_ + nodes_P_;
+		// std::cout << "diff = " << (deformed_points_ - prevV).norm() << std::endl;
+		
+		#ifdef DEBUG
+		Timer::EventID end_calc_energy = time.get_time();
+		eps_time1 = time.elapsed_time(run_time, end_calc_energy);
+		calc_energy_time += eps_time1;
+		#endif
+
+		
+
+		#ifdef DEBUG
+		Timer::EventID end_update_r = time.get_time();
+		eps_time1 = time.elapsed_time(end_calc_energy, end_update_r);
+		update_r_time += eps_time1;
+		#endif
+		
+		CalcLocalRotations(0);
+		CalcNodeRotations();
+		CalcDeformedNormals();
+
+		if (n_src_vertex_ == n_tar_vertex_)
+			gt_err = (deformed_points_ - Eigen::Map<VectorX>(tar_points_.data(), 3 * n_src_vertex_)).squaredNorm();
+
+		// save results
+		// pars_.each_gt_mean_errs.push_back(gt_err);
+		// pars_.each_gt_max_errs.push_back(0);
+		// pars_.each_energys.push_back(energy);
+		// pars_.each_iters.push_back(total_inner_iters);
+		// pars_.each_term_energy.push_back(Vector4(0.0, reg_err, rot_err, arap_err));
+
+		if((deformed_points_ - prevV).norm()/sqrtf(n_src_vertex_) < pars_.stop_coarse)
+		{
+			break;
+		}
+		prevV = deformed_points_;
+		out_iter++;
+
+		#ifdef DEBUG
+		Timer::EventID end_find_cp2 = time.get_time();
+		eps_time1 = time.elapsed_time(end_calc_energy, end_find_cp2);
+		find_cp_time += eps_time1;
+		#endif
+	}
+
+	#ifdef DEBUG
+	std::cout << "find cp time = " << find_cp_time
+	 << "\nconstruct_mat_timem = " << construct_mat_time
+	 << "\nsolve_eq_time = " << solve_eq_time
+	 << "\ncalc_energy_time = " << calc_energy_time
+	 << "\nrobust_weight_time = " << robust_weight_time
+	 << "\nupdate_r_time = " << update_r_time
+	 << "\nacculate_iter = " << out_iter << std::endl;
+	#endif
+}
+
+
+Scalar NonrigidSpareRegistration::DoNonRigid()
+{
+    // Data term parameters
+    Scalar nu1 = pars_.Data_initk * pars_.Data_nu;
+    if(pars_.landmark_src.size()>0)
+	{
+		optimize_w_landmark = 1.0/pars_.landmark_src.size();
+		optimize_w_smo = pars_.w_smo/reg_coeff_B_.rows(); 
+		optimize_w_rot = pars_.w_rot/num_sample_nodes;
+		optimize_w_arap = pars_.w_arap_coarse/arap_coeff_.rows();
+		std::cout << "Init Stage: optimize w_landmark = " << optimize_w_landmark << " | w_smo = " << optimize_w_smo << " | w_rot = " << optimize_w_rot  << " | w_arap = " << optimize_w_arap << std::endl;
+		InitwithLandmark();
+	}
+    optimize_w_align = 1.0;
+    //A parameter used to balance the influence of each energy component on the total energy.
+    optimize_w_smo = pars_.w_smo/reg_coeff_B_.rows() * sampling_indices_.size();
+    optimize_w_rot = pars_.w_rot/num_sample_nodes * sampling_indices_.size();
+    optimize_w_arap = pars_.w_arap_coarse/arap_coeff_.rows() * sampling_indices_.size();
+    std::cout << "Coarse Stage: optimize w_align = " << optimize_w_align << " | w_smo = " << optimize_w_smo << " | w_rot = " << optimize_w_rot << " | w_arap = " << optimize_w_arap << std::endl;
+    GraphCoarseReg(nu1);//nu1 is the parameter of welsch kernel
+
+
+    optimize_w_align = 1.0;
+    optimize_w_arap = pars_.w_arap_fine /arap_coeff_fine_.rows() * n_src_vertex_;
+    std::cout << "Fine Stage: optimize w_align = " << optimize_w_align << " | w_arap = " << optimize_w_arap << std::endl;
+    PointwiseFineReg(nu1);//nu1 is still the parameter of welsch kernel
+
+
+    // set the final deformed points to the source mesh
+    //and return the final gt error if needed,if not return -1
+    deformed_points_3X_ = Eigen::Map<Eigen::Matrix<double, 3, Eigen::Dynamic>>(deformed_points_.data(), 3, n_src_vertex_);
+    return 0;
+}
+
+
+void NonrigidSpareRegistration::CalcNodeRotations()
+{
+#pragma omp parallel for
+    for (int i = 0; i < num_sample_nodes; i++)
+    {
+        Matrix33 rot;
+        Eigen::JacobiSVD<Matrix33> svd(Eigen::Map<Matrix33>(X_.data()+12*i, 3,3), Eigen::ComputeFullU | Eigen::ComputeFullV);
+        if (svd.matrixU().determinant()*svd.matrixV().determinant() < 0.0) {
+            Vector3 S = Vector3::Ones(); S(2) = -1.0;
+            rot = svd.matrixU()*S.asDiagonal()*svd.matrixV().transpose();
+        }
+        else {
+            rot = svd.matrixU()*svd.matrixV().transpose();
+        }
+        nodes_R_.segment(9 * i, 9) = Eigen::Map<VectorX>(rot.data(), 9);
+    }
+}
+
+
+
+
+void NonrigidSpareRegistration::FullInARAPCoeff()
+{
+    arap_laplace_weights_.resize(n_src_vertex_);
+    Timer timer;
+
+    if(pars_.use_geodesic_dist)
+    {
+        for (int i = 0; i < n_src_vertex_; i++)
+        {
+            int nn = 0;
+            OpenMesh::VertexHandle vh = src_mesh_->vertex_handle(i);
+            for (auto vv = src_mesh_->vv_begin(vh); vv != src_mesh_->vv_end(vh); vv++)
+            {
+                nn++;
+            }
+            arap_laplace_weights_[i] = 1.0 / nn;
+        }
+
+        std::vector<Triplet> coeffs;
+        std::vector<Triplet> coeffs_fine;
+        for (int i = 0; i < src_mesh_->n_halfedges(); i++)
+        {
+            int src_idx = src_mesh_->from_vertex_handle(src_mesh_->halfedge_handle(i)).idx();
+            int tar_idx = src_mesh_->to_vertex_handle(src_mesh_->halfedge_handle(i)).idx();
+            Scalar w = sqrtf(arap_laplace_weights_[src_idx]);
+
+
+            for (int k = 0; k < 3; k++)
+            {
+                for (RowMajorSparseMatrix::InnerIterator it(align_coeff_PV0_, src_idx*3+k); it; ++it)
+                {
+                    coeffs.push_back(Triplet(i*3+k, it.col(), w*it.value()));
+                }
+                for (RowMajorSparseMatrix::InnerIterator it(align_coeff_PV0_, tar_idx*3+k); it; ++it)
+                {
+                    coeffs.push_back(Triplet(i*3+k, it.col(), -w*it.value()));
+                }
+            }
+
+
+            coeffs_fine.push_back(Triplet(i * 3, src_idx * 3, w));
+            coeffs_fine.push_back(Triplet(i * 3 + 1, src_idx * 3 + 1, w));
+            coeffs_fine.push_back(Triplet(i * 3 + 2, src_idx * 3 + 2, w));
+            coeffs_fine.push_back(Triplet(i * 3, tar_idx * 3, -w));
+            coeffs_fine.push_back(Triplet(i * 3 + 1, tar_idx * 3 + 1, -w));
+            coeffs_fine.push_back(Triplet(i * 3 + 2, tar_idx * 3 + 2, -w));
+        }
+
+
+        arap_coeff_.resize(src_mesh_->n_halfedges()*3, num_sample_nodes * 12);
+        arap_coeff_.setFromTriplets(coeffs.begin(), coeffs.end());
+        arap_coeff_mul_ = arap_coeff_.transpose() * arap_coeff_;
+
+
+        arap_coeff_fine_.resize(src_mesh_->n_halfedges() * 3, n_src_vertex_ * 3);
+        arap_coeff_fine_.setFromTriplets(coeffs_fine.begin(), coeffs_fine.end());
+        arap_coeff_mul_fine_ = arap_coeff_fine_.transpose() * arap_coeff_fine_;
+    }
+    else
+    {
+        int nn = src_knn_indices_.rows();
+        for (int i = 0; i < n_src_vertex_; i++)
+        {
+            arap_laplace_weights_[i] = 1.0 / nn;
+        }
+
+        std::vector<Triplet> coeffs;
+        std::vector<Triplet> coeffs_fine;
+        for(int src_idx = 0; src_idx < n_src_vertex_; src_idx++)
+        {
+            for(int j = 0; j < nn; j++)
+            {
+                int i = src_idx*nn+j;
+                int tar_idx = src_knn_indices_(j, src_idx);
+                Scalar w = sqrtf(arap_laplace_weights_[src_idx]);
+
+
+                for (int k = 0; k < 3; k++)
+                {
+                    for (RowMajorSparseMatrix::InnerIterator it(align_coeff_PV0_, src_idx*3+k); it; ++it)
+                    {
+                        coeffs.push_back(Triplet(i*3+k, it.col(), w*it.value()));
+                    }
+                    for (RowMajorSparseMatrix::InnerIterator it(align_coeff_PV0_, tar_idx*3+k); it; ++it)
+                    {
+                        coeffs.push_back(Triplet(i*3+k, it.col(), -w*it.value()));
+                    }
+                }
+
+
+                coeffs_fine.push_back(Triplet(i * 3, src_idx * 3, w));
+                coeffs_fine.push_back(Triplet(i * 3 + 1, src_idx * 3 + 1, w));
+                coeffs_fine.push_back(Triplet(i * 3 + 2, src_idx * 3 + 2, w));
+                coeffs_fine.push_back(Triplet(i * 3, tar_idx * 3, -w));
+                coeffs_fine.push_back(Triplet(i * 3 + 1, tar_idx * 3 + 1, -w));
+                coeffs_fine.push_back(Triplet(i * 3 + 2, tar_idx * 3 + 2, -w));
+            }
+        }
+
+        arap_coeff_.resize(n_src_vertex_*nn*3, num_sample_nodes * 12);
+        arap_coeff_.setFromTriplets(coeffs.begin(), coeffs.end());
+        arap_coeff_mul_ = arap_coeff_.transpose() * arap_coeff_;
+
+
+        arap_coeff_fine_.resize(n_src_vertex_*nn * 3, n_src_vertex_ * 3);
+        arap_coeff_fine_.setFromTriplets(coeffs_fine.begin(), coeffs_fine.end());
+        arap_coeff_mul_fine_ = arap_coeff_fine_.transpose() * arap_coeff_fine_;
+    }
+}
+
+
+void NonrigidSpareRegistration::CalcARAPRight()
+{
+    if(pars_.use_geodesic_dist)
+    {
+#pragma omp parallel for
+        for (int i = 0; i < src_mesh_->n_halfedges(); i++)
+        {
+            int src_idx = src_mesh_->from_vertex_handle(src_mesh_->halfedge_handle(i)).idx();
+            int tar_idx = src_mesh_->to_vertex_handle(src_mesh_->halfedge_handle(i)).idx();
+
+            Vector3 vij = local_rotations_.block(0, 3 * src_idx,3, 3) * (src_points_.col(src_idx) - src_points_.col(tar_idx));
+
+            Scalar w = sqrtf(arap_laplace_weights_[src_idx]);
+
+
+            arap_right_[i * 3] = w*(vij[0] - nodes_P_[src_idx * 3] + nodes_P_[tar_idx * 3]);
+            arap_right_[i * 3 + 1] = w*(vij[1] - nodes_P_[src_idx * 3 + 1] + nodes_P_[tar_idx * 3 + 1]);
+            arap_right_[i * 3 + 2] = w*(vij[2] - nodes_P_[src_idx * 3 + 2] + nodes_P_[tar_idx * 3 + 2]);
+        }
+    }
+    else
+    {
+        int nn = src_knn_indices_.rows();
+#pragma omp parallel for
+        for (int src_idx = 0; src_idx < n_src_vertex_; src_idx++)
+        {
+            for (int j = 0; j < nn; j++)
+            {
+                int i = src_idx*nn + j;
+                int tar_idx = src_knn_indices_(j, src_idx);
+
+                Vector3 vij = local_rotations_.block(0, 3 * src_idx,3, 3) * (src_points_.col(src_idx) - src_points_.col(tar_idx));
+
+                Scalar w = sqrtf(arap_laplace_weights_[src_idx]);
+
+
+                arap_right_[i * 3] = w*(vij[0] - nodes_P_[src_idx * 3] + nodes_P_[tar_idx * 3]);
+                arap_right_[i * 3 + 1] = w*(vij[1] - nodes_P_[src_idx * 3 + 1] + nodes_P_[tar_idx * 3 + 1]);
+                arap_right_[i * 3 + 2] = w*(vij[2] - nodes_P_[src_idx * 3 + 2] + nodes_P_[tar_idx * 3 + 2]);
+            }
+        }
+    }
+}
+
+void NonrigidSpareRegistration::CalcARAPRightFine()
+{
+    if(pars_.use_geodesic_dist)
+    {
+#pragma omp parallel for
+        for (int i = 0; i < src_mesh_->n_halfedges(); i++)
+        {
+            int src_idx = src_mesh_->from_vertex_handle(src_mesh_->halfedge_handle(i)).idx();
+            int tar_idx = src_mesh_->to_vertex_handle(src_mesh_->halfedge_handle(i)).idx();
+
+            Vector3 vij = local_rotations_.block(0, 3 * src_idx, 3, 3) * (src_points_.col(src_idx) - src_points_.col(tar_idx));
+
+            Scalar w = sqrtf(arap_laplace_weights_[src_idx]);
+
+            arap_right_fine_[i * 3] = w*(vij[0]);
+            arap_right_fine_[i * 3 + 1] = w*(vij[1]);
+            arap_right_fine_[i * 3 + 2] = w*(vij[2]);
+        }
+    }
+    else
+    {
+        int nn = src_knn_indices_.rows();
+#pragma omp parallel for
+        for(int src_idx = 0; src_idx < n_src_vertex_; src_idx++)
+        {
+            for(int j = 0; j < nn; j++)
+            {
+                int tar_idx = src_knn_indices_(j, src_idx);
+                int i = src_idx*nn+j;
+
+                Vector3 vij = local_rotations_.block(0, 3 * src_idx, 3, 3) * (src_points_.col(src_idx) - src_points_.col(tar_idx));
+
+                Scalar w = sqrtf(arap_laplace_weights_[src_idx]);
+
+                arap_right_fine_[i * 3] = w*(vij[0]);
+                arap_right_fine_[i * 3 + 1] = w*(vij[1]);
+                arap_right_fine_[i * 3 + 2] = w*(vij[2]);
+            }
+        }
+    }
+}
+
+
+void NonrigidSpareRegistration::InitRotations()
+{
+    local_rotations_.resize(3, n_src_vertex_ * 3);
+    local_rotations_.setZero();
+#pragma omp parallel for
+    for (int i = 0; i < n_src_vertex_; i++)
+    {
+        local_rotations_(0, i * 3) = 1;
+        local_rotations_(1, i * 3 + 1) = 1;
+        local_rotations_(2, i * 3 + 2) = 1;
+    }
+}
+
+
+void NonrigidSpareRegistration::CalcLocalRotations(bool isCoarseAlign)
+{
+
+#pragma omp parallel for
+    for (int i = 0; i < n_src_vertex_; i++)
+    {
+        Matrix33 sum;
+        sum.setZero();
+
+        int nn = 0;
+        if(pars_.use_geodesic_dist)
+        {
+            OpenMesh::VertexHandle vh = src_mesh_->vertex_handle(i);
+            for (auto vv = src_mesh_->vv_begin(vh); vv != src_mesh_->vv_end(vh); vv++)
+            {
+                int neighbor_idx = vv->idx();
+                Vector3 dv = src_points_.col(i) - src_points_.col(neighbor_idx);
+                Vector3 new_dv = deformed_points_.segment(3 * i, 3) - deformed_points_.segment(3 * neighbor_idx, 3);
+                sum += dv * new_dv.transpose();
+                nn++;
+            }
+        }
+        else
+        {
+            nn = src_knn_indices_.rows();
+            for(int j = 0; j < nn; j++)
+            {
+                int neighbor_idx = src_knn_indices_(j, i);
+                Vector3 dv = src_points_.col(i) - src_points_.col(neighbor_idx);
+                Vector3 new_dv = deformed_points_.segment(3 * i, 3) - deformed_points_.segment(3 * neighbor_idx, 3);
+                sum += dv * new_dv.transpose();
+            }
+        }
+
+
+        sum*= 1.0*optimize_w_arap/nn;
+
+        if(!isCoarseAlign)
+        {
+            int tar_idx = correspondence_pairs_[i].tar_idx;
+            Vector3 d = deformed_points_.segment(3 * i, 3) - tar_points_.col(tar_idx);
+            Scalar c = (target_normals_.col(tar_idx) + deformed_normals_.col(i)).dot(d);
+            Scalar d_norm2 = d[0]*d[0] + d[1]*d[1] + d[2]*d[2];
+            Vector3 h = deformed_normals_.col(i) - c*d/d_norm2;
+
+            Scalar w = optimize_w_align*d_norm2*weight_d_[i];
+            sum += w * src_normals_.col(i) * h.transpose();
+        }
+        else if(vertex_sample_indices_[i] >= 0)
+        {
+            int tar_idx = correspondence_pairs_[vertex_sample_indices_[i]].tar_idx;
+            Vector3 d = deformed_points_.segment(3 * i, 3) - tar_points_.col(tar_idx);
+            Scalar c = (target_normals_.col(tar_idx) + deformed_normals_.col(i)).dot(d);
+            Scalar d_norm2 = d[0]*d[0] + d[1]*d[1] + d[2]*d[2];
+            Vector3 h = deformed_normals_.col(i) - c*d/d_norm2;
+
+            Scalar w = optimize_w_align*d_norm2*weight_d_[vertex_sample_indices_[i]];
+            sum += w * src_normals_.col(i) * h.transpose();
+        }
+
+
+        Eigen::JacobiSVD<Matrix33> svd(sum, Eigen::ComputeFullU | Eigen::ComputeFullV);
+
+        if (svd.matrixU().determinant()*svd.matrixV().determinant() < 0.0) {
+            Vector3 S = Vector3::Ones(); S(2) = -1.0;
+            sum = svd.matrixV()*S.asDiagonal()*svd.matrixU().transpose();
+        }
+        else {
+            sum = svd.matrixV()*svd.matrixU().transpose();
+        }
+        for (int s = 0; s < 3; s++)
+        {
+            for (int t = 0; t < 3; t++)
+            {
+                local_rotations_(s, 3 * i + t) = sum(s, t);
+            }
+        }
+    }
+}
+
+void NonrigidSpareRegistration::CalcDeformedNormals()
+{
+#pragma omp parallel for
+    for (int i = 0; i < n_src_vertex_; i++)
+    {
+        deformed_normals_.col(i) = local_rotations_.block(0, i * 3, 3, 3) * src_normals_.col(i);
+    }
+}
+
+void NonrigidSpareRegistration::CalcNormalsSum()
+{
+#pragma omp parallel for
+    for(int i = 0; i < correspondence_pairs_.size(); i++)
+    {
+        int sidx = correspondence_pairs_[i].src_idx;
+        int tidx = correspondence_pairs_[i].tar_idx;
+        int j = 0;
+        for (RowMajorSparseMatrix::InnerIterator it(normals_sum_, i); it; ++it)
+        {
+            it.valueRef() = deformed_normals_(j, sidx) + target_normals_(j, tidx);
+            j++;
+        }
+    }
+}
+
+
+void NonrigidSpareRegistration::InitNormalsSum()
+{
+    std::vector<Triplet> coeffs(3 * correspondence_pairs_.size());
+    normals_sum_.resize(correspondence_pairs_.size(), 3*n_src_vertex_);
+    normals_sum_.setZero();
+
+#pragma omp parallel for
+    for(int i = 0; i < correspondence_pairs_.size(); i++)
+    {
+        int sidx = correspondence_pairs_[i].src_idx;
+        int tidx = correspondence_pairs_[i].tar_idx;
+        coeffs[i * 3] = Triplet(i, 3 * sidx, deformed_normals_(0, sidx) + target_normals_(0, tidx));
+        coeffs[i * 3 + 1] = Triplet(i, 3 * sidx + 1, deformed_normals_(1, sidx) + target_normals_(1, tidx));
+        coeffs[i * 3 + 2] = Triplet(i, 3 * sidx + 2, deformed_normals_(2, sidx) + target_normals_(2, tidx));
+    }
+    normals_sum_.setFromTriplets(coeffs.begin(), coeffs.end());
+}
+
+void NonrigidSpareRegistration::PointwiseFineReg(Scalar nu1)
+{
+
+    Scalar energy=-1., align_err=-1., arap_err=-1.;
+
+    VectorX prevV = VectorX::Zero(n_src_vertex_ * 3);
+
+    bool run_once = true;
+
+
+    // Smooth term parameters
+    w_align = optimize_w_align;
+    w_smo = optimize_w_smo;
+    w_align = optimize_w_align *(2.0*nu1*nu1);
+
+
+    Scalar gt_err = -1;
+
+    double construct_mat_time = 0.0;
+    double solve_eq_time = 0.0;
+
+
+    int out_iter = 0;
+    while (out_iter < pars_.max_outer_iters)
+    {
+        // Find clost points
+        FindClosestPoints(target_tree_,deformed_points_,correspondence_pairs_);
+        // according correspondence_pairs to update corres_U0_;
+        corres_U0_.setZero();
+        weight_d_.resize(n_src_vertex_);
+
+
+        for (size_t i = 0; i < correspondence_pairs_.size(); i++)
+        {
+            corres_U0_.segment(i * 3, 3) = correspondence_pairs_[i].position;
+            weight_d_[i] = correspondence_pairs_[i].min_dist2;
+            int tar_idx = correspondence_pairs_[i].tar_idx;
+            if(deformed_normals_.col(i).dot(target_normals_.col(tar_idx))<0)
+                weight_d_[i] = -1;
+        }
+
+        // update weight
+
+        welsch_weight(weight_d_, nu1);
+
+        if (run_once == true && pars_.use_landmark == true)
+        {
+            weight_d_.setOnes();
+        }
+
+        // construct matrix A0 and pre-decompose
+        if(out_iter==0)
+            InitNormalsSum();
+        else
+            CalcNormalsSum();
+
+        RowMajorSparseMatrix normals_sum_mul = normals_sum_.transpose() * weight_d_.asDiagonal()* normals_sum_;
+        mat_A0_ = optimize_w_align * normals_sum_mul
+                  + optimize_w_arap * arap_coeff_mul_fine_;
+
+        CalcARAPRightFine();
+
+        vec_b_ = optimize_w_align * normals_sum_mul * corres_U0_
+                 + optimize_w_arap * arap_coeff_fine_.transpose() * arap_right_fine_;
+
+
+        if (run_once)
+        {
+            solver_.analyzePattern(mat_A0_);
+            run_once = false;
+        }
+        solver_.factorize(mat_A0_);
+        deformed_points_ = solver_.solve(vec_b_);
+        CalcLocalRotations(false);
+        CalcDeformedNormals();
+        if((deformed_points_ - prevV).norm()/sqrtf(n_src_vertex_) < pars_.stop_fine)
+        {
+            break;
+        }
+        prevV = deformed_points_;
+        out_iter++;
+    }
+}
+
+void NonrigidSpareRegistration::GraphCoarseReg(Scalar nu1)
+{
+    Scalar energy=0., align_err=0., reg_err=0., rot_err=0., arap_err=0.;
+    VectorX prevV = VectorX::Zero(n_src_vertex_ * 3);
+
+    // welsch_sweight
+    bool run_once = true;
+
+
+    w_align = optimize_w_align;
+    w_smo = optimize_w_smo; 
+    w_align = optimize_w_align *(2.0*nu1*nu1);
+
+    Scalar gt_err;
+
+    VectorX prev_X = X_;
+
+    RowMajorSparseMatrix A_fixed_coeff = optimize_w_smo * reg_coeff_B_.transpose() * reg_cwise_weights_.asDiagonal() * reg_coeff_B_  + optimize_w_rot * rigid_coeff_L_ + optimize_w_arap * arap_coeff_mul_;
+    int out_iter = 0;
+    while (out_iter < pars_.max_outer_iters)
+    {
+
+
+        correspondence_pairs_.clear();
+        FindClosestPoints(target_tree_,correspondence_pairs_, deformed_points_, sampling_indices_);
+        corres_U0_.setZero();
+        weight_d_.resize(correspondence_pairs_.size());
+        weight_d_.setConstant(-1);
+
+#pragma omp parallel for
+        for (size_t i = 0; i < correspondence_pairs_.size(); i++)
+        {
+            corres_U0_.segment(correspondence_pairs_[i].src_idx * 3, 3) = correspondence_pairs_[i].position;
+            weight_d_[i] = correspondence_pairs_[i].min_dist2;
+            if(deformed_normals_.col(correspondence_pairs_[i].src_idx).dot(target_normals_.col(correspondence_pairs_[i].tar_idx))<0)
+                weight_d_[i] = -1;
+        }
+
+        // update weight
+
+        welsch_weight(weight_d_, nu1);
+
+        if (run_once == true && pars_.use_landmark == true)
+        {
+            weight_d_.setOnes();
+        }
+
+        if(out_iter==0)
+            InitNormalsSum();
+        else
+            CalcNormalsSum();
+
+        diff_UP_ = (corres_U0_ - nodes_P_);
+
+        RowMajorSparseMatrix weight_NPV = normals_sum_ * align_coeff_PV0_;
+
+        mat_A0_ = optimize_w_align * weight_NPV.transpose() * weight_d_.asDiagonal() *  weight_NPV + A_fixed_coeff;
+
+        CalcARAPRight();
+        vec_b_ = optimize_w_align * weight_NPV.transpose() * weight_d_.asDiagonal() * normals_sum_ * diff_UP_ + optimize_w_smo * reg_coeff_B_.transpose() * reg_cwise_weights_.asDiagonal() * reg_right_D_ + optimize_w_rot * rigid_coeff_J_ * nodes_R_ + optimize_w_arap * arap_coeff_.transpose() * arap_right_;
+
+
+        if (run_once)
+        {
+            solver_.analyzePattern(mat_A0_);
+            run_once = false;
+        }
+        solver_.factorize(mat_A0_);
+        X_ = solver_.solve(vec_b_);
+        deformed_points_ = align_coeff_PV0_ * X_ + nodes_P_;
+        CalcLocalRotations(true);
+        CalcNodeRotations();
+        CalcDeformedNormals();
+
+        if (n_src_vertex_ == n_tar_vertex_)
+            gt_err = (deformed_points_ - Eigen::Map<VectorX>(tar_points_.data(), 3 * n_src_vertex_)).squaredNorm();
+
+
+        if((deformed_points_ - prevV).norm()/sqrtf(n_src_vertex_) < pars_.stop_coarse)
+        {
+            break;
+        }
+        prevV = deformed_points_;
+        out_iter++;
+    }
+
+}
+
+void NonrigidSpareRegistration::Paras_init(
+    int iters,
+    double stopcoarse,
+    double stopfine,
+    bool uselandmark,
+    std::vector<int> src,
+    std::vector<int> tar)
+{
+    paras.max_outer_iters = iters;
+    paras.stop_coarse = stopcoarse;
+    paras.stop_fine = stopfine;
+
+    if (uselandmark && !src.empty() && src.size() == tar.size()) {
+        paras.use_landmark = true;
+        paras.landmark_src = src;
+        paras.landmark_tar = tar;
+    } else {
+        paras.use_landmark = false;
+        paras.landmark_src.clear();
+        paras.landmark_tar.clear();
+    }
+}
+
+
+void NonrigidSpareRegistration::Register()
+{
+	if(src_mesh.n_vertices()==0 || tar_mesh.n_vertices()==0)
+        exit(0);
+
+    if(src_mesh.n_vertices() != tar_mesh.n_vertices())
+        paras.calc_gt_err = false;
+
+    if(src_mesh.n_faces()==0)
+        paras.use_geodesic_dist = false;
+
+    if(paras.use_landmark)
+        read_landmark(landmark_file.c_str(), paras.landmark_src, paras.landmark_tar);
+    if(normalize)// The default value of `normalize` is `true`.
+        scale = mesh_scaling(src_mesh, tar_mesh);
+    pars_ = paras;
+    Timer time;
+    std::cout << "registration to initial... (mesh scale: " << scale << ")" << std::endl;
+    Timer::EventID time1 = time.get_time();
+    Init_data();
+    Initialize();
+    Timer::EventID time2 = time.get_time();
+    std::cout << "non-rigid registration... (graph node number: " << pars_.num_sample_nodes << ")" << std::endl;
+    DoNonRigid();
+    Timer::EventID time3 = time.get_time();
+    std::cout << "Registration done!\ninitialize time : "
+              << time.elapsed_time(time1, time2) << " s \tnon-rigid reg running time = " << time.elapsed_time(time2, time3) << " s" << std::endl;
+    return;
+}
+
+void NonrigidSpareRegistration::Reg(const std::string& file_target,
+                       const std::string& file_source,
+                       const std::string& out_path )
+{
+    Read_data(file_target, file_source);
+    Register();
+    Output_data(out_path, "spare");
+}

cpp/core/nonrigid_spare_registration.h

@@ -0,0 +1,142 @@
+#ifndef NONRIGIDREG_SPARE_H
+#define NONRIGIDREG_SPARE_H
+
+#include "nodeSampler.h"
+#include "registration.h"
+
+
+
+
+#ifdef USE_PARDISO
+#include <Eigen/PardisoSupport>
+#endif
+
+
+class NonrigidSpareRegistration : public Registration {
+public:
+    NonrigidSpareRegistration();
+    ~NonrigidSpareRegistration();
+
+    // Adjusted parameters
+    spare::Parameters pars_;
+
+    // Non-rigid energy function parameters
+    VectorX weight_d_;                        // robust weight for alignment α_i
+    RowMajorSparseMatrix mat_A0_;             // symmetric coefficient matrix for linear equations
+    VectorX vec_b_;                           // right-hand side for linear equations
+
+    // Welsch parameters
+    Scalar nu;
+
+#ifdef USE_PARDISO
+    Eigen::PardisoLDLT<RowMajorSparseMatrix, Eigen::Lower> solver_;
+#else
+    Eigen::SimplicialLDLT<RowMajorSparseMatrix, Eigen::Lower> solver_;
+#endif
+
+    // Sampling parameters
+    int num_sample_nodes;                     // (r,) number of sample nodes
+    int num_graph_edges;
+    int num_edges;
+
+    // Node storage structure
+    svr::nodeSampler src_sample_nodes;
+
+    // Variables
+    VectorX X_;                               // (12r,) transformations of sample nodes
+
+    // Alignment term
+    VectorX nodes_P_;                         // (3n,) all sample nodes' coordinates
+    RowMajorSparseMatrix align_coeff_PV0_;    // (3n, 12r) coefficient matrix F
+
+    // Smooth term
+    RowMajorSparseMatrix reg_coeff_B_;        // (6|E_G|, 12r) smoothness between nodes
+    VectorX reg_right_D_;                     // (6|E_G|,) coordinate differences between xi and xj
+    VectorX reg_cwise_weights_;               // (6|E_G|,) smooth weights
+
+    // landmark term 
+    RowMajorSparseMatrix    landmark_mul_;             // (12r, 12r) 
+    VectorX                 tar_landmarks_;             // (3, l)    
+    VectorX                 landmark_right_;            // (12r,)
+    RowMajorSparseMatrix    landmark_mul_fine_;         // (3n, 3n) 
+    VectorX                 landmark_right_fine_;       // (3n,)
+    // Rotation matrix term
+    VectorX nodes_R_;                         // (9r,) proj(A)
+    RowMajorSparseMatrix rigid_coeff_L_;      // (12r, 12r) H
+    RowMajorSparseMatrix rigid_coeff_J_;      // (9r, 12r) Y
+    VectorX diff_UP_;                         // auxiliary matrix
+
+    // ARAP term (coarse)
+    VectorX arap_laplace_weights_;            // (6E,)
+    Matrix3X local_rotations_;                // (3n, 3) R
+    RowMajorSparseMatrix arap_coeff_;         // (6E, 12r) B
+    RowMajorSparseMatrix arap_coeff_mul_;     // (12r, 12r) BᵀB
+    VectorX arap_right_;                      // (6E,) L
+
+    // ARAP term (fine)
+    RowMajorSparseMatrix arap_coeff_fine_;    // (6E, 3n) B
+    RowMajorSparseMatrix arap_coeff_mul_fine_;// (3n, 3n) BᵀB
+    VectorX arap_right_fine_;                 // (6E,) Y
+
+    // Point clouds
+    RowMajorSparseMatrix normals_sum_;        // (n, 3n) N for alignment term
+
+    // Sampling points & vertices relation
+    std::vector<size_t> sampling_indices_;
+    std::vector<int> vertex_sample_indices_;
+    std::vector<int>        vertex_landmark_indices_;
+    // KNN-neighbor indices for source points (if no faces)
+    Eigen::MatrixXi src_knn_indices_;
+
+    int align_sampling_num_ = 3000;
+
+    // Weights of terms during optimization
+    Scalar w_align;
+    Scalar w_smo;
+    Scalar optimize_w_align;
+    Scalar optimize_w_smo;
+    Scalar optimize_w_rot;
+    Scalar optimize_w_arap;
+    Scalar optimize_w_landmark;
+    // Initialization & main steps
+    void Initialize();
+    void InitFromInput(Mesh& src_mesh, Mesh& tar_mesh, spare::Parameters& paras);
+    virtual Scalar DoNonRigid();
+
+    // Welsch weighting
+    //void welsch_weight(VectorX& r, Scalar p);
+    void InitWelschParam();
+    void InitwithLandmark();
+
+    // Rotation calculations
+    void CalcNodeRotations();
+    void InitRotations();
+    void CalcLocalRotations(bool isCoarseAlign);
+
+    // ARAP calculations
+    void FullInARAPCoeff();
+    void CalcARAPRight();
+    void CalcARAPRightFine();
+    // Normal calculations
+    void CalcDeformedNormals();
+    void InitNormalsSum();
+    void CalcNormalsSum();
+    void CalcLandmarkCoeff();
+    // Optimization steps
+    void PointwiseFineReg(Scalar nu1);
+    void GraphCoarseReg(Scalar nu1);
+void Paras_init(int iters = 30 ,double stopcoarse=1e-3,double stopfine=1e-4,bool uselandmark = false,std::vector<int> src=std::vector<int>(),std::vector<int> tar=std::vector<int>());
+    
+    void Register()override;
+    void Reg(const std::string& file_target,
+                       const std::string& file_source,
+                       const std::string& out_path)override;
+    std::string landmark_file;
+    spare::Parameters paras;
+    bool normalize = true; 
+
+};
+
+
+
+#endif

cpp/core/registration.cpp

@@ -0,0 +1,279 @@
+#include "registration.h"
+#include "io.h"
+#include "tools.h"
+#include <median.h>
+
+void Registration::FindClosestPoints(VPairs & corres)
+{
+    corres.resize(n_src_vertex_);
+
+    #pragma omp parallel for
+    for (int i = 0; i < n_src_vertex_; i++)
+    {
+        Scalar mini_dist2;
+        int idx = target_tree_->closest(src_mesh_->point(src_mesh_->vertex_handle(i)).data(), mini_dist2);
+        Closest c;
+        c.src_idx = i;
+        c.position = tar_points_.col(idx);
+        c.normal = Vec2Eigen(tar_mesh_->normal(tar_mesh_->vertex_handle(idx)));
+        c.min_dist2 = mini_dist2;
+        c.tar_idx = idx;
+        corres[i] = c;
+    }
+}
+void Registration::FindClosestPoints(KDtree* target_tree_tem,VectorX & deformed_v,VPairs & corres)
+{
+	corres.resize(n_src_vertex_);
+#pragma omp parallel for
+	for (int i = 0; i < n_src_vertex_; i++)
+	{
+		Scalar mini_dist2;
+		int idx = target_tree_tem->closest(deformed_v.data() + 3*i, mini_dist2);
+		Closest c;
+		c.src_idx = i;
+		c.position = tar_points_.col(idx);
+		c.min_dist2 = mini_dist2;
+		c.tar_idx = idx;
+		corres[i] = c;
+    }
+
+}
+void Registration::FindClosestPoints(KDtree* target_tree_tem,VPairs & corres, VectorX & deformed_v, std::vector<size_t>& sample_indices)
+{
+    corres.resize(sample_indices.size());
+#pragma omp parallel for
+    for(int i = 0; i < sample_indices.size(); i++)
+    {
+        int sidx = sample_indices[i];
+        Scalar mini_dist2;
+        int tidx = target_tree_tem->closest(deformed_v.data() + 3*sidx, mini_dist2);
+            Closest c;
+        c.src_idx = sidx;
+        c.position = tar_points_.col(tidx);
+        c.min_dist2 = mini_dist2;
+        c.tar_idx = tidx;
+        corres[i] = c;
+    }
+}
+double Registration::FindKnearestMed(const KDtree& kdtree,
+                           const Matrix3X& X, int nk)
+    {
+        Eigen::VectorXd X_nearest(X.cols());
+#pragma omp parallel for
+        for(int i = 0; i<X.cols(); i++)
+        {
+            int* id = new int[nk];
+            double *dist = new double[nk];
+            kdtree.query(X.col(i).data(), nk, id, dist);
+            Eigen::VectorXd k_dist = Eigen::Map<Eigen::VectorXd>(dist, nk);
+            igl::median(k_dist.tail(nk-1), X_nearest[i]);
+            delete[]id;
+            delete[]dist;
+        }
+        double med;
+        igl::median(X_nearest, med);
+        return sqrt(med);
+    }
+void Registration::LandMarkCorres(VPairs & corres)
+{
+    corres.clear();
+    if (pars_.landmark_src.size() != pars_.landmark_tar.size())
+    {
+        std::cout << "Error: landmark data wrong!!" << std::endl;
+    }
+    n_landmark_nodes_ = pars_.landmark_tar.size();
+    for (int i = 0; i < n_landmark_nodes_; i++)
+    {
+        Closest c;
+        c.src_idx = pars_.landmark_src[i];
+        OpenMesh::VertexHandle vh = tar_mesh_->vertex_handle(pars_.landmark_tar[i]);
+
+        if (c.src_idx > n_src_vertex_ || c.src_idx < 0)
+            std::cout << "Error: source index in Landmark is out of range!" << std::endl;
+        if (vh.idx() < 0)
+            std::cout << "Error: target index in Landmark is out of range!" << std::endl;
+
+        c.position = Vec2Eigen(tar_mesh_->point(vh));
+        c.normal = Vec2Eigen(tar_mesh_->normal(vh));
+        corres.push_back(c);
+	}
+    std::cout << " use landmark and landmark is ... " << pars_.landmark_src.size() << std::endl;
+}
+
+
+bool Registration::read_landmark(const char* filename, std::vector<int>& landmark_src, std::vector<int>& landmark_tar)
+{
+    std::ifstream in(filename);
+    std::cout << "filename = " << filename << std::endl;
+    if (!in)
+    {
+        std::cout << "Can't open the landmark file!!" << std::endl;
+        return false;
+    }
+    int x, y;
+    landmark_src.clear();
+    landmark_tar.clear();
+    while (!in.eof())
+    {
+        if (in >> x >> y) {
+            landmark_src.push_back(x);
+            landmark_tar.push_back(y);
+        }
+    }
+    in.close();
+    std::cout << "landmark_src = " << landmark_src.size() << " tar = " << landmark_tar.size() << std::endl;
+    return true;
+}
+
+
+void Registration::InitCorrespondence(VPairs & corres)
+{
+    if(pars_.use_landmark)
+    {
+        corres.clear();
+        for(size_t i = 0; i < pars_.landmark_src.size(); i++)
+        {
+            Closest c;
+            c.src_idx = pars_.landmark_src[i];
+            c.tar_idx = pars_.landmark_tar[i];
+            c.position = tar_points_.col(c.tar_idx);
+            c.normal = Vec2Eigen(tar_mesh_->normal(tar_mesh_->vertex_handle(c.tar_idx)));
+            corres.push_back(c);
+        }
+    }
+    else
+    {
+        FindClosestPoints(corres);
+    }
+}
+void Registration::Init_data()
+{
+    src_mesh_ = new Mesh;
+    tar_mesh_ = new Mesh;
+    src_mesh_ = &src_mesh;
+    tar_mesh_ = &tar_mesh;
+    deformed_mesh =src_mesh;
+    deformed_mesh_=&deformed_mesh;
+    n_src_vertex_ = src_mesh_->n_vertices();
+    n_tar_vertex_ = tar_mesh_->n_vertices();
+    tar_points_.resize(3, n_tar_vertex_);
+    target_normals_.resize(3, n_tar_vertex_);
+    for (int i = 0; i < n_tar_vertex_; i++)
+    {
+        auto vh = tar_mesh_->vertex_handle(i);
+
+    // 顶点坐标
+        tar_points_(0, i) = tar_mesh_->point(vh)[0];
+        tar_points_(1, i) = tar_mesh_->point(vh)[1];
+        tar_points_(2, i) = tar_mesh_->point(vh)[2];  
+
+    // 顶点法向量
+        target_normals_(0, i) = tar_mesh_->normal(vh)[0];
+        target_normals_(1, i) = tar_mesh_->normal(vh)[1];
+        target_normals_(2, i) = tar_mesh_->normal(vh)[2];
+    }
+
+        // construct kd Tree
+	target_tree_ = new KDtree(tar_points_);
+	src_points_.resize(3, n_src_vertex_);
+	src_normals_.resize(3, n_src_vertex_);
+	corres_U0_.resize(3* n_src_vertex_);
+	#pragma omp parallel for
+	for (int i = 0; i < n_src_vertex_; i++)
+	{
+		Vec3 p = src_mesh_->point(src_mesh_->vertex_handle(i));
+		src_points_(0, i) = p[0];
+		src_points_(1, i) = p[1];
+		src_points_(2, i) = p[2];
+		Vec3 n = src_mesh_->normal(src_mesh_->vertex_handle(i));
+		src_normals_(0, i) = n[0];
+		src_normals_(1, i) = n[1];
+		src_normals_(2, i) = n[2];
+	}
+	deformed_normals_ = src_normals_;
+    deformed_points_ = Eigen::Map<VectorX>(src_points_.data(), 3 * n_src_vertex_);
+}
+
+void Registration::Read_data(const std::string& file_target,
+                       const std::string& file_source)
+{
+    read_by_openmesh(file_source, src_mesh);
+    read_by_openmesh(file_target, tar_mesh);
+    return;
+}
+void Registration::Read_data(const Matrix3X &target_p,const Matrix3X &source_p,const Matrix3X &target_n,const Matrix3X &source_n) 
+{
+    SetMeshPoints(&src_mesh, source_p, source_n);
+    SetMeshPoints(&tar_mesh, target_p, target_n);
+    return;
+}
+
+
+void Registration::Read_data(const Mesh& tar,const Mesh& src)
+{
+    src_mesh=src;
+    tar_mesh=tar;
+    return;
+}
+Scalar Registration::SetMeshPoints(Mesh* mesh, const Matrix3X &point, const Matrix3X &point_n)
+{
+    int n_vertices = point.cols();
+    mesh->request_vertex_normals(); // 确保法向量可以设置
+
+    bool use_input_normals = (point_n.cols() == n_vertices);
+    if (!use_input_normals) {
+        std::cout << "[Info] 法向量数据不正确或为空，正在自动计算法向量..." << std::endl;
+    }
+
+    // 判断 mesh 是否为空
+    bool mesh_is_empty = (mesh->n_vertices() == 0);
+    std::vector<Mesh::VertexHandle> vhandles(n_vertices);
+
+    if (mesh_is_empty) {
+        // 空 mesh：添加顶点
+        for (int i = 0; i < n_vertices; i++) {
+            vhandles[i] = mesh->add_vertex(Vec3(point(0, i), point(1, i), point(2, i)));
+        }
+    } else {
+        // 已有 mesh：直接覆盖顶点位置
+        if (mesh->n_vertices() != n_vertices) {
+            std::cerr << "[Warning] mesh 顶点数量和要替换的mesh不匹配" << std::endl;
+        }
+        int idx = 0;
+        for (auto v : mesh->vertices()) {
+            if (idx >= n_vertices) break;
+            mesh->set_point(v, Vec3(point(0, idx), point(1, idx), point(2, idx)));
+            vhandles[idx] = v;
+            idx++;
+        }
+    }
+
+// 设置法向量
+#pragma omp parallel for
+    for (int i = 0; i < n_vertices; i++) {
+        if (use_input_normals) {
+            Vec3 n(point_n(0, i), point_n(1, i), point_n(2, i));
+            mesh->set_normal(vhandles[i], n);
+        } else {
+            Vec3 n(0.0, 0.0, 0.0); // 临时零向量，后续自动计算
+            mesh->set_normal(vhandles[i], n);
+        }
+    }
+
+    // 如果没有输入法向量，则自动计算
+    if (!use_input_normals) {
+        mesh->update_normals();
+    }
+
+    return 0;
+}
+
+void Registration::Output_data(const std::string& out_path,const std::string& method_name)
+{
+    Scalar gt_err = SetMeshPoints(deformed_mesh_, deformed_points_3X_, deformed_normals_);
+    std::string out_file;
+    out_file = out_path + method_name+"_res.ply";
+    write_by_openmesh(out_file.c_str(), deformed_mesh, scale);
+    std::cout<< "write the result to " << out_file << "\n" << std::endl;
+    return;
+}

cpp/core/registration.h

@@ -0,0 +1,84 @@
+#ifndef REGISTRATION_H
+#define REGISTRATION_H
+#include "Types.h"
+#include "parameters.h"
+
+
+
+
+class Registration
+{
+public:
+    Registration() = default;
+    virtual ~Registration() = default;
+    
+
+    Matrix3X                src_points_;                // (3,n)
+	Matrix3X                src_normals_;               // (3,n)
+    Matrix3X                src_vert_colors_;            // (3,n)
+	Matrix3X                deformed_normals_;          // (3,n)
+    Matrix3X                deformed_points_3X_;
+	VectorX                 deformed_points_;           // (3n,)
+    Matrix3X                tar_points_;               //  (3,n);
+	Matrix3X                target_normals_;            // (3,n)
+    Matrix3X                tar_vert_colors;           // (3,n)
+
+    double scale=1;
+    Mesh src_mesh;
+    Mesh tar_mesh;
+    Mesh deformed_mesh;
+    Mesh* src_mesh_;
+    Mesh* tar_mesh_;
+    Mesh* deformed_mesh_;
+    int n_src_vertex_;
+    int n_tar_vertex_;
+    int n_landmark_nodes_;   
+    KDtree* target_tree_;                // correspondence paras
+    spare::Parameters pars_;
+
+    std::string res_trans_path_;
+    MatrixXX res_trans;
+
+    struct Closest{
+        int src_idx; // vertex index from source model
+        int tar_idx; // face index from target model
+        Vector3 position;
+        Vector3 normal;
+        Scalar  min_dist2;
+    };
+    typedef std::vector<Closest> VPairs;
+    VPairs correspondence_pairs_;
+
+    VectorX corres_U0_;  // all correspondence points (3, n)
+
+    void InitCorrespondence(VPairs & corres);
+    void FindClosestPoints(VPairs & corres);
+	void FindClosestPoints(KDtree* target_tree_tem,VectorX & deformed_v,VPairs & corres);
+    void FindClosestPoints(KDtree* target_tree_tem,VPairs & corres, VectorX & deformed_v, std::vector<size_t>& sample_indices);
+    double FindKnearestMed(const KDtree& kdtree,
+                           const Matrix3X& X, int nk);
+    void LandMarkCorres(VPairs & correspondence_pairs);
+    bool read_landmark(const char* filename, std::vector<int>& landmark_src, std::vector<int>& landmark_tar);
+    virtual void Read_data(const std::string& file_target,
+                       const std::string& file_source) ;
+    virtual void Read_data(const Matrix3X &target_p,const Matrix3X &source_p,const Matrix3X &target_n,const Matrix3X &source_n) ;
+    virtual void Read_data(const Mesh& tar,const Mesh& src);
+
+    virtual void Register(){}; 
+
+    
+    
+    virtual void Init_data();
+    Scalar SetMeshPoints(Mesh* mesh, const Matrix3X &point,const Matrix3X &point_n);
+    virtual void Output_data(const std::string& out_path,const std::string& method_name) ;
+    virtual void Reg(const std::string& file_target,
+                       const std::string& file_source,
+                       const std::string& out_path){};
+};
+
+
+
+
+#endif // REGISTRATION_H
+
+

cpp/core/rigid_fricp_registration.cpp

@@ -0,0 +1,448 @@
+#include "rigid_fricp_registration.h"
+#include "tools.h"
+#include "robust_norm.h"
+#include <median.h>
+#include "io.h"
+
+    AffineMatrix3 RigidFricpRegistration::LogMatrix(const AffineMatrix3& T)
+     {
+        Eigen::RealSchur<AffineMatrix3> schur(T);
+        AffineMatrix3 U = schur.matrixU();
+        AffineMatrix3 R = schur.matrixT();
+        std::vector<bool> selected(3, true);
+        Matrix33 mat_B = Matrix33::Zero(3, 3);
+        Matrix33 mat_V = Matrix33::Identity(3, 3);
+
+        for (int i = 0; i < 3; i++)
+        {
+            if (selected[i] && fabs(R(i, i) - 1)> SAME_THRESHOLD)
+            {
+                int pair_second = -1;
+                for (int j = i + 1; j <3; j++)
+                {
+                    if (fabs(R(j, j) - R(i, i)) < SAME_THRESHOLD)
+                    {
+                        pair_second = j;
+                        selected[j] = false;
+                        break;
+                    }
+                }
+                if (pair_second > 0)
+                {
+                    selected[i] = false;
+                    R(i, i) = R(i, i) < -1 ? -1 : R(i, i);
+                    double theta = acos(R(i, i));
+                    if (R(i, pair_second) < 0)
+                    {
+                        theta = -theta;
+                    }
+                    mat_B(i, pair_second) += theta;
+                    mat_B(pair_second, i) += -theta;
+                    mat_V(i, pair_second) += -theta / 2;
+                    mat_V(pair_second, i) += theta / 2;
+                    double coeff = 1 - (theta * R(i, pair_second)) / (2 * (1 - R(i, i)));
+                    mat_V(i, i) += -coeff;
+                    mat_V(pair_second, pair_second) += -coeff;
+                }
+            }
+        }
+
+        AffineMatrix3 LogTrim = AffineMatrix3::Zero();
+        LogTrim.block(0, 0, 3, 3) = mat_B;
+        LogTrim.block(0, 3, 3, 1) = mat_V * R.block(0, 3, 3, 1);
+        AffineMatrix3 res = U * LogTrim * U.transpose();
+        return res;
+    }
+
+    Affine3d RigidFricpRegistration::point_to_point(Matrix3X& X,
+                         Matrix3X& Y,
+                        const VectorX& w) {
+        int dim = X.rows();
+        /// Normalize weight vector
+        Eigen::VectorXd w_normalized = w / w.sum();
+        /// De-mean
+        Eigen::VectorXd X_mean(dim), Y_mean(dim);
+        for (int i = 0; i<dim; ++i) {
+            X_mean(i) = (X.row(i).array()*w_normalized.transpose().array()).sum();
+            Y_mean(i) = (Y.row(i).array()*w_normalized.transpose().array()).sum();
+        }
+        X.colwise() -= X_mean;
+        Y.colwise() -= Y_mean;
+        /// Compute transformation
+        Affine3d transformation;
+        MatrixXX sigma = X * w_normalized.asDiagonal() * Y.transpose();
+        Eigen::JacobiSVD<MatrixXX> svd(sigma, Eigen::ComputeFullU | Eigen::ComputeFullV);
+        if (svd.matrixU().determinant()*svd.matrixV().determinant() < 0.0) {
+            Vector3 S = Vector3::Ones(dim); S(dim-1) = -1.0;
+            transformation.linear() = svd.matrixV()*S.asDiagonal()*svd.matrixU().transpose();
+        }
+        else {
+            transformation.linear() = svd.matrixV()*svd.matrixU().transpose();
+        }
+        transformation.translation() = Y_mean - transformation.linear()*X_mean;
+        /// Re-apply mean
+        X.colwise() += X_mean;
+        Y.colwise() += Y_mean;
+        /// Return transformation
+        return transformation;
+    }
+
+     Eigen::Affine3d RigidFricpRegistration::point_to_plane(Eigen::Matrix3Xd& X,
+                                   Eigen::Matrix3Xd& Y,
+                                   const Eigen::Matrix3Xd& Norm,
+                                   const Eigen::VectorXd& w,
+                                   const Eigen::VectorXd& u) {
+        /// Normalize weight vector
+        Eigen::VectorXd w_normalized = w / w.sum();
+        /// De-mean
+        Eigen::Vector3d X_mean;
+        for (int i = 0; i<3; ++i)
+            X_mean(i) = (X.row(i).array()*w_normalized.transpose().array()).sum();
+        X.colwise() -= X_mean;
+        Y.colwise() -= X_mean;
+        /// Prepare LHS and RHS
+        Matrix66 LHS = Matrix66::Zero();
+        Vector6 RHS = Vector6::Zero();
+        Block33 TL = LHS.topLeftCorner<3, 3>();
+        Block33 TR = LHS.topRightCorner<3, 3>();
+        Block33 BR = LHS.bottomRightCorner<3, 3>();
+        Eigen::MatrixXd C = Eigen::MatrixXd::Zero(3, X.cols());
+
+#pragma omp parallel
+        {
+#pragma omp for
+            for (int i = 0; i<X.cols(); i++) {
+                C.col(i) = X.col(i).cross(Norm.col(i));
+            }
+#pragma omp sections nowait
+            {
+#pragma omp section
+                for (int i = 0; i<X.cols(); i++) TL.selfadjointView<Eigen::Upper>().rankUpdate(C.col(i), w(i));
+#pragma omp section
+                for (int i = 0; i<X.cols(); i++) TR += (C.col(i)*Norm.col(i).transpose())*w(i);
+#pragma omp section
+                for (int i = 0; i<X.cols(); i++) BR.selfadjointView<Eigen::Upper>().rankUpdate(Norm.col(i), w(i));
+#pragma omp section
+                for (int i = 0; i<C.cols(); i++) {
+                    double dist_to_plane = -((X.col(i) - Y.col(i)).dot(Norm.col(i)) - u(i))*w(i);
+                    RHS.head<3>() += C.col(i)*dist_to_plane;
+                    RHS.tail<3>() += Norm.col(i)*dist_to_plane;
+                }
+            }
+        }
+        LHS = LHS.selfadjointView<Eigen::Upper>();
+        /// Compute transformation
+        Eigen::Affine3d transformation;
+        Eigen::LDLT<Matrix66> ldlt(LHS);
+        RHS = ldlt.solve(RHS);
+        transformation = Eigen::AngleAxisd(RHS(0), Eigen::Vector3d::UnitX()) *
+                Eigen::AngleAxisd(RHS(1), Eigen::Vector3d::UnitY()) *
+                Eigen::AngleAxisd(RHS(2), Eigen::Vector3d::UnitZ());
+        transformation.translation() = RHS.tail<3>();
+
+        /// Apply transformation
+        /// Re-apply mean
+        X.colwise() += X_mean;
+        Y.colwise() += X_mean;
+        transformation.translation() += X_mean - transformation.linear()*X_mean;
+        /// Return transformation
+        return transformation;
+    }
+
+void RigidFricpRegistration::point_to_point(Matrix3X& X, Matrix3X& Y,Matrix3X& Z, Vector3& source_mean_,
+                        Vector3& target_mean_, ICP::Parameters& par){
+        /// Build kd-tree
+        KDtree kdtree(Y);
+        /// Buffers
+        n_src_vertex_ = X.cols();
+        Matrix3X Q = Matrix3X::Zero(3, X.cols());
+        VectorX W = VectorX::Zero(X.cols());
+        deformed_points_ = VectorX::Zero(3 * n_src_vertex_);
+        Affine3d T;
+        if (par.use_init) 
+        {
+            T.matrix() = par.init_trans;
+        }
+        else 
+        {
+            T = Affine3d::Identity();
+        }
+        MatrixXX To1 = T.matrix();
+        MatrixXX To2 = T.matrix();
+        //Anderson Acc para
+        AndersonAcceleration accelerator_;
+        Affine3d SVD_T = T;
+        double energy = .0, last_energy = std::numeric_limits<double>::max();
+        //ground truth point clouds
+        Matrix3X X_gt = X;
+        if(par.has_groundtruth)
+        {
+            Vector3 temp_trans = par.gt_trans.col(3).head(3);
+            X_gt.colwise() += source_mean_;
+            X_gt = par.gt_trans.block(0, 0, 3, 3) * X_gt;
+            X_gt.colwise() += temp_trans - target_mean_;
+        }
+        //output para
+        std::string file_out = par.out_path;
+        double gt_mse = 0.0;
+        // dynamic welsch paras
+        double nu1 = 1, nu2 = 1;
+        Matrix3X X_deformed = T * X;
+        deformed_points_ = Eigen::Map<const VectorX>(X_deformed.data(), 3 * n_src_vertex_);
+        FindClosestPoints(target_tree_,deformed_points_,correspondence_pairs_);
+        #pragma omp parallel for
+        for (int i = 0; i<n_src_vertex_; ++i) 
+        {
+            W[i] =  correspondence_pairs_[i].min_dist2;
+            Q.col(i) = correspondence_pairs_[i].position;
+        }
+        //dynamic welsch, calc k-nearest points with itself;
+        nu2 = par.nu_end_k * FindKnearestMed(kdtree, Y, 7);
+        double med1;
+        Eigen::VectorXd W_sqrt = W.array().sqrt();
+        igl::median(W_sqrt, med1);
+
+        nu1 = par.nu_begin_k * med1;
+        nu1 = nu1>nu2? nu1:nu2;
+    
+        //AA init
+        accelerator_.init(par.anderson_m, (3 + 1) * (3 + 1), LogMatrix(T.matrix()).data());
+
+        bool stop1 = false;
+        while(!stop1)
+        {
+            /// run ICP
+            int icp = 0;
+            for (; icp<par.max_icp; ++icp)
+            {
+                bool accept_aa = false;
+                energy = get_energy(W, nu1);
+                
+                    if (energy < last_energy) 
+                    {
+                        last_energy = energy;
+                        accept_aa = true;
+                    }
+                    else
+                    {
+                        accelerator_.replace(LogMatrix(SVD_T.matrix()).data());
+                        X_deformed = SVD_T * X;
+                        deformed_points_ = Eigen::Map<const VectorX>(X_deformed.data(), 3 * n_src_vertex_);
+                        FindClosestPoints(target_tree_,deformed_points_,correspondence_pairs_);
+                    #pragma omp parallel for
+                        for (int i = 0; i<n_src_vertex_; ++i) 
+                            {
+                                W[i] =  correspondence_pairs_[i].min_dist2;
+                                Q.col(i) = correspondence_pairs_[i].position;
+                            }
+                        last_energy = get_energy(W, nu1);
+                    }
+                
+                
+
+                if(par.has_groundtruth)
+                {
+                    gt_mse = (T*X - X_gt).squaredNorm()/n_src_vertex_;
+                }
+
+                robust_weight(W, nu1);
+                // Rotation and translation update
+                Eigen::VectorXd W_sqrt = W.array().sqrt();
+                T = point_to_point(X, Q, W_sqrt);
+
+                //Anderson Acc
+                SVD_T = T;
+                
+                AffineMatrix3 Trans = (Eigen::Map<const AffineMatrix3>(accelerator_.compute(LogMatrix(T.matrix()).data()).data(), 3+1, 3+1)).exp();
+                T.linear() = Trans.block(0,0,3,3);
+                T.translation() = Trans.block(0,3,3,1);
+                X_deformed = T * X;
+                deformed_points_ = Eigen::Map<const VectorX>(X_deformed.data(), 3 * n_src_vertex_);
+                FindClosestPoints(target_tree_,deformed_points_,correspondence_pairs_);
+            #pragma omp parallel for
+                for (int i = 0; i<n_src_vertex_; ++i) 
+                    {
+                        W[i] =  correspondence_pairs_[i].min_dist2;
+                        Q.col(i) = correspondence_pairs_[i].position;
+                    }
+                /// Stopping criteria
+                double stop2 = (T.matrix() - To2).norm();
+                To2 = T.matrix();
+                if(stop2 < par.stop)
+                {
+                    break;
+                }
+            }
+            
+            stop1 = fabs(nu1 - nu2)<SAME_THRESHOLD? true: false;
+            nu1 = nu1*par.nu_alpha > nu2? nu1*par.nu_alpha : nu2;
+                
+            accelerator_.reset(LogMatrix(T.matrix()).data());
+            last_energy = std::numeric_limits<double>::max();
+                
+        }
+
+        ///calc convergence energy
+   
+        last_energy = get_energy(W, nu1);
+        Z = T * X;
+        gt_mse = (Z-X_gt).squaredNorm()/n_src_vertex_;
+        T.translation() += - T.rotation() * source_mean_ + target_mean_;
+        Z.colwise() += target_mean_;
+
+        ///save convergence result
+        par.convergence_energy = last_energy;
+        par.convergence_gt_mse = gt_mse;
+        par.res_trans = T.matrix();
+    }
+
+    void RigidFricpRegistration::point_to_plane(Eigen::Matrix3Xd& X,
+                        Eigen::Matrix3Xd& Y, Eigen::Matrix3Xd& norm_x, Eigen::Matrix3Xd& norm_y,
+                        Eigen::Vector3d& source_mean_, Eigen::Vector3d& target_mean_,
+                        ICP::Parameters &par) {
+        /// Build kd-tree
+        KDtree kdtree(Y);
+        /// Buffers
+        Eigen::Matrix3Xd Qp = Eigen::Matrix3Xd::Zero(3, X.cols());
+        Eigen::Matrix3Xd Qn = Eigen::Matrix3Xd::Zero(3, X.cols());
+        Eigen::VectorXd W = Eigen::VectorXd::Zero(X.cols());
+        Eigen::Matrix3Xd ori_X = X;
+        Affine3d T;
+        if (par.use_init) T.matrix() = par.init_trans;
+        else T = Affine3d::Identity();
+        AffineMatrix3 To1 = T.matrix();
+        X = T*X;
+
+        Eigen::Matrix3Xd X_gt = X;
+        if(par.has_groundtruth)
+        {
+            Eigen::Vector3d temp_trans = par.gt_trans.block(0, 3, 3, 1);
+            X_gt = ori_X;
+            X_gt.colwise() += source_mean_;
+            X_gt = par.gt_trans.block(0, 0, 3, 3) * X_gt;
+            X_gt.colwise() += temp_trans - target_mean_;
+        }
+      
+        double gt_mse = 0.0;
+
+        //Anderson Acc para
+        AndersonAcceleration accelerator_;
+        Affine3d LG_T = T;
+        double energy = 0.0, prev_res = std::numeric_limits<double>::max(), res = 0.0;
+
+        // Find closest point
+#pragma omp parallel for
+        for (int i = 0; i<X.cols(); ++i) {
+            int id = kdtree.closest(X.col(i).data());
+            Qp.col(i) = Y.col(id);
+            Qn.col(i) = norm_y.col(id);
+            W[i] = std::abs(Qn.col(i).transpose() * (X.col(i) - Qp.col(i)));
+        }
+
+        bool stop1 = false;
+        while(!stop1)
+        {
+            /// ICP
+            for(int icp=0; icp<par.max_icp; ++icp) {
+                
+
+                bool accept_aa = false;
+                W = W.array().square();//如果后续合并逻辑，就要去掉
+                energy = get_energy( W, par.p);
+        
+                Eigen::VectorXd test_w = (X-Qp).colwise().norm();
+                if(par.has_groundtruth)
+                {
+                    gt_mse = (X - X_gt).squaredNorm()/X.cols();
+                }
+
+                /// Compute weights
+                W = W.array().square();
+                robust_weight( W, par.p);
+                /// Rotation and translation update
+                T = point_to_plane(X, Qp, Qn, W, Eigen::VectorXd::Zero(X.cols()))*T;
+                /// Find closest point
+#pragma omp parallel for
+                for(int i=0; i<X.cols(); i++) {
+                    X.col(i) = T * ori_X.col(i);
+                    int id = kdtree.closest(X.col(i).data());
+                    Qp.col(i) = Y.col(id);
+                    Qn.col(i) = norm_y.col(id);
+                    W[i] = std::abs(Qn.col(i).transpose() * (X.col(i) - Qp.col(i)));
+                }
+
+                /// Stopping criteria
+                double stop2 = (T.matrix() - To1).norm();
+                To1 = T.matrix();
+                if(stop2 < par.stop) break;
+            }
+            stop1 = true;
+        }
+
+        par.res_trans = T.matrix();
+
+        ///calc convergence energy
+        W = (Qn.array()*(X - Qp).array()).colwise().sum().abs().transpose();
+        W = W.array().square();
+        energy = get_energy(W, par.p);
+        gt_mse = (X - X_gt).squaredNorm() / X.cols();
+        T.translation().noalias() += -T.rotation()*source_mean_ + target_mean_;
+        X.colwise() += target_mean_;
+        norm_x = T.rotation()*norm_x;
+
+        ///save convergence result
+        par.convergence_energy = energy;
+        par.convergence_gt_mse = gt_mse;
+        par.res_trans = T.matrix();
+    
+    }
+    void RigidFricpRegistration::Paras_init(bool useinit,std::string fileinit,int maxiter,double stop)
+    {
+        pars.use_init = useinit;//not use the initial transformation
+        file_init_ = fileinit;
+        pars.max_icp=maxiter;
+        pars.stop=stop;
+
+    }
+void RigidFricpRegistration::use_init_transform(bool a)
+{
+    if(a)
+    {
+        MatrixXX init_trans;
+        read_transMat(init_trans, file_init_);
+        init_trans.block(0, 3, 3, 1) /= scale;
+        init_trans.block(0,3,3,1) += init_trans.block(0,0,3,3)*source_mean_ - target_mean_;
+        pars.use_init = true;
+        pars.init_trans = init_trans;
+        //spars.init_trans = init_trans;
+    }
+
+
+}
+
+void RigidFricpRegistration::Register()
+{   
+    // normalization
+    scale = mesh_scaling(src_mesh, tar_mesh);
+    Init_data();
+    //scale=pointwise_normalize(tar_points_,src_points_,source_mean_,target_mean_);
+    use_init_transform(pars.use_init);
+    //--- Execute registration
+    std::cout << "begin registration..." << std::endl;
+    point_to_point(src_points_, tar_points_, deformed_points_3X_,source_mean_, target_mean_, pars);
+    res_trans = pars.res_trans;
+	std::cout << "Registration done!" <<std::endl;
+    return ;
+}
+void RigidFricpRegistration::Reg(const std::string& file_target,
+                       const std::string& file_source,
+                       const std::string& out_path)
+{
+    Read_data(file_target, file_source);
+    Register();
+    Output_data(out_path,"FRICP");
+    return;
+}
+
+
+

cpp/core/rigid_fricp_registration.h

@@ -0,0 +1,48 @@
+
+#ifndef FRICP_H
+#define FRICP_H
+
+#include <AndersonAcceleration.h>
+#define TUPLE_SCALE	  0.95
+#define TUPLE_MAX_CNT 1000
+#include "registration.h"
+
+
+
+
+
+
+class RigidFricpRegistration:public Registration
+{
+public:
+    RigidFricpRegistration(){};
+    ~RigidFricpRegistration(){};
+    std::string file_init_  ;
+    VectorN source_mean_, target_mean_;
+    double time;
+    ICP::Parameters pars;
+    void use_init_transform(bool a);
+    AffineMatrix3 LogMatrix(const AffineMatrix3& T);
+    Affine3d point_to_point(Matrix3X& X,
+                         Matrix3X& Y,
+                        const VectorX& w) ;
+    Eigen::Affine3d point_to_plane(Eigen::Matrix3Xd& X,
+                                   Eigen::Matrix3Xd& Y,
+                                   const Eigen::Matrix3Xd& Norm,
+                                   const Eigen::VectorXd& w,
+                                   const Eigen::VectorXd& u) ;
+    void point_to_point(Matrix3X& X, Matrix3X& Y,Matrix3X& Z, Vector3& source_mean_,
+                        Vector3& target_mean_, ICP::Parameters& par);
+    void point_to_plane(Eigen::Matrix3Xd& X,
+                        Eigen::Matrix3Xd& Y, Eigen::Matrix3Xd& norm_x, Eigen::Matrix3Xd& norm_y,
+                        Eigen::Vector3d& source_mean_, Eigen::Vector3d& target_mean_,
+                        ICP::Parameters &par) ;
+    void Paras_init(bool useinit =false,std::string fileinit=" ",int maxiter=100,double stop=1e-5);
+    void Register()override;
+    void Reg(const std::string& file_target,
+                       const std::string& file_source,
+                       const std::string& out_path)override;
+    };
+
+
+#endif

cpp/io/io.h

@@ -0,0 +1,94 @@
+#ifndef IO_H
+#define IO_H
+#include <cstdio>
+#include "pch.h"
+#include "Types.h"
+
+
+inline bool read_by_openmesh(const std::string filename, Mesh& mesh)
+{
+    OpenMesh::IO::Options opt_read = OpenMesh::IO::Options::VertexNormal;
+    mesh.request_vertex_normals();
+    bool read_OK = OpenMesh::IO::read_mesh(mesh, filename,opt_read);
+
+	std::cout << "filename = " << filename << std::endl;
+    if (read_OK)
+    {
+        mesh.request_vertex_status();
+        mesh.request_edge_status();
+        mesh.request_face_status();
+
+        mesh.request_face_normals();
+
+        mesh.update_face_normals();
+        if(mesh.n_faces()>0)
+            mesh.update_vertex_normals();
+
+        Vec3 MeshScales;
+        MeshScales[0] = 0.0; MeshScales[1] = 0.0; MeshScales[2] = 0.0;
+        for (Mesh::VertexIter v_it = mesh.vertices_begin(); v_it != mesh.vertices_end(); ++v_it)
+        {
+            MeshScales += mesh.point(*v_it);
+        }
+        MeshScales /= mesh.n_vertices();
+        return true;
+    }
+    std::cout << "#vertices = " << mesh.n_vertices() << std::endl;
+    return false;
+}
+
+//---openmesh for output 
+inline bool write_by_openmesh(const char* filename, Mesh& mesh, Scalar scale)
+{
+    for (Mesh::VertexIter v_it = mesh.vertices_begin(); v_it != mesh.vertices_end(); ++v_it)
+    {
+
+       mesh.point(*v_it) = mesh.point(*v_it)*scale;
+    }
+    OpenMesh::IO::Options opt_write = OpenMesh::IO::Options::VertexNormal;
+    bool ok = OpenMesh::IO::write_mesh(mesh, filename,opt_write);
+	return ok;
+}
+
+// If it exists, read the initial rigid transformation matrix.
+template <class MatrixType>
+bool read_transMat(MatrixType& trans, const std::string& filename)
+{
+	std::ifstream input(filename);
+	std::string line;
+	int rows, cols;
+	std::vector<std::vector<double>> total_data;
+	while (getline(input, line)) {
+        if(line[0] == 'V' || line[0] == 'M')
+            continue;
+		std::istringstream iss(line);
+		std::vector<double> lineVec;
+		while (iss) {
+			double item;
+			if (iss >> item)
+				lineVec.push_back(item);
+		}
+		cols = lineVec.size();
+		total_data.push_back(lineVec);
+	}
+	if (total_data.size() == 0)
+	{
+		std::cout << filename << " is empty !! " << std::endl;
+		return false;
+	}
+	rows = total_data.size();
+	trans.resize(rows, cols);
+    std::cout << "rows = " << rows << " cols = " << cols << std::endl;
+	for (int i = 0; i < rows; i++)
+	{
+		for (int j = 0; j < cols; j++)
+		{
+			trans(i, j) = total_data[i][j];
+		}
+	}
+	input.close();
+    std::cout << "read trans = \n" << trans << std::endl;
+	return true;
+}
+
+#endif // IO_H

cpp/pch.h

@@ -0,0 +1,21 @@
+#pragma once
+#include <Eigen/Dense>
+#include <vector>
+#include <utility>
+#include <iostream>  
+#include <string>
+#include <nanoflann.hpp>
+#include "nanoflann.h"
+#include <unsupported/Eigen/MatrixFunctions>
+#include <limits>
+#include <type_traits>
+#include <time.h>
+#include <fstream>
+#include <algorithm>
+#include <Eigen/Sparse>
+#include <OpenMesh/Core/Geometry/VectorT.hh>
+#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
+#include <OpenMesh/Core/IO/MeshIO.hh>
+#include <cmath>
+#include "Types.h"
+#include "OmpHelper.h"

cpp/tools/AndersonAcceleration.h

@@ -0,0 +1,132 @@
+#ifndef ANDERSONACCELERATION_H_
+#define ANDERSONACCELERATION_H_
+
+#include "Types.h"
+#include <cassert>
+#include <algorithm>
+#include <vector>
+#include <omp.h>
+#include <fstream>
+
+class AndersonAcceleration
+{
+public:
+	AndersonAcceleration()
+		:m_(-1), dim_(-1), iter_(-1), col_idx_(-1) {}
+
+	void replace(const Scalar *u)
+	{
+		current_u_ = Eigen::Map<const VectorX>(u, dim_);
+	}
+
+	const VectorX& compute(const Scalar* g)
+	{
+		assert(iter_ >= 0);
+
+		Eigen::Map<const VectorX> G(g, dim_);
+		current_F_ = G - current_u_;
+
+		if (iter_ == 0)
+		{
+			prev_dF_.col(0) = -current_F_;
+			prev_dG_.col(0) = -G;
+			current_u_ = G;
+		}
+		else
+		{
+			prev_dF_.col(col_idx_) += current_F_;
+			prev_dG_.col(col_idx_) += G;
+
+			Scalar eps = 1e-14;
+			Scalar scale = std::max(eps, prev_dF_.col(col_idx_).norm());
+			dF_scale_(col_idx_) = scale;
+			prev_dF_.col(col_idx_) /= scale;
+
+			int m_k = std::min(m_, iter_);
+
+
+			if (m_k == 1)
+			{
+				theta_(0) = 0;
+				Scalar dF_sqrnorm = prev_dF_.col(col_idx_).squaredNorm();
+				M_(0, 0) = dF_sqrnorm;
+				Scalar dF_norm = std::sqrt(dF_sqrnorm);
+
+                if (dF_norm > eps) {
+					theta_(0) = (prev_dF_.col(col_idx_) / dF_norm).dot(current_F_ / dF_norm);
+				}
+			}
+			else
+			{
+				// Update the normal equation matrix, for the column and row corresponding to the new dF column
+				VectorX new_inner_prod = (prev_dF_.col(col_idx_).transpose() * prev_dF_.block(0, 0, dim_, m_k)).transpose();
+				M_.block(col_idx_, 0, 1, m_k) = new_inner_prod.transpose();
+				M_.block(0, col_idx_, m_k, 1) = new_inner_prod;
+
+				// Solve normal equation
+				cod_.compute(M_.block(0, 0, m_k, m_k));
+				theta_.head(m_k) = cod_.solve(prev_dF_.block(0, 0, dim_, m_k).transpose() * current_F_);
+			}
+
+			// Use rescaled theata to compute new u
+			current_u_ = G - prev_dG_.block(0, 0, dim_, m_k) * ((theta_.head(m_k).array() / dF_scale_.head(m_k).array()).matrix());
+			col_idx_ = (col_idx_ + 1) % m_;
+			prev_dF_.col(col_idx_) = -current_F_;
+			prev_dG_.col(col_idx_) = -G;
+		}
+
+		iter_++;
+		return current_u_;
+	}
+    void reset(const Scalar *u)
+    {
+        iter_ = 0;
+        col_idx_ = 0;
+        current_u_ = Eigen::Map<const VectorX>(u, dim_);
+    }
+
+	// m: number of previous iterations used
+	// d: dimension of variables
+	// u0: initial variable values
+	void init(int m, int d, const Scalar* u0)
+	{
+		assert(m > 0);
+		m_ = m;
+		dim_ = d;
+		current_u_.resize(d);
+		current_F_.resize(d);
+		prev_dG_.resize(d, m);
+		prev_dF_.resize(d, m);
+		M_.resize(m, m);
+		theta_.resize(m);
+		dF_scale_.resize(m);
+		current_u_ = Eigen::Map<const VectorX>(u0, d);
+		iter_ = 0;
+		col_idx_ = 0;
+	}
+
+private:
+	VectorX current_u_;
+	VectorX current_F_;
+	MatrixXX prev_dG_;
+	MatrixXX prev_dF_;
+	MatrixXX M_;		// Normal equations matrix for the computing theta
+	VectorX	theta_;	// theta value computed from normal equations
+	VectorX dF_scale_;		// The scaling factor for each column of prev_dF
+	Eigen::CompleteOrthogonalDecomposition<MatrixXX> cod_;
+
+	int m_;		// Number of previous iterates used for Andreson Acceleration
+	int dim_;	// Dimension of variables
+	int iter_;	// Iteration count since initialization
+	int col_idx_;	// Index for history matrix column to store the next value
+	int m_k_;
+
+	Eigen::Matrix4d current_T_;
+	Eigen::Matrix4d current_F_T_;
+
+	MatrixXX T_prev_dF_;
+	MatrixXX T_prev_dG_;
+};
+
+
+#endif /* ANDERSONACCELERATION_H_ */

cpp/tools/OmpHelper.h

@@ -0,0 +1,79 @@
+#ifndef OMPHELPER_H_
+#define OMPHELPER_H_
+
+#define USE_OPENMP
+
+#ifdef USE_OPENMP
+#include <omp.h>
+#ifdef USE_MSVC
+#define OMP_PARALLEL __pragma(omp parallel)
+#define OMP_FOR __pragma(omp for)
+#define OMP_SINGLE __pragma(omp single)
+#define OMP_SECTIONS __pragma(omp sections)
+#define OMP_SECTION __pragma(omp section)
+#else
+#define OMP_PARALLEL _Pragma("omp parallel")
+#define OMP_FOR _Pragma("omp for")
+#define OMP_SINGLE _Pragma("omp single")
+#define OMP_SECTIONS _Pragma("omp sections")
+#define OMP_SECTION _Pragma("omp section")
+#endif
+#else
+#include <ctime>
+#define OMP_PARALLEL
+#define OMP_FOR
+#define OMP_SINGLE
+#define OMP_SECTIONS
+#define OMP_SECTION
+#endif
+
+#include <cassert>
+#include <vector>
+
+
+
+class Timer
+{
+public:
+
+	typedef int EventID;
+
+	EventID get_time()
+	{
+		EventID id = time_values_.size();
+
+#ifdef USE_OPENMP
+		time_values_.push_back(omp_get_wtime());
+#else
+		time_values_.push_back(clock());
+#endif
+
+		return id;
+	}
+
+	double elapsed_time(EventID event1, EventID event2)
+	{
+		assert(event1 >= 0 && event1 < static_cast<EventID>(time_values_.size()));
+		assert(event2 >= 0 && event2 < static_cast<EventID>(time_values_.size()));
+
+#ifdef USE_OPENMP
+		return time_values_[event2] - time_values_[event1];
+#else
+		return double(time_values_[event2] - time_values_[event1]) / CLOCKS_PER_SEC;
+#endif
+	}
+
+	void reset()
+	{
+		time_values_.clear();
+	}
+
+private:
+#ifdef USE_OPENMP
+	std::vector<double> time_values_;
+#else
+	std::vector<clock_t> time_values_;
+#endif
+};
+
+#endif /* OMPHELPER_H_ */

cpp/tools/Types.h

@@ -0,0 +1,248 @@
+#ifndef TYPES_H
+#define TYPES_H
+
+#include <Eigen/Dense>
+#include <Eigen/Sparse>
+#include <OpenMesh/Core/Geometry/VectorT.hh>
+#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
+#include <OpenMesh/Core/IO/MeshIO.hh>
+
+#include <iostream>
+#include <fstream>
+#include <iomanip>
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <cstdio>
+
+#include <algorithm>
+#include <math.h>
+#include <cmath>
+
+#include <vector>
+#include <list>
+#include <set>
+#include <queue>
+#include <map>
+#include <string>
+
+#include <time.h>
+#include <assert.h>
+#include <cstddef>
+#include <limits>
+
+//#define USE_FLOAT_SCALAR
+
+#define SAME_THRESHOLD 1e-6
+
+
+#ifdef USE_FLOAT_SCALAR
+typedef float Scalar;
+#else
+typedef double Scalar;
+#endif
+
+typedef Eigen::SparseMatrix<Scalar, Eigen::ColMajor> ColMajorSparseMatrix;
+typedef Eigen::SparseMatrix<Scalar, Eigen::RowMajor> RowMajorSparseMatrix;
+
+
+#ifdef EIGEN_DONT_ALIGN
+#define EIGEN_ALIGNMENT Eigen::DontAlign
+#else
+#define EIGEN_ALIGNMENT Eigen::AutoAlign
+#endif
+
+
+typedef Eigen::Triplet<Scalar> Triplet;
+
+// ======================================================
+// 基础模板别名
+// ======================================================
+template <int Rows, int Cols, int Options = (Eigen::ColMajor | Eigen::AutoAlign)>
+using MatrixT = Eigen::Matrix<Scalar, Rows, Cols, Options>;
+
+// ======================
+// 向量类型
+// ======================
+using Vector2 = MatrixT<2,1>;
+using Vector3 = MatrixT<3,1>;
+using Vector4 = MatrixT<4,1>;
+using Vector6 = MatrixT<6,1>;
+using VectorX = MatrixT<Eigen::Dynamic,1>;
+using VectorN = Vector3;                // 别名（等价于 Vector3）
+
+// ======================
+// 固定尺寸矩阵
+// ======================
+using Matrix22 = MatrixT<2,2>;
+using Matrix23 = MatrixT<2,3>;
+using Matrix32 = MatrixT<3,2>;
+using Matrix33 = MatrixT<3,3>;
+using Matrix34 = MatrixT<3,4>;
+using Matrix44 = MatrixT<4,4>;
+using Matrix66 = MatrixT<6,6>;
+using EigenMatrix12 = MatrixT<12,12>;
+
+// ======================
+// 动态尺寸矩阵
+// ======================
+using Matrix2X = MatrixT<2,Eigen::Dynamic>;
+using Matrix3X = MatrixT<3,Eigen::Dynamic>;
+using Matrix4X = MatrixT<4,Eigen::Dynamic>;
+using MatrixX2 = MatrixT<Eigen::Dynamic,2>;
+using MatrixX3 = MatrixT<Eigen::Dynamic,3>;
+using MatrixXX = MatrixT<Eigen::Dynamic,Eigen::Dynamic>;
+using Vertices = Matrix3X;              // 顶点矩阵，3×N
+
+// ======================
+// 特殊变换与几何类型
+// ======================
+using Affine3  = Eigen::Transform<Scalar,3,Eigen::Affine>;
+using Affine3d = Affine3;               // 同义，方便兼容
+using AffineMatrix3 = Matrix44;         // 4×4 变换矩阵
+
+using EigenAngleAxis  = Eigen::AngleAxis<Scalar>;
+using EigenQuaternion = Eigen::Quaternion<Scalar, Eigen::DontAlign>;
+
+// ======================
+// 块类型
+// ======================
+using Block33 = Eigen::Block<Matrix66,3,3>;
+
+// Conversion between a 3d vector type to Eigen::Vector3d
+template<typename Vec_T>
+inline Vector3 to_eigen_vec3(const Vec_T &vec)
+{
+    return Vector3(vec[0], vec[1], vec[2]);
+}
+
+
+template<typename Vec_T>
+inline Vec_T from_eigen_vec3(const Vector3 &vec)
+{
+    Vec_T v;
+    v[0] = vec(0);
+    v[1] = vec(1);
+    v[2] = vec(2);
+
+    return v;
+}
+
+enum VertexState
+{
+    OUTSIDE,
+    FRONT,
+    INSIDE
+};
+
+struct TriTraits : public OpenMesh::DefaultTraits {
+    #ifdef USE_FLOAT_SCALAR
+    typedef OpenMesh::Vec3f Point;
+    typedef OpenMesh::Vec3f Normal;
+#else
+    typedef OpenMesh::Vec3d Point;
+    typedef OpenMesh::Vec3d Normal;
+#endif
+    typedef OpenMesh::Vec4f Color;
+
+
+
+    VertexAttributes(OpenMesh::Attributes::Status);
+    FaceAttributes(OpenMesh::Attributes::Status);
+    EdgeAttributes(OpenMesh::Attributes::Status);
+    HalfedgeAttributes(OpenMesh::Attributes::Status);
+
+    VertexTraits
+    {
+        VertexT() : geodesic_distance(1e100), state(OUTSIDE),incident_point(0.0),
+          saddle_or_boundary(false)
+        {};
+    public:
+        Scalar geodesic_distance;
+        VertexState state;
+        OpenMesh::FaceHandle incident_face;
+        Scalar incident_point;
+        bool saddle_or_boundary;
+    };
+    FaceTraits
+    {
+        FaceT() : corner_angles(0,0,0)
+        {};
+        public:
+        Vector3 corner_angles;
+    };
+    EdgeTraits
+    {
+        EdgeT() : length(0.0)
+        {};
+    public:
+        Scalar length;
+    };
+};
+typedef OpenMesh::TriMesh_ArrayKernelT<TriTraits> Mesh;
+#ifdef USE_FLOAT_SCALAR
+typedef OpenMesh::Vec3f Vec3;
+#else
+typedef OpenMesh::Vec3d Vec3;
+#endif
+
+
+class Matrix3333 // 3x3 matrix: each element is a 3x3 matrix
+{
+public:
+    Matrix3333();
+    Matrix3333(const Matrix3333& other);
+    ~Matrix3333() {}
+
+    void SetZero(); // [0 0 0; 0 0 0; 0 0 0]; 0 = 3x3 zeros
+    void SetIdentity(); //[I 0 0; 0 I 0; 0 0 I]; 0 = 3x3 zeros, I = 3x3 identity
+
+                        // operators
+    Matrix33& operator() (int row, int col);
+    Matrix3333 operator+ (const Matrix3333& plus);
+    Matrix3333 operator- (const Matrix3333& minus);
+    Matrix3333 operator* (const Matrix33& multi);
+    friend Matrix3333 operator* (const Matrix33& multi1, Matrix3333& multi2);
+    Matrix3333 operator* (Scalar multi);
+    friend Matrix3333 operator* (Scalar multi1, Matrix3333& multi2);
+    Matrix3333 transpose();
+    Matrix33 Contract(const Matrix33& multi); // this operator is commutative
+    Matrix3333 Contract(Matrix3333& multi);
+
+    //protected:
+
+    Matrix33 mat[3][3];
+};
+
+class Matrix2222 // 2x2 matrix: each element is a 2x2 matrix
+{
+public:
+    Matrix2222();
+    Matrix2222(const Matrix2222& other);
+    ~Matrix2222() {}
+
+    void SetZero(); // [0 0; 0 0]; 0 = 2x2 zeros
+    void SetIdentity(); //[I 0; 0 I;]; 0 = 2x2 zeros, I = 2x2 identity
+
+                        // operators and basic functions
+    Matrix22& operator() (int row, int col);
+    Matrix2222 operator+ (const Matrix2222& plus);
+    Matrix2222 operator- (const Matrix2222& minus);
+    Matrix2222 operator* (const Matrix22& multi);
+    friend Matrix2222 operator* (const Matrix22& multi1, Matrix2222& multi2);
+    Matrix2222 operator* (Scalar multi);
+    friend Matrix2222 operator* (Scalar multi1, Matrix2222& multi2);
+    Matrix2222 transpose();
+    Matrix22 Contract(const Matrix22& multi); // this operator is commutative
+    Matrix2222 Contract(Matrix2222& multi);
+
+protected:
+
+    Matrix22 mat[2][2];
+};
+
+// dst = src1 \kron src2
+void directProduct(Matrix3333& dst, const Matrix33& src1, const Matrix33& src2);
+void directProduct(Matrix2222& dst, const Matrix22& src1, const Matrix22& src2);
+#endif // TYPES_H
+///////////////////////////////////////////////////////////////////////////////

cpp/tools/geodesic/geodesic_algorithm_base.h

@@ -0,0 +1,417 @@
+#ifndef GEODESIC_ALGORITHM_BASE
+#define GEODESIC_ALGORITHM_BASE
+
+#include "geodesic_algorithm_exact_elements.h"
+#include "geodesic_constants_and_simple_functions.h"
+#define hfid0 0
+#define hfid1 1
+
+namespace geodesic{
+class GeodesicAlgorithmBase
+{
+public:
+    vertex_pointer opposite_vertex(edge_pointer e, vertex_pointer v)
+    {
+        halfedge_handle hf0 = this->mesh()->halfedge_handle(e, hfid0);
+        if(this->mesh()->from_vertex_handle(hf0).idx()==v.idx())
+            return this->mesh()->to_vertex_handle(hf0);
+        else
+            return this->mesh()->from_vertex_handle(hf0);
+    };
+
+    vertex_pointer opposite_vertex(face_pointer f, edge_pointer e)
+    {
+        halfedge_handle hf = this->mesh()->halfedge_handle(e, hfid0);
+        hf = this->mesh()->face_handle(hf)==f? hf : this->mesh()->opposite_halfedge_handle(hf);
+        return this->mesh()->to_vertex_handle(this->mesh()->next_halfedge_handle(hf));
+    };
+
+    bool belongs_v(edge_pointer e, vertex_pointer v)
+    {
+        halfedge_handle hf = this->mesh()->halfedge_handle(e, hfid0);
+        return this->mesh()->from_vertex_handle(hf) == v ||
+                this->mesh()->to_vertex_handle(hf) == v;
+    }
+
+    edge_pointer next_edge(face_pointer f, edge_pointer e, vertex_pointer v)
+    {
+
+        halfedge_handle hf = this->mesh()->halfedge_handle(e, hfid0);
+        hf = this->mesh()->face_handle(hf)==f? hf : this->mesh()->opposite_halfedge_handle(hf);
+        halfedge_handle next_hf;
+        if(this->mesh()->from_vertex_handle(hf)==v)
+        {
+            next_hf = this->mesh()->next_halfedge_handle(this->mesh()->next_halfedge_handle(hf));
+        }
+        else if(this->mesh()->to_vertex_handle(hf)==v)
+            next_hf = this->mesh()->next_halfedge_handle(hf);
+        else
+        {
+            std::cout << "next_edge e and v has no connection" << std::endl;
+            exit(1);
+        }
+        return this->mesh()->edge_handle(next_hf);
+    };
+
+    Scalar vertex_angle(face_pointer f, vertex_pointer v)
+    {
+        halfedge_handle hf0 = this->mesh()->halfedge_handle(f);
+        vertex_pointer v0 = this->mesh()->from_vertex_handle(hf0);
+        vertex_pointer v1 = this->mesh()->to_vertex_handle(hf0);
+        vertex_pointer v2 = this->mesh()->to_vertex_handle(this->mesh()->next_halfedge_handle(hf0));
+        Scalar l1 = (this->mesh()->point(v0) - this->mesh()->point(v1)).norm();
+        Scalar l2 = (this->mesh()->point(v1) - this->mesh()->point(v2)).norm();
+        Scalar l3 = (this->mesh()->point(v2) - this->mesh()->point(v0)).norm();
+        if(v0.idx()==v.idx())
+        {
+            return angle_from_edges(l2, l3, l1);
+        }
+        if(v1.idx()==v.idx())
+        {
+            return angle_from_edges(l3, l1, l2);
+        }
+        if(v2.idx()==v.idx())
+        {
+            return angle_from_edges(l1, l2, l3);
+        }
+    };
+
+    void update_edgelen()
+    {
+        for(auto eit = this->mesh()->edges_begin(); eit != this->mesh()->edges_end(); eit++)
+        {
+            halfedge_handle hf = this->mesh()->halfedge_handle(*eit, hfid0);
+            Vec3 v1 = this->mesh()->point(this->mesh()->from_vertex_handle(hf));
+            Vec3 v2 = this->mesh()->point(this->mesh()->to_vertex_handle(hf));
+            this->mesh()->data(*eit).length = (v1-v2).norm();
+        }
+    };
+
+    void build_adjacencies();
+
+    edge_pointer opposite_edge(face_pointer f, vertex_pointer v)
+    {
+        for(auto e_it = this->mesh()->fe_begin(f); e_it != this->mesh()->fe_end(f); ++e_it)
+        {
+            edge_pointer e = *e_it;
+            if(!belongs_v(e, v))
+            {
+                return e;
+            }
+        }
+    };
+
+    face_pointer opposite_face(edge_pointer e, face_pointer f)
+    {
+        halfedge_handle hf = this->mesh()->halfedge_handle(e, hfid0);
+        return this->mesh()->face_handle(hf) == f?
+                    this->mesh()->face_handle(this->mesh()->opposite_halfedge_handle(hf))
+                  :this->mesh()->face_handle(hf);
+    };
+
+    void initialize(Mesh* mesh);
+
+    GeodesicAlgorithmBase(Mesh* mesh)
+    {
+       initialize(mesh);
+    };
+
+    GeodesicAlgorithmBase(){m_mesh = NULL;};
+
+    virtual ~GeodesicAlgorithmBase(){};
+
+    virtual void print_statistics()		//print info about timing and memory usage in the propagation step of the algorithm
+    {
+        std::cout << "propagation step took " << m_time_consumed << " seconds " << std::endl;
+    };
+
+    Mesh* mesh(){return m_mesh;};
+
+    // propagate a window
+    bool compute_propagated_parameters(Scalar pseudo_x,
+                                       Scalar pseudo_y,
+                                       Scalar start,
+                                       Scalar end,		//start/end of the interval
+                                       Scalar alpha,	//corner angle
+                                       Scalar L,		//length of the new edge
+                                       interval_pointer candidates,
+                                       Scalar d);		//if it is the last interval on the edge
+
+    // intersection point on an edge
+    Scalar compute_positive_intersection(Scalar start,
+                                         Scalar pseudo_x,
+                                         Scalar pseudo_y,
+                                         Scalar sin_alpha,
+                                         Scalar cos_alpha);
+
+    inline bool calculate_triangle_parameters(list_pointer &list, Triangle &Tri); // calculate the parameters of the triangle to be propagated
+
+    list_pointer interval_list_0(edge_pointer e)
+    {
+        return &m_edge_interval_lists_0[e.idx()];
+    };
+
+    list_pointer interval_list_1(edge_pointer e)
+    {
+        return &m_edge_interval_lists_1[e.idx()];
+    };
+
+
+protected:
+
+    Mesh* m_mesh;
+
+    Triangle Tri; // the triangle to be propagated
+
+    IntervalList wl_left, wl_right;
+
+    std::vector<IntervalList> m_edge_interval_lists_0;		// windows propagated from adjacent_face[0] of the edge
+    std::vector<IntervalList> m_edge_interval_lists_1;		// windows propagated from adjacent_face[1] of the edge
+
+};
+
+inline void GeodesicAlgorithmBase::initialize(Mesh* mesh)
+{
+    m_mesh = mesh;
+    m_edge_interval_lists_0.resize(mesh->n_edges());
+    m_edge_interval_lists_1.resize(mesh->n_edges());
+
+    // initialize statistics
+    m_queue_max_size      = 0;
+    m_windows_propagation = 0;
+    m_windows_wavefront   = 0;
+    m_windows_peak        = 0;
+
+    // initialize window lists, similar to half-edge structure
+    for (unsigned i = 0; i < m_edge_interval_lists_0.size(); ++i)
+    {
+        edge_pointer edge = mesh->edge_handle(i);
+        m_edge_interval_lists_0[i].initialize(edge);
+        m_edge_interval_lists_1[i].initialize(edge);
+        interval_list_0(edge)->start_vertex() = mesh->from_vertex_handle(mesh->halfedge_handle(edge, hfid0));
+        interval_list_1(edge)->start_vertex() = mesh->from_vertex_handle(mesh->halfedge_handle(edge, hfid1));
+    }
+    // verify list links
+    for (unsigned i = 0; i < mesh->n_faces(); ++i)
+    {
+        face_pointer f = (mesh->face_handle(i));
+        vertex_pointer v[3];
+        size_t j = 0;
+        for (auto e_it = mesh->fe_begin(f); e_it != mesh->fe_end(f); ++e_it)
+        {
+            edge_pointer e = *e_it;
+            if (mesh->face_handle(mesh->halfedge_handle(e, hfid0)) == f)
+                v[j] = interval_list_0(e)->start_vertex();
+            else
+                v[j] = interval_list_1(e)->start_vertex();
+
+            if ((interval_list_0(e)->start_vertex().idx() < 0) || (interval_list_1(e)->start_vertex().idx() < 0))
+            {
+                std::cout << "list link error" << std::endl;
+                exit(1);
+            }
+
+            if (interval_list_0(e)->start_vertex() == interval_list_1(e)->start_vertex())
+            {
+                std::cout << "list link error" << std::endl;
+                exit(1);
+            }
+
+            if (!((belongs_v(e, interval_list_0(e)->start_vertex())) &&
+                  (belongs_v(e, interval_list_1(e)->start_vertex()))))
+            {
+                std::cout << "list link error" << std::endl;
+                exit(1);
+            }
+            j++;
+        }
+        if ((v[0].idx() >-1 && v[0] == v[1]) || (v[0].idx() >-1 && v[0] == v[2]) || (v[1].idx() >-1 && v[1] == v[2]))
+        {
+            std::cout << "list link error" << std::endl;
+            exit(1);
+        }
+    }
+    update_edgelen();
+    build_adjacencies();
+};
+
+inline Scalar GeodesicAlgorithmBase::compute_positive_intersection(Scalar start,
+                                                                   Scalar pseudo_x,
+                                                                   Scalar pseudo_y,
+                                                                   Scalar sin_alpha,
+                                                                   Scalar cos_alpha)
+{
+    //assert(pseudo_y < 0);
+    assert(pseudo_y <= 0);
+
+    Scalar denominator = sin_alpha*(pseudo_x - start) - cos_alpha*pseudo_y;
+    if (denominator < 0.0)
+    {
+        return -1.0;
+    }
+
+    Scalar numerator = -pseudo_y*start;
+
+    if (numerator < 1e-30)
+    {
+        return 0.0;
+    }
+
+    if (denominator < 1e-30)
+    {
+        return -1.0;
+    }
+
+    return numerator / denominator;
+}
+
+inline bool GeodesicAlgorithmBase::compute_propagated_parameters(Scalar pseudo_x,
+                                                                 Scalar pseudo_y,
+                                                                 Scalar begin,
+                                                                 Scalar end,		//start/end of the interval
+                                                                 Scalar alpha,	//corner angle
+                                                                 Scalar L,		//length of the new edge
+                                                                 interval_pointer candidates,
+                                                                 Scalar d)
+{
+    assert(pseudo_y <= 0.0);
+    assert(begin <= end);
+    assert(begin >= 0);
+
+    ++m_windows_propagation; // Statistics
+
+    interval_pointer p = candidates;
+
+    Scalar sin_alpha = sin(alpha);
+    Scalar cos_alpha = cos(alpha);
+
+    //important: for the first_interval, this function returns zero only if the new edge is "visible" from the source
+    //if the new edge can be covered only after turn_over, the value is negative (-1.0)
+    Scalar L1 = compute_positive_intersection(begin,
+                                              pseudo_x,
+                                              pseudo_y,
+                                              sin_alpha,
+                                              cos_alpha);
+
+    if (L1 < 0 || L1 >= L) // Does not produce a window on the edge
+        return false;
+
+    Scalar L2 = compute_positive_intersection(end,
+                                              pseudo_x,
+                                              pseudo_y,
+                                              sin_alpha,
+                                              cos_alpha);
+
+    if (L2 < 0 || L2 >= L) // Covers vertex
+    {
+        p->start() = L1;
+        p->stop() = L;
+        p->pseudo_x() = cos_alpha*pseudo_x + sin_alpha*pseudo_y;
+        p->pseudo_y() = -sin_alpha*pseudo_x + cos_alpha*pseudo_y;
+        assert(p->pseudo_y() <= 0.0);
+
+        return true;
+    }
+    else
+    {
+        // Does not cover vertex
+        p->start() = L1;
+        p->stop() = L2;
+        p->pseudo_x() = cos_alpha*pseudo_x + sin_alpha*pseudo_y;
+        p->pseudo_y() = -sin_alpha*pseudo_x + cos_alpha*pseudo_y;
+        assert(p->pseudo_y() <= 0.0);
+
+        return true;
+    }
+}
+
+inline bool GeodesicAlgorithmBase::calculate_triangle_parameters(list_pointer &list, Triangle &Tri) // Calculate the parameters of the triangle to be propagated
+{
+    OpenMesh::HalfedgeHandle hf0 = this->mesh()->halfedge_handle(list->edge(), hfid0);
+    OpenMesh::HalfedgeHandle hf1 = this->mesh()->halfedge_handle(list->edge(), hfid1);
+    size_t adjface_size=0;
+    if(this->mesh()->face_handle(hf0).idx()>-1)
+        adjface_size++;
+    if(this->mesh()->face_handle(hf1).idx()>-1)
+        adjface_size++;
+
+    if (adjface_size > 1)
+    {
+        Tri.bottom_edge = list->edge();
+
+        if (list == interval_list_0(Tri.bottom_edge))
+            Tri.face = this->mesh()->face_handle(hf1);
+        else
+            Tri.face = this->mesh()->face_handle(hf0);
+
+        Tri.top_vertex = opposite_vertex(Tri.face, Tri.bottom_edge);
+        Tri.left_vertex = list->start_vertex();
+        Tri.right_vertex = opposite_vertex(Tri.bottom_edge, Tri.left_vertex);
+
+        Tri.left_edge = next_edge(Tri.face, Tri.bottom_edge, Tri.left_vertex);
+        Tri.right_edge = next_edge(Tri.face, Tri.bottom_edge, Tri.right_vertex);
+
+        Tri.top_alpha = vertex_angle(Tri.face, Tri.top_vertex);
+        Tri.left_alpha = vertex_angle(Tri.face, Tri.left_vertex);
+        Tri.right_alpha = vertex_angle(Tri.face, Tri.right_vertex);
+
+        if (this->mesh()->face_handle(this->mesh()->halfedge_handle(Tri.left_edge, hfid0)) == Tri.face)
+            Tri.left_list = interval_list_0(Tri.left_edge);
+        else
+            Tri.left_list = interval_list_1(Tri.left_edge);
+
+        if (this->mesh()->face_handle(this->mesh()->halfedge_handle(Tri.right_edge, hfid0)) == Tri.face)
+            Tri.right_list = interval_list_0(Tri.right_edge);
+        else
+            Tri.right_list = interval_list_1(Tri.right_edge);
+
+        return false;
+    }
+    else
+    {
+        return true;
+    }
+}
+
+
+inline void GeodesicAlgorithmBase::build_adjacencies()
+{
+    // define m_turn_around_flag for vertices
+    std::vector<Scalar> total_vertex_angle(this->mesh()->n_vertices(), 0);
+    for(auto f_it = this->mesh()->faces_begin(); f_it != this->mesh()->faces_end(); ++f_it)
+    {
+        halfedge_handle hf0 = this->mesh()->halfedge_handle(*f_it);
+        vertex_pointer v0 = this->mesh()->from_vertex_handle(hf0);
+        vertex_pointer v1 = this->mesh()->to_vertex_handle(hf0);
+        vertex_pointer v2 = this->mesh()->to_vertex_handle(this->mesh()->next_halfedge_handle(hf0));
+        Scalar l1 = (this->mesh()->point(v0) - this->mesh()->point(v1)).norm();
+        Scalar l2 = (this->mesh()->point(v1) - this->mesh()->point(v2)).norm();
+        Scalar l3 = (this->mesh()->point(v2) - this->mesh()->point(v0)).norm();
+
+        total_vertex_angle[v0.idx()] += angle_from_edges(l2, l3, l1);
+        total_vertex_angle[v1.idx()] += angle_from_edges(l3, l1, l2);
+        total_vertex_angle[v2.idx()] += angle_from_edges(l1, l2, l3);
+    }
+
+    for(auto v_it = this->mesh()->vertices_begin(); v_it != this->mesh()->vertices_end(); ++v_it)
+    {
+        vertex_pointer v = *v_it;
+        this->mesh()->data(v).saddle_or_boundary = (total_vertex_angle[v.idx()] > 2.0*M_PI - 1e-5);
+    }
+
+    for(auto e_it = this->mesh()->edges_begin(); e_it != this->mesh()->edges_end(); ++e_it)
+    {
+        edge_pointer e = *e_it;
+        if(this->mesh()->is_boundary(e))
+        {
+            halfedge_handle hf = this->mesh()->halfedge_handle(e, hfid0);
+            this->mesh()->data(this->mesh()->from_vertex_handle(hf)).saddle_or_boundary = true;
+            this->mesh()->data(this->mesh()->to_vertex_handle(hf)).saddle_or_boundary = true;
+        }
+    }
+}
+
+
+}//geodesic
+
+#endif

cpp/tools/geodesic/geodesic_algorithm_exact.h

@@ -0,0 +1,918 @@
+#ifndef GEODESIC_ALGORITHM_EXACT
+#define GEODESIC_ALGORITHM_EXACT
+
+#include "geodesic_memory.h"
+#include "geodesic_algorithm_base.h"
+#include "geodesic_algorithm_exact_elements.h"
+#include <string.h>
+
+namespace geodesic {
+
+	class GeodesicAlgorithmExact : public GeodesicAlgorithmBase
+	{
+	public:
+
+		// basic functions related to class
+        GeodesicAlgorithmExact(Mesh* mesh) :
+			GeodesicAlgorithmBase(mesh),
+            m_memory_allocator(mesh->n_edges(), mesh->n_edges())
+        {ori_mesh = mesh;};
+
+        // construct neighboring mesh around src within m_radius, if no mesh is constructed,
+        // increase m_radius with increase_radio
+        GeodesicAlgorithmExact(Mesh* mesh, size_t src, Scalar m_radius)
+        {
+            ori_mesh = mesh;
+            Mesh* sub_mesh = new Mesh;
+            bool ok = construct_submesh(sub_mesh, src, m_radius);
+            if(ok)
+            {
+                GeodesicAlgorithmBase::initialize(sub_mesh);
+                m_memory_allocator.reset(sub_mesh->n_edges(), sub_mesh->n_edges());
+            }
+            else
+            {
+                std::cerr << "Error:Some points cannot be covered under the specified radius, please increase the radius" << std::endl;
+                exit(1);
+            }
+        };
+		~GeodesicAlgorithmExact() {};
+        void clear() {
+            m_memory_allocator.clear();
+            for(auto v_it = this->mesh()->vertices_begin();v_it != this->mesh()->vertices_end(); ++v_it)
+            {
+                this->mesh()->data(*v_it).geodesic_distance = GEODESIC_INF;
+            }
+       };
+
+        // main entry
+        void propagate(unsigned source, std::vector<size_t>& idxs);
+
+        // print the resulting statistics
+        void print_statistics();
+	private:
+
+        // simple functions
+        void initialize_propagation_data();
+        void create_pseudo_source_windows(vertex_pointer &v, bool UpdateFIFOQueue);
+        void erase_from_queue(vertex_pointer& v);
+
+        // propagate a windows list (Rule 1)
+        void find_separating_point(list_pointer &list); // find the separating point of the windows and the list
+        void propagate_windows_to_two_edges(list_pointer &list); // propagates windows to two edges accross a triangle face
+
+        // pairwise windows checking (Rule 2)
+        void check_with_vertices(list_pointer &list);
+        windows_state check_between_two_windows(interval_pointer &w1, interval_pointer &w2); // Check two neighbouring crossing windows on same edge
+        void pairwise_windows_checking(list_pointer &list); // Check crossing windows on same edge
+
+        // main operation
+        void propagate_one_windows_list(list_pointer &list);
+
+        // construct neighboring mesh
+        bool construct_submesh(Mesh* sub_mesh, size_t source_idx, Scalar radius);
+
+		// member variables
+        std::set<vertex_pointer> m_vertex_queue;
+		std::queue<list_pointer> m_list_queue;                // FIFO queue for lists
+		MemoryAllocator<Interval> m_memory_allocator;		  // quickly allocate and deallocate intervals 
+        Scalar neighbor_radius;
+
+        Eigen::VectorXi SubVidxfromMesh;
+        std::vector<int> MeshVidxfromSub;
+
+        Mesh* ori_mesh;
+		unsigned m_source;
+	};
+
+
+	//----------------- simple functions ---------------------
+	inline void GeodesicAlgorithmExact::initialize_propagation_data()
+	{
+		clear();
+
+		// initialize source's parameters
+        vertex_pointer source = (this->mesh()->vertex_handle(m_source));
+        this->mesh()->data(source).geodesic_distance = 0;
+        this->mesh()->data(source).state = VertexState::INSIDE;
+
+		// initialize windows around source
+		create_pseudo_source_windows(source, false);
+	}
+
+    inline void GeodesicAlgorithmExact::erase_from_queue(vertex_pointer& v)
+	{
+        assert(m_vertex_queue.count(v) <= 1);
+
+        std::multiset<vertex_pointer>::iterator it = m_vertex_queue.find(v);
+        if (it != m_vertex_queue.end())
+            m_vertex_queue.erase(it);
+	}
+
+	inline void GeodesicAlgorithmExact::create_pseudo_source_windows(vertex_pointer &pseudo_source, bool inside_traversed_area)
+	{
+		// update vertices around pseudo_source
+        for (auto e_it = this->mesh()->ve_begin(pseudo_source); e_it != this->mesh()->ve_end(pseudo_source); ++e_it)
+		{
+            edge_pointer   edge_it = *e_it;
+            vertex_pointer vert_it = opposite_vertex(edge_it, pseudo_source);
+
+            Scalar distance = this->mesh()->data(pseudo_source).geodesic_distance
+                    + this->mesh()->data(edge_it).length;
+
+            if (distance < this->mesh()->data(vert_it).geodesic_distance)
+			{
+				m_vertex_queue.erase(vert_it);
+
+                this->mesh()->data(vert_it).geodesic_distance = distance;
+                if (this->mesh()->data(vert_it).state == VertexState::OUTSIDE)
+                    this->mesh()->data(vert_it).state = VertexState::FRONT;
+
+                this->mesh()->data(vert_it).incident_face = this->mesh()->face_handle(this->mesh()->halfedge_handle(edge_it, hfid0));
+                edge_pointer next_edge = geodesic::GeodesicAlgorithmBase::next_edge(
+                            this->mesh()->data(vert_it).incident_face,edge_it, pseudo_source);
+                this->mesh()->data(vert_it).incident_point =
+                        (this->mesh()->from_vertex_handle(this->mesh()->halfedge_handle(next_edge, hfid0)) == pseudo_source) ?
+                            0 : this->mesh()->data(next_edge).length;
+
+                m_vertex_queue.insert(vert_it);
+			}
+		}
+
+        // update pseudo_source windows around pseudo_source
+        for(auto f_it = this->mesh()->vf_begin(pseudo_source); f_it != this->mesh()->vf_end(pseudo_source); ++f_it)
+		{
+            face_pointer face_it = *f_it;
+            edge_pointer edge_it = geodesic::GeodesicAlgorithmBase::opposite_edge(face_it, pseudo_source);
+            list_pointer list = (this->mesh()->face_handle(this->mesh()->halfedge_handle(edge_it, hfid0))==face_it)?
+                    interval_list_0(edge_it) : interval_list_1(edge_it);
+
+			// create a window
+			interval_pointer candidate = new Interval;
+
+			candidate->start() = 0;
+            candidate->stop() = this->mesh()->data(edge_it).length;
+            candidate->d() = this->mesh()->data(pseudo_source).geodesic_distance;
+            Scalar angle = geodesic::GeodesicAlgorithmBase::vertex_angle(face_it, list->start_vertex());
+            Scalar length = this->mesh()->data(geodesic::GeodesicAlgorithmBase::next_edge
+                                               (face_it, edge_it,list->start_vertex())).length;
+			candidate->pseudo_x() = cos(angle) * length;
+			candidate->pseudo_y() = -sin(angle) * length;
+
+			// insert into list
+			list->push_back(candidate);
+
+			// push into M_LIST_QUEUE if inside traversed area
+            vertex_pointer v0 = this->mesh()->from_vertex_handle(this->mesh()->halfedge_handle(edge_it, hfid0));
+            vertex_pointer v1 = this->mesh()->from_vertex_handle(this->mesh()->halfedge_handle(edge_it, hfid1));
+            if ((inside_traversed_area) &&
+                    ((this->mesh()->data(v0).state != VertexState::FRONT)
+                     || (this->mesh()->data(v1).state != VertexState::FRONT)))
+				m_list_queue.push(list);
+
+			// Statistics
+			++m_windows_wavefront;
+			if (m_windows_peak < m_windows_wavefront)
+				m_windows_peak = m_windows_wavefront;
+
+		}
+    }
+
+	//----------------- propagate a windows list (Rule 1) ---------------------
+	inline void GeodesicAlgorithmExact::find_separating_point(list_pointer &list)
+    {
+        const Scalar LOCAL_EPSILON = 1e-20 * this->mesh()->data(list->edge()).length; // numerical issue
+
+        Scalar L = this->mesh()->data(Tri.left_edge).length;
+        Scalar top_x = L * cos(Tri.left_alpha);
+        Scalar top_y = L * sin(Tri.left_alpha);
+
+        Scalar temp_geodesic = GEODESIC_INF;
+        face_pointer temp_face_handle = this->mesh()->data(Tri.top_vertex).incident_face;
+        Scalar temp_incident_point = this->mesh()->data(Tri.top_vertex).incident_point;
+
+		interval_pointer iter = list->begin();
+
+        Scalar wlist_sp = 0;
+        Scalar wlist_pseudo_x = 0;
+        Scalar wlist_pseudo_y = 0;
+
+		while (iter != NULL)
+		{
+            interval_pointer &w = iter;
+
+            Scalar w_sp = w->pseudo_x() - w->pseudo_y() * ((top_x - w->pseudo_x()) / (top_y - w->pseudo_y()));
+            Scalar distance = GEODESIC_INF;
+
+			// shortest path from the window
+			if ((w_sp - w->start() > LOCAL_EPSILON) && (w_sp - w->stop() < -LOCAL_EPSILON))
+			{
+				distance = w->d() + sqrt((top_x - w->pseudo_x()) * (top_x - w->pseudo_x()) + (top_y - w->pseudo_y()) * (top_y - w->pseudo_y()));
+				w->shortest_distance() = distance;
+			}
+			else if (w_sp - w->start() <= LOCAL_EPSILON)
+			{
+				distance = w->d() + sqrt((top_x - w->start()) * (top_x - w->start()) + top_y * top_y) + sqrt((w->start() - w->pseudo_x()) * (w->start() - w->pseudo_x()) + w->pseudo_y() * w->pseudo_y());
+				w->shortest_distance() = distance;
+				w_sp = w->start();
+			}
+			else if (w_sp - w->stop() >= -LOCAL_EPSILON)
+			{
+				distance = w->d() + sqrt((top_x - w->stop()) * (top_x - w->stop()) + top_y * top_y) + sqrt((w->stop() - w->pseudo_x()) * (w->stop() - w->pseudo_x()) + w->pseudo_y() * w->pseudo_y());
+				w->shortest_distance() = distance;
+				w_sp = w->stop();
+			}
+
+			// update information at top_t
+            if (distance < temp_geodesic)
+			{
+                temp_geodesic = distance;
+                temp_face_handle = Tri.face;
+                vertex_pointer v0 = this->mesh()->from_vertex_handle(this->mesh()->halfedge_handle(list->edge(), hfid0));
+                temp_incident_point = (list->start_vertex() == v0) ?
+                            w_sp : this->mesh()->data(list->edge()).length - w_sp;
+				wlist_sp = w_sp;
+				wlist_pseudo_x = w->pseudo_x();
+				wlist_pseudo_y = w->pseudo_y();
+			}
+			w->sp() = w_sp;
+
+			iter = iter->next();
+		}
+
+        // update top_vertex and M_VERTEX_QUEUE
+        if (temp_geodesic < this->mesh()->data(Tri.top_vertex).geodesic_distance)
+		{
+            if (this->mesh()->data(Tri.top_vertex).state == VertexState::FRONT) erase_from_queue(Tri.top_vertex);
+            this->mesh()->data(Tri.top_vertex).geodesic_distance = temp_geodesic;
+            this->mesh()->data(Tri.top_vertex).incident_face = temp_face_handle;
+            this->mesh()->data(Tri.top_vertex).incident_point = temp_incident_point;
+            if (this->mesh()->data(Tri.top_vertex).state == VertexState::FRONT)
+            {
+                m_vertex_queue.insert(Tri.top_vertex);
+            }
+
+            if ((this->mesh()->data(Tri.top_vertex).state == VertexState::INSIDE)
+                    && (this->mesh()->data(Tri.top_vertex).saddle_or_boundary))
+                create_pseudo_source_windows(Tri.top_vertex, true); // handle saddle vertex
+		}
+
+		list->sp() = wlist_sp;
+		list->pseudo_x() = wlist_pseudo_x;
+		list->pseudo_y() = wlist_pseudo_y;
+	}
+
+	inline void GeodesicAlgorithmExact::propagate_windows_to_two_edges(list_pointer &list)
+	{
+        const Scalar LOCAL_EPSILON = 1e-8 * this->mesh()->data(list->edge()).length; // numerical issue
+
+		interval_pointer iter = list->begin();
+		interval_pointer iter_t;
+
+		enum PropagationDirection
+		{
+			LEFT,
+			RIGHT,
+			BOTH
+		};
+
+		PropagationDirection direction;
+
+		while (!list->empty() && (iter != NULL))
+		{
+			interval_pointer &w = iter;
+			assert(w->start() <= w->stop());
+
+			if (w->sp() < list->sp() - LOCAL_EPSILON)
+			{
+				// only propagate to left edge
+                Scalar Intersect_X, Intersect_Y;
+
+				// judge the positions of the two windows
+				CalculateIntersectionPoint(list->pseudo_x(), list->pseudo_y(), list->sp(), 0, w->pseudo_x(), w->pseudo_y(), w->stop(), 0, Intersect_X, Intersect_Y);
+				if ((w->stop() < list->sp()) || ((Intersect_Y <= 0) && (Intersect_Y >= list->pseudo_y()) && (Intersect_Y >= w->pseudo_y())))
+				{
+					direction = PropagationDirection::LEFT;
+				}
+				else
+				{
+					direction = PropagationDirection::BOTH;
+				}				
+			}
+			else if (w->sp() > list->sp() + LOCAL_EPSILON)
+			{
+				// only propagate to right edge
+                Scalar Intersect_X, Intersect_Y;
+
+				// judge the positions of the two windows
+				CalculateIntersectionPoint(list->pseudo_x(), list->pseudo_y(), list->sp(), 0, w->pseudo_x(), w->pseudo_y(), w->start(), 0, Intersect_X, Intersect_Y);
+				if ((w->start() > list->sp())||((Intersect_Y <= 0) && (Intersect_Y >= list->pseudo_y()) && (Intersect_Y >= w->pseudo_y())))
+				{
+					direction = PropagationDirection::RIGHT;
+				}
+				else
+				{
+					direction = PropagationDirection::BOTH;
+				}	
+			}
+			else
+			{
+				// propagate to both edges
+				direction = PropagationDirection::BOTH;
+			}
+
+			bool ValidPropagation;
+			interval_pointer right_w;
+
+			switch (direction) {
+			case PropagationDirection::LEFT:
+				ValidPropagation = compute_propagated_parameters(w->pseudo_x(),
+					w->pseudo_y(),
+					w->start(),
+					w->stop(),
+					Tri.left_alpha,
+                    this->mesh()->data(Tri.left_edge).length,
+					w,
+					w->d());
+
+				iter_t = iter->next();
+				if (ValidPropagation)
+				{
+					list->erase(w);
+					wl_left.push_back(w);
+				}
+				else
+				{
+					list->erase(w);
+					delete w;
+					--m_windows_wavefront;
+				}
+				iter = iter_t;
+				break;
+
+			case PropagationDirection::RIGHT:
+                ValidPropagation = compute_propagated_parameters(this->mesh()->data(Tri.bottom_edge).length - w->pseudo_x(),
+					w->pseudo_y(),
+                    this->mesh()->data(Tri.bottom_edge).length - w->stop(),
+                    this->mesh()->data(Tri.bottom_edge).length - w->start(),
+					Tri.right_alpha,
+                    this->mesh()->data(Tri.right_edge).length,
+					w,
+					w->d());
+
+				iter_t = iter->next();
+				if (ValidPropagation)
+				{
+                    Scalar length = this->mesh()->data(Tri.right_edge).length; // invert window
+                    Scalar start = length - w->stop();
+					w->stop() = length - w->start();
+					w->start() = start;
+					w->pseudo_x() = length - w->pseudo_x();
+
+					list->erase(w);
+					wl_right.push_back(w);
+				}
+				else
+				{
+					list->erase(w);
+					delete w;
+					--m_windows_wavefront;
+				}
+				iter = iter_t;
+				break;
+
+			case PropagationDirection:: BOTH:
+				right_w = new Interval;
+				memcpy(right_w, w, sizeof(Interval));
+
+				ValidPropagation = compute_propagated_parameters(w->pseudo_x(),
+					w->pseudo_y(),
+					w->start(),
+					w->stop(),
+                    geodesic::GeodesicAlgorithmBase::vertex_angle(Tri.face, Tri.left_vertex),
+                    this->mesh()->data(Tri.left_edge).length,
+					w,
+					w->d());
+
+				iter_t = iter->next();
+				if (ValidPropagation)
+				{
+					list->erase(w);
+					wl_left.push_back(w);
+				}
+				else
+				{
+					list->erase(w);
+					delete w;
+					--m_windows_wavefront;
+				}
+				iter = iter_t;
+
+                ValidPropagation = compute_propagated_parameters(this->mesh()->data(Tri.bottom_edge).length - right_w->pseudo_x(),
+					right_w->pseudo_y(),
+                    this->mesh()->data(Tri.bottom_edge).length - right_w->stop(),
+                    this->mesh()->data(Tri.bottom_edge).length - right_w->start(),
+                    geodesic::GeodesicAlgorithmBase::vertex_angle(Tri.face, Tri.right_vertex),
+                    this->mesh()->data(Tri.right_edge).length,
+					right_w,
+					right_w->d());
+
+				if (ValidPropagation)
+				{
+					// invert window
+                    Scalar length = this->mesh()->data(Tri.right_edge).length;
+                    Scalar start = length - right_w->stop();
+					right_w->stop() = length - right_w->start();
+					right_w->start() = start;
+					right_w->pseudo_x() = length - right_w->pseudo_x();
+
+					wl_right.push_back(right_w);
+
+					++m_windows_wavefront;
+					if (m_windows_peak < m_windows_wavefront)
+						m_windows_peak = m_windows_wavefront;
+				}
+				else
+				{
+					delete right_w;
+				}
+				break;
+
+			default:
+				break;
+			}
+		}
+	}
+
+	//----------------- pairwise windows checking (Rule 2) ----------------------
+	inline void GeodesicAlgorithmExact::check_with_vertices(list_pointer &list)
+    {
+		if (list->empty()) return;
+
+		interval_pointer iter = list->begin();
+		interval_pointer iter_t;
+
+		while ((!list->empty()) && (iter != NULL))
+		{
+			interval_pointer &w = iter;
+			bool w_survive = true;
+
+            edge_pointer   e = list->edge();
+			vertex_pointer v1 = list->start_vertex();
+            vertex_pointer v2 = opposite_vertex(e, v1);
+            Scalar d1 = GEODESIC_INF;
+
+			d1 = w->d() + sqrt((w->stop() - w->pseudo_x()) * (w->stop() - w->pseudo_x()) + w->pseudo_y() * w->pseudo_y());
+            if (this->mesh()->data(v1).geodesic_distance + w->stop() < d1)
+				w_survive = false;
+
+			d1 = w->d() + sqrt((w->start() - w->pseudo_x()) * (w->start() - w->pseudo_x()) + w->pseudo_y() * w->pseudo_y());
+            if (this->mesh()->data(v2).geodesic_distance + this->mesh()->data(e).length - w->start() < d1)
+				w_survive = false;
+
+
+			iter_t = iter;
+			iter = iter->next();
+
+			if (!w_survive)
+			{
+				list->erase(iter_t);
+				delete iter_t;
+				--m_windows_wavefront;
+			}
+		}
+	}
+
+	inline windows_state GeodesicAlgorithmExact::check_between_two_windows(interval_pointer &w1, interval_pointer &w2)
+	{
+        Scalar NUMERCIAL_EPSILON = 1 - 1e-12;
+		// we implement the discussed 6 cases as follows for simplicity
+
+		if ((w1->start() >= w2->start()) && (w1->start() <= w2->stop())) // w1->start
+		{
+            Scalar Intersect_X, Intersect_Y;
+
+			// judge the order of the two windows
+			CalculateIntersectionPoint(w2->pseudo_x(), w2->pseudo_y(), w1->start(), 0, w1->pseudo_x(), w1->pseudo_y(), w1->stop(), 0, Intersect_X, Intersect_Y);
+
+			if ((Intersect_Y <= 0) && (Intersect_Y >= w1->pseudo_y()) && (Intersect_Y >= w2->pseudo_y()))
+			{
+                Scalar d1, d2;
+				d1 = w1->d() + sqrt((w1->start() - w1->pseudo_x()) * (w1->start() - w1->pseudo_x()) + (w1->pseudo_y()) * (w1->pseudo_y()));
+				d2 = w2->d() + sqrt((w1->start() - w2->pseudo_x()) * (w1->start() - w2->pseudo_x()) + (w2->pseudo_y()) * (w2->pseudo_y()));
+
+				if (d2 < d1 * NUMERCIAL_EPSILON)
+					return w1_invalid;
+				if (d1 < d2 * NUMERCIAL_EPSILON)
+					w2->start() = w1->start();
+			}
+		}
+
+		if ((w1->stop() >= w2->start()) && (w1->stop() <= w2->stop())) // w1->stop
+		{
+            Scalar Intersect_X, Intersect_Y;
+
+			// judge the order of the two windows
+			CalculateIntersectionPoint(w2->pseudo_x(), w2->pseudo_y(), w1->stop(), 0, w1->pseudo_x(), w1->pseudo_y(), w1->start(), 0, Intersect_X, Intersect_Y);
+
+			if ((Intersect_Y <= 0) && (Intersect_Y >= w1->pseudo_y()) && (Intersect_Y >= w2->pseudo_y()))
+			{
+                Scalar d1, d2;
+				d1 = w1->d() + sqrt((w1->stop() - w1->pseudo_x()) * (w1->stop() - w1->pseudo_x()) + (w1->pseudo_y()) * (w1->pseudo_y()));
+				d2 = w2->d() + sqrt((w1->stop() - w2->pseudo_x()) * (w1->stop() - w2->pseudo_x()) + (w2->pseudo_y()) * (w2->pseudo_y()));
+
+				if (d2 < d1 * NUMERCIAL_EPSILON)
+					return w1_invalid;
+				if (d1 < d2 * NUMERCIAL_EPSILON)
+					w2->stop() = w1->stop();
+			}
+		}
+
+		if ((w2->start() >= w1->start()) && (w2->start() <= w1->stop())) // w2->start
+		{
+            Scalar Intersect_X, Intersect_Y;
+
+			// judge the previous order of the two windows
+			CalculateIntersectionPoint(w1->pseudo_x(), w1->pseudo_y(), w2->start(), 0, w2->pseudo_x(), w2->pseudo_y(), w2->stop(), 0, Intersect_X, Intersect_Y);
+
+			if ((Intersect_Y <= 0) && (Intersect_Y >= w1->pseudo_y()) && (Intersect_Y >= w2->pseudo_y()))
+			{
+                Scalar d1, d2;
+				d1 = w1->d() + sqrt((w2->start() - w1->pseudo_x()) * (w2->start() - w1->pseudo_x()) + (w1->pseudo_y()) * (w1->pseudo_y()));
+				d2 = w2->d() + sqrt((w2->start() - w2->pseudo_x()) * (w2->start() - w2->pseudo_x()) + (w2->pseudo_y()) * (w2->pseudo_y()));
+
+				if (d1 < d2 * NUMERCIAL_EPSILON)
+					return w2_invalid;
+				if (d2 < d1 * NUMERCIAL_EPSILON)
+					w1->start() = w2->start();
+			}
+		}
+
+		if ((w2->stop() >= w1->start()) && (w2->stop() <= w1->stop())) // w2->stop
+		{
+            Scalar Intersect_X, Intersect_Y;
+
+			// judge the previous order of the two windows
+			CalculateIntersectionPoint(w1->pseudo_x(), w1->pseudo_y(), w2->stop(), 0, w2->pseudo_x(), w2->pseudo_y(), w2->start(), 0, Intersect_X, Intersect_Y);
+
+			if ((Intersect_Y <= 0) && (Intersect_Y >= w1->pseudo_y()) && (Intersect_Y >= w2->pseudo_y()))
+			{
+                Scalar d1, d2;
+				d1 = w1->d() + sqrt((w2->stop() - w1->pseudo_x()) * (w2->stop() - w1->pseudo_x()) + (w1->pseudo_y()) * (w1->pseudo_y()));
+				d2 = w2->d() + sqrt((w2->stop() - w2->pseudo_x()) * (w2->stop() - w2->pseudo_x()) + (w2->pseudo_y()) * (w2->pseudo_y()));
+
+				if (d1 < d2 * NUMERCIAL_EPSILON)
+					return w2_invalid;
+				if (d2 < d1 * NUMERCIAL_EPSILON)
+					w1->stop() = w2->stop();
+			}
+		}
+
+		if (w1->start() >= w2->stop())
+		{
+            Scalar Intersect_X, Intersect_Y;
+
+			// judge the previous order of the two windows
+			CalculateIntersectionPoint(w1->pseudo_x(), w1->pseudo_y(), w1->start(), 0, w2->pseudo_x(), w2->pseudo_y(), w2->stop(), 0, Intersect_X, Intersect_Y);
+
+            face_pointer f = opposite_face(Tri.bottom_edge, Tri.face);
+            edge_pointer e = next_edge(f, Tri.bottom_edge, Tri.left_vertex);
+            Scalar angle = vertex_angle(f, Tri.left_vertex);
+            Scalar Cx = this->mesh()->data(e).length * cos(angle);
+            Scalar Cy = this->mesh()->data(e).length * -sin(angle);
+
+            if ((PointInTriangle(Intersect_X, Intersect_Y, this->mesh()->data(Tri.bottom_edge).length, Cx, Cy))
+				&& (Intersect_Y <= 0) && (Intersect_Y >= w1->pseudo_y()) && (Intersect_Y >= w2->pseudo_y()))
+			{
+                Scalar d1, d2;
+				d1 = w1->d() + sqrt((Intersect_X - w1->pseudo_x()) * (Intersect_X - w1->pseudo_x()) + (Intersect_Y - w1->pseudo_y()) * (Intersect_Y - w1->pseudo_y()));
+				d2 = w2->d() + sqrt((Intersect_X - w2->pseudo_x()) * (Intersect_X - w2->pseudo_x()) + (Intersect_Y - w2->pseudo_y()) * (Intersect_Y - w2->pseudo_y()));
+
+				if (d1 < d2 * NUMERCIAL_EPSILON)
+					return w2_invalid;
+				if (d2 < d1 * NUMERCIAL_EPSILON)
+					return w1_invalid;
+			}
+		}
+
+		if (w2->start() >= w1->stop())
+		{
+            Scalar Intersect_X, Intersect_Y;
+
+			// judge the previous order of the two windows
+			CalculateIntersectionPoint(w2->pseudo_x(), w2->pseudo_y(), w2->start(), 0, w1->pseudo_x(), w1->pseudo_y(), w1->stop(), 0, Intersect_X, Intersect_Y);
+
+            face_pointer f = opposite_face(Tri.bottom_edge, Tri.face);
+            edge_pointer e = next_edge(f, Tri.bottom_edge, Tri.left_vertex);
+            Scalar angle = vertex_angle(f, Tri.left_vertex);
+            Scalar Cx = this->mesh()->data(e).length * cos(angle);
+            Scalar Cy = this->mesh()->data(e).length * -sin(angle);
+
+            if ((PointInTriangle(Intersect_X, Intersect_Y, this->mesh()->data(Tri.bottom_edge).length, Cx, Cy))
+				&& (Intersect_Y <= 0) && (Intersect_Y >= w1->pseudo_y()) && (Intersect_Y >= w2->pseudo_y()))
+			{
+                Scalar d1, d2;
+				d1 = w1->d() + sqrt((Intersect_X - w1->pseudo_x()) * (Intersect_X - w1->pseudo_x()) + (Intersect_Y - w1->pseudo_y()) * (Intersect_Y - w1->pseudo_y()));
+				d2 = w2->d() + sqrt((Intersect_X - w2->pseudo_x()) * (Intersect_X - w2->pseudo_x()) + (Intersect_Y - w2->pseudo_y()) * (Intersect_Y - w2->pseudo_y()));
+
+				if (d1 < d2 - NUMERCIAL_EPSILON)
+					return w2_invalid;
+				if (d2 < d1 - NUMERCIAL_EPSILON)
+					return w1_invalid;
+			}
+		}
+
+		return both_valid;
+	}
+
+	inline void GeodesicAlgorithmExact::pairwise_windows_checking(list_pointer &list)
+	{
+		if (list->empty()) return;
+
+		interval_pointer iter = list->begin();
+		interval_pointer next, iter_t;
+
+		next = iter->next();
+
+		// traverse successive pairs of windows
+		while ((!list->empty()) && (next != NULL))
+		{
+			windows_state ws = check_between_two_windows(iter, next);
+
+			switch (ws)
+			{
+			case geodesic::w1_invalid:
+				iter_t = iter;
+				if (iter == list->begin())
+				{
+					iter = iter->next();
+				}
+				else
+				{
+					iter = iter->previous();
+				}
+
+				list->erase(iter_t);
+				delete iter_t;
+				--m_windows_wavefront;
+				break;
+
+			case geodesic::w2_invalid:
+				list->erase(next);
+				delete next;
+				--m_windows_wavefront;
+				break;
+
+			case geodesic::both_valid:
+				iter = iter->next();
+				break;
+
+			default:
+				break;
+			}
+
+			next = iter->next();
+		}
+	}
+
+	//------------------------- main operation ----------------------------
+	inline void GeodesicAlgorithmExact::propagate_one_windows_list(list_pointer &list)
+	{
+		if (list->empty()) return;
+        OpenMesh::HalfedgeHandle hf0 = this->mesh()->halfedge_handle(list->edge(), hfid0);
+        OpenMesh::HalfedgeHandle hf1 = this->mesh()->halfedge_handle(list->edge(), hfid1);
+        if (this->mesh()->face_handle(hf0).idx()>-1 && this->mesh()->face_handle(hf1).idx()>-1)
+        {
+			// Rule 2: pairwise windows checking
+            check_with_vertices(list);
+			pairwise_windows_checking(list);
+
+            // Rule 1: "One Angle Two Sides"
+            find_separating_point(list);
+            propagate_windows_to_two_edges(list);
+		}
+	}
+
+	//-------------------------- main entry --------------------------
+    inline void GeodesicAlgorithmExact::propagate(unsigned source, std::vector<size_t>& idxs)
+	{
+		// initialization
+        m_source = SubVidxfromMesh[source];
+		initialize_propagation_data();
+		while (!m_vertex_queue.empty())
+		{
+			// (1) pop a vertex from M_VERTEX_QUEUE
+            vertex_pointer vert = *m_vertex_queue.begin();
+			m_vertex_queue.erase(m_vertex_queue.begin());
+
+			// (2) update wavefront
+            this->mesh()->data(vert).state = VertexState::INSIDE;
+            for(auto e_it = this->mesh()->ve_begin(vert); e_it != this->mesh()->ve_end(vert); ++e_it)
+			{
+                vertex_pointer vert_it = opposite_vertex(*e_it, vert);
+                if (this->mesh()->data(vert_it).state == VertexState::OUTSIDE)
+                    this->mesh()->data(vert_it).state = VertexState::FRONT;
+			}
+
+			// (3) handle saddle vertex
+            if (this->mesh()->data(vert).saddle_or_boundary) create_pseudo_source_windows(vert, false);
+
+            // (4) push window lists on the wavefront incident to v into M_LIST_QUEUE
+            for(auto e_it = this->mesh()->ve_begin(vert); e_it != this->mesh()->ve_end(vert); ++e_it)
+			{
+                edge_pointer edge_it = *e_it;
+                if (!interval_list_0(edge_it)->empty())
+                {
+                    m_list_queue.push(interval_list_0(edge_it));
+                }
+                if (!interval_list_1(edge_it)->empty())
+                {
+                    m_list_queue.push(interval_list_1(edge_it));
+                }
+			}
+
+
+            for(auto f_it = this->mesh()->vf_begin(vert); f_it != this->mesh()->vf_end(vert); ++f_it)
+			{
+                edge_pointer   edge_it = opposite_edge(*f_it, vert);
+                bool two_adjface = (this->mesh()->face_handle(this->mesh()->halfedge_handle(edge_it, hfid0)).idx()>-1)
+                        && (this->mesh()->face_handle(this->mesh()->halfedge_handle(edge_it, hfid1)).idx()>-1);
+                vertex_pointer vert_it;
+                if(two_adjface)
+                {
+                    face_pointer faceid = opposite_face(edge_it, *f_it);
+                    vert_it = opposite_vertex(faceid, edge_it);
+                }
+                if (!two_adjface || (this->mesh()->data(vert_it).state != VertexState::OUTSIDE))
+                {
+                    if (!interval_list_0(edge_it)->empty())
+                    {
+                        m_list_queue.push(interval_list_0(edge_it));
+                    }
+                    if (!interval_list_1(edge_it)->empty())
+                    {
+                        m_list_queue.push(interval_list_1(edge_it));
+                    }
+                }
+			}
+
+
+			// (5) propagate window lists in a FIFO order
+			while (!m_list_queue.empty())
+			{
+				// pop an list from M_LIST_QUEUE
+                list_pointer list = m_list_queue.front();
+
+				m_list_queue.pop();
+
+                bool is_boundary = calculate_triangle_parameters(list, Tri);
+				if (!is_boundary)
+				{
+					// propagate the window list using Rule 1 and 2
+					wl_left.clear(); wl_right.clear();
+					propagate_one_windows_list(list);
+
+					// merge windows lists
+					if (!wl_left.empty())
+					{
+						// in VTP, both "PrimeMerge" and "SecondMerge" connect window lists in an order-free way
+						if (!Tri.left_list->empty())
+						{
+							Tri.left_list->begin()->previous() = wl_left.end();
+							wl_left.end()->next() = Tri.left_list->begin();
+                            Tri.left_list->begin() = wl_left.begin();
+						}
+						else
+						{
+							Tri.left_list->begin() = wl_left.begin();
+                            Tri.left_list->end() = wl_left.end();
+						}
+
+						// push updated list into M_LIST_QUEUE
+                        vertex_pointer v0 = this->mesh()->from_vertex_handle(this->mesh()->halfedge_handle(Tri.left_edge, hfid0));
+                        vertex_pointer v1 = this->mesh()->from_vertex_handle(this->mesh()->halfedge_handle(Tri.left_edge, hfid1));
+                        if (((this->mesh()->data(v0).state == VertexState::INSIDE)
+                             || (this->mesh()->data(v1).state == VertexState::INSIDE))
+                                && (!Tri.left_list->empty()))
+                        {
+							m_list_queue.push(Tri.left_list);
+                        }
+					}
+
+					if (!wl_right.empty())
+					{
+						// in VTP, both "PrimeMerge" and "SecondMerge" connect window lists in an order-free way
+						if (!Tri.right_list->empty())
+						{
+							Tri.right_list->end()->next() = wl_right.begin();
+							wl_right.begin()->previous() = Tri.right_list->end();
+							Tri.right_list->end() = wl_right.end();
+						}
+						else
+						{
+							Tri.right_list->begin() = wl_right.begin();
+							Tri.right_list->end() = wl_right.end();
+						}
+
+						// push updated list into M_LIST_QUEUE
+                        vertex_pointer v0 = this->mesh()->from_vertex_handle(this->mesh()->halfedge_handle(Tri.right_edge, hfid0));
+                        vertex_pointer v1 = this->mesh()->from_vertex_handle(this->mesh()->halfedge_handle(Tri.right_edge, hfid1));
+                        if (((this->mesh()->data(v0).state == VertexState::INSIDE)
+                             || (this->mesh()->data(v1).state == VertexState::INSIDE)) && (!Tri.right_list->empty()))
+							m_list_queue.push(Tri.right_list);
+					}
+				}
+
+				list->clear();
+			}
+			// statistics
+			if (m_vertex_queue.size() > m_queue_max_size)
+				m_queue_max_size = m_vertex_queue.size();
+		}
+        idxs.clear();
+        for(auto v_it = this->mesh()->vertices_begin(); v_it != this->mesh()->vertices_end(); ++v_it)
+        {
+            idxs.push_back(MeshVidxfromSub[v_it->idx()]);
+            this->ori_mesh->data(this->ori_mesh->vertex_handle(MeshVidxfromSub[v_it->idx()])).geodesic_distance
+                    = this->mesh()->data(*v_it).geodesic_distance;
+        }
+	}
+
+    // construct sub mesh
+    inline bool GeodesicAlgorithmExact::construct_submesh(Mesh* sub_mesh, size_t source_idx, Scalar radius)
+    {
+        std::queue<size_t> vertexlist;
+        vertexlist.push(source_idx);
+        Vec3 srcp = ori_mesh->point(ori_mesh->vertex_handle(source_idx));
+        std::vector<bool> visited(ori_mesh->n_vertices(), false);
+        std::vector<bool> added_face(ori_mesh->n_faces(), false);
+        SubVidxfromMesh.resize(ori_mesh->n_vertices());
+        SubVidxfromMesh.setConstant(-1);
+        MeshVidxfromSub.clear();
+        visited[source_idx] = true;
+        while(!vertexlist.empty())
+        {
+            size_t vidx = vertexlist.front();
+            vertexlist.pop();
+            OpenMesh::VertexHandle vh = ori_mesh->vertex_handle(vidx);
+            Vec3 vp = ori_mesh->point(vh);
+            if((srcp - vp).norm() < radius)
+            {
+                vertex_pointer new_v = sub_mesh->add_vertex(vp);
+                SubVidxfromMesh[vh.idx()] = new_v.idx();
+                MeshVidxfromSub.push_back(vh.idx());
+                for(auto vv_it = ori_mesh->vv_begin(vh); vv_it != ori_mesh->vv_end(vh); vv_it++)
+                {
+                    if(!visited[vv_it->idx()])
+                    {
+                        vertexlist.push(vv_it->idx());
+                        visited[vv_it->idx()] = true;
+                    }
+                }
+                for(auto vf_it = ori_mesh->vf_begin(vh); vf_it != ori_mesh->vf_end(vh); vf_it++)
+                {
+                    halfedge_handle hf = ori_mesh->halfedge_handle(*vf_it);
+                    if(!added_face[vf_it->idx()])
+                    {
+                        vertex_pointer vh = ori_mesh->from_vertex_handle(hf);
+                        vertex_pointer nextv = ori_mesh->to_vertex_handle(hf);
+                        vertex_pointer thirdv = ori_mesh->to_vertex_handle(ori_mesh->next_halfedge_handle(hf));
+                        if(SubVidxfromMesh[vh.idx()] >= 0
+                            && SubVidxfromMesh[nextv.idx()] >= 0
+                            && SubVidxfromMesh[thirdv.idx()] >= 0)
+                        {
+                            std::vector<vertex_pointer> vertices;
+                            vertices.push_back(sub_mesh->vertex_handle(SubVidxfromMesh[vh.idx()]));
+                            vertices.push_back(sub_mesh->vertex_handle(SubVidxfromMesh[nextv.idx()]));
+                            vertices.push_back(sub_mesh->vertex_handle(SubVidxfromMesh[thirdv.idx()]));
+                            sub_mesh->add_face(vertices);
+                            added_face[vf_it->idx()] = true;
+                        }
+                    }
+                }
+            }
+        }
+
+        sub_mesh->delete_isolated_vertices();
+        sub_mesh->garbage_collection();
+        if(sub_mesh->n_vertices() > 0)
+            return true;
+        else
+            return false;
+    }
+
+	//---------------------- print statistics --------------------------
+	inline void GeodesicAlgorithmExact::print_statistics()
+	{
+		GeodesicAlgorithmBase::print_statistics();
+
+        Scalar memory = sizeof(Interval);
+
+		//std::cout << std::endl;
+		std::cout << "Peak number of intervals on wave-front " << m_windows_peak << std::endl;
+		std::cout << "uses about " << memory * m_windows_peak / 1e6 << "MB of memory" << std::endl;
+		std::cout << "total interval propagation number " << m_windows_propagation << std::endl;
+		std::cout << "maximum interval queue size is " << m_queue_max_size << std::endl;
+	}
+}		//geodesic
+#endif

cpp/tools/geodesic/geodesic_algorithm_exact_elements.h

@@ -0,0 +1,152 @@
+#ifndef GEODESIC_ALGORITHM_EXACT_ELEMENTS
+#define GEODESIC_ALGORITHM_EXACT_ELEMENTS
+
+#include "../tools/Types.h"
+
+namespace geodesic {
+
+	class Interval;
+	class IntervalList;
+	typedef Interval* interval_pointer;
+    typedef IntervalList* list_pointer;
+    typedef OpenMesh::FaceHandle face_pointer;
+    typedef OpenMesh::EdgeHandle edge_pointer;
+    typedef OpenMesh::VertexHandle vertex_pointer;
+    typedef OpenMesh::HalfedgeHandle halfedge_handle;
+
+	struct Triangle // Components of a face to be propagated
+	{
+        face_pointer face; // Current Face
+
+        edge_pointer bottom_edge, // Edges
+			left_edge,
+			right_edge;
+
+        vertex_pointer top_vertex, // Vertices
+			left_vertex,
+			right_vertex;
+
+        Scalar top_alpha,
+			left_alpha,
+			right_alpha; // Angles
+
+		list_pointer left_list,
+			right_list; // Lists
+	};
+
+	class Interval						//interval of the edge
+	{
+	public:
+
+		Interval() {};
+		~Interval() {};
+
+        Scalar& start() { return m_start; };
+        Scalar& stop() { return m_stop; };
+        Scalar& d() { return m_d; };
+        Scalar& pseudo_x() { return m_pseudo_x; };
+        Scalar& pseudo_y() { return m_pseudo_y; };
+
+        Scalar& sp() { return m_sp; };
+
+        Scalar& shortest_distance() { return m_shortest_distance; }
+
+		interval_pointer& next() { return m_next; };
+		interval_pointer& previous() { return m_previous; };
+
+	private:
+        Scalar m_start;						//initial point of the interval on the edge
+        Scalar m_stop;
+        Scalar m_d;							//distance from the source to the pseudo-source
+        Scalar m_pseudo_x;					//coordinates of the pseudo-source in the local coordinate system
+        Scalar m_pseudo_y;					//y-coordinate should be always negative
+
+        Scalar m_sp;                        //separating point
+
+        Scalar m_shortest_distance;         //shortest distance from the interval to top_vertex, for numerical precision issue
+
+		interval_pointer m_next;			//pointer to the next interval in the list	
+		interval_pointer m_previous;        //pointer to the previous interval in the list
+	};
+
+	class IntervalList						//list of the of intervals of the given edge
+	{
+	public:
+        IntervalList() {  /*m_start = NULL; m_edge = NULL;*/ m_sp = -1; m_begin = m_end = NULL; }
+		~IntervalList() {};
+
+		void clear() { m_begin = m_end = NULL; }
+        void initialize(edge_pointer e) { m_edge = e; }
+
+        vertex_pointer& start_vertex() { return m_start; }
+        edge_pointer& edge() { return m_edge; }
+
+        Scalar& sp() { return m_sp; };
+
+        Scalar& pseudo_x() { return m_pseudo_x; };
+        Scalar& pseudo_y() { return m_pseudo_y; };
+
+		// List operation
+		interval_pointer& begin() { return m_begin; }
+
+		interval_pointer& end() { return m_end; }
+
+		bool empty() { return m_begin == NULL; }
+
+		void push_back(interval_pointer & w)
+		{
+			if (!m_end)
+			{
+				w->previous() = NULL;
+				w->next() = NULL;
+				m_begin = m_end = w;
+			}
+			else
+			{
+				w->next() = NULL;
+				w->previous() = m_end;
+				m_end->next() = w;
+				m_end = w;
+			}
+		}
+
+		void erase(interval_pointer & w)
+		{
+			if ((w == m_begin) && (w == m_end))
+			{
+				m_begin = m_end = NULL;
+			}
+			else if (w == m_begin)
+			{
+				m_begin = m_begin->next();
+				m_begin->previous() = NULL;
+			}
+			else if (w == m_end)
+			{
+				m_end = m_end->previous();
+				m_end->next() = NULL;
+			}
+			else
+			{
+				w->previous()->next() = w->next();
+				w->next()->previous() = w->previous();
+			}
+		}
+
+	private:
+
+        edge_pointer     m_edge;		    //edge that owns this list
+        vertex_pointer   m_start;           //vertex from which the interval list starts
+
+		interval_pointer m_begin;
+		interval_pointer m_end;
+
+        Scalar m_pseudo_x;
+        Scalar m_pseudo_y;
+
+        Scalar m_sp;                        //separating point
+	};
+
+}		//geodesic
+
+#endif

cpp/tools/geodesic/geodesic_constants_and_simple_functions.h

@@ -0,0 +1,153 @@
+#ifndef GEODESIC_CONSTANTS
+#define GEODESIC_CONSTANTS
+
+// some constants and simple math functions
+#include "../tools/Types.h"
+namespace geodesic{
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+Scalar const GEODESIC_INF = 1e100;
+
+// statistics
+Scalar m_time_consumed;		//how much time does the propagation step takes
+unsigned m_queue_max_size;			//used for statistics
+unsigned m_windows_propagation; // how many time a window is propagated
+unsigned m_windows_wavefront; // the number of windows on the wavefront
+unsigned m_windows_peak; // the maximum number of windows, used to calculate the memory
+
+// two windows' states after checking
+enum windows_state
+{
+	w1_invalid,
+	w2_invalid,
+	both_valid
+};
+
+inline Scalar cos_from_edges(Scalar const a,			//compute the cosine of the angle given the lengths of the edges
+                             Scalar const b,
+                             Scalar const c)
+{
+	assert(a > 1e-50);
+	assert(b > 1e-50);
+	assert(c > 1e-50);
+
+    Scalar result = (b * b + c * c - a * a)/(2.0 * b * c);
+    result = result>-1.0?result:-1.0;
+    return result <1.0 ? result:1.0;
+}
+
+inline Scalar angle_from_edges(Scalar const a,			//compute the cosine of the angle given the lengths of the edges
+                               Scalar const b,
+                               Scalar const c)
+{
+	return acos(cos_from_edges(a, b, c));
+}
+
+template<class Points, class Faces>
+inline bool read_mesh_from_file(char* filename,
+								Points& points,
+								Faces& faces, 
+								std::vector<int> &realIndex, 
+								int& originalVertNum)
+{
+	std::ifstream file(filename);
+	assert(file.is_open());
+	if(!file.is_open()) return false;
+
+	char type;
+	std::string curLine;
+    Scalar coord[3];
+	unsigned int vtxIdx[3];
+	std::map<std::string, int> mapForDuplicate;
+	originalVertNum = 0;
+
+	while(getline(file, curLine))
+	{
+		if (curLine.size() < 2) continue;
+		if (curLine[0] == 'v' && curLine[1] != 't')
+		{
+			std::map<std::string, int>::iterator pos = mapForDuplicate.find(curLine);
+			if (pos == mapForDuplicate.end())
+			{
+				int oldSize = mapForDuplicate.size();
+				realIndex.push_back(oldSize);
+				mapForDuplicate[curLine] = oldSize;
+                sscanf(curLine.c_str(), "v %lf %lf %lf", &coord[0], &coord[1], &coord[2]);
+				for (int i = 0;i < 3;++i) points.push_back(coord[i]);
+			}
+			else
+			{
+				realIndex.push_back(pos->second);
+			}
+			++originalVertNum;
+		}
+		else if (curLine[0] == 'f')
+		{
+			unsigned tex;
+			if (curLine.find('/') != std::string::npos)
+                sscanf(curLine.c_str(), "f %d/%d %d/%d %d/%d", &vtxIdx[0], &tex, &vtxIdx[1], &tex, &vtxIdx[2], &tex);
+			else
+                sscanf(curLine.c_str(), "f %d %d %d", &vtxIdx[0], &vtxIdx[1], &vtxIdx[2]);
+			
+			vtxIdx[0] = realIndex[vtxIdx[0]-1];
+			vtxIdx[1] = realIndex[vtxIdx[1]-1];
+			vtxIdx[2] = realIndex[vtxIdx[2]-1];
+			if (vtxIdx[0] == vtxIdx[1] || vtxIdx[0] == vtxIdx[2] || vtxIdx[1] == vtxIdx[2]) continue;
+
+			for (int i = 0;i < 3;++i) faces.push_back(vtxIdx[i]);
+		}
+	}
+	file.close();
+
+	printf("There are %d non-coincidence vertices.\n", points.size() / 3);
+
+	return true;
+}
+
+
+
+inline void CalculateIntersectionPoint(Scalar X1, Scalar Y1, // compute intersection point of two windows
+    Scalar X2, Scalar Y2,
+    Scalar X3, Scalar Y3,
+    Scalar X4, Scalar Y4,
+    Scalar &Intersect_X, Scalar &Intersect_Y)
+{
+    Scalar A1, B1, C1, A2, B2, C2;
+	A1 = Y2 - Y1;
+	B1 = X1 - X2;
+	C1 = X2 * Y1 - X1 * Y2;
+	A2 = Y4 - Y3;
+	B2 = X3 - X4;
+	C2 = X4 * Y3 - X3 * Y4;
+
+	Intersect_X = (B2 * C1 - B1 * C2) / (A2 * B1 - A1 * B2);
+	Intersect_Y = (A1 * C2 - A2 * C1) / (A2 * B1 - A1 * B2);
+}
+
+
+inline bool PointInTriangle(Scalar &X, Scalar &Y, // judge if a point is inside a triangle
+    //Scalar Ax, Scalar Ay, // 0, 0
+    Scalar &Bx, //Scalar By, // By = 0
+    Scalar &Cx, Scalar &Cy)
+{
+    Scalar dot00 = Cx * Cx + Cy * Cy;// dot00 = dot(v0, v0)
+    Scalar dot01 = Cx * Bx;// dot01 = dot(v0, v1)
+    Scalar dot02 = Cx * X + Cy * Y;// dot02 = dot(v0, v2)
+    Scalar dot11 = Bx * Bx; // dot11 = dot(v1, v1)
+    Scalar dot12 = Bx * X;  // dot12 = dot(v1, v2)
+
+    Scalar invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
+    Scalar u = (dot11 * dot02 - dot01 * dot12) * invDenom;
+    Scalar v = (dot00 * dot12 - dot01 * dot02) * invDenom;
+
+	//return (u >= 0) && (v >= 0) && (u + v < 1);
+	return (u >= 1e-10) && (v >= 1e-10) && (u + v < 1 - 1e-10);
+}
+
+
+} //geodesic
+
+#endif	//GEODESIC_CONSTANTS_20071231

cpp/tools/geodesic/geodesic_memory.h

@@ -0,0 +1,192 @@
+#ifndef _GEODESIC_MEMORY
+#define _GEODESIC_MEMORY
+
+//two fast and simple memory allocators
+
+#include "../tools/Types.h"
+#include <memory>
+
+namespace geodesic{
+
+template<class T>			//quickly allocates multiple elements of a given type; no deallocation
+class SimlpeMemoryAllocator
+{
+public:
+	typedef T* pointer;
+
+	SimlpeMemoryAllocator(unsigned block_size = 0, 
+						  unsigned max_number_of_blocks = 0)
+	{
+		reset(block_size, 
+			  max_number_of_blocks);
+	};
+
+	~SimlpeMemoryAllocator(){};
+
+	void reset(unsigned block_size, 
+			   unsigned max_number_of_blocks)
+	{
+		m_block_size = block_size;
+		m_max_number_of_blocks = max_number_of_blocks;
+
+
+		m_current_position = 0;
+
+		m_storage.reserve(max_number_of_blocks);
+		m_storage.resize(1);
+		m_storage[0].resize(block_size);
+	};
+
+	pointer allocate(unsigned const n)		//allocate n units
+	{
+		assert(n < m_block_size);
+
+		if(m_current_position + n >= m_block_size)
+		{
+			m_storage.push_back( std::vector<T>() );
+			m_storage.back().resize(m_block_size);
+			m_current_position = 0;
+		}
+		pointer result = & m_storage.back()[m_current_position];
+		m_current_position += n;
+
+		return result;
+	};
+private:
+	std::vector<std::vector<T> > m_storage;	
+	unsigned m_block_size;				//size of a single block
+	unsigned m_max_number_of_blocks;		//maximum allowed number of blocks
+	unsigned m_current_position;			//first unused element inside the current block
+};
+
+
+template<class T>		//quickly allocates and deallocates single elements of a given type
+class MemoryAllocator
+{
+public:
+	typedef T* pointer;
+
+	MemoryAllocator(unsigned block_size = 1024, 
+				    unsigned max_number_of_blocks = 1024)
+	{
+		reset(block_size, 
+			  max_number_of_blocks);
+	};
+
+	~MemoryAllocator(){};
+
+	void clear()
+	{
+		reset(m_block_size, 
+			  m_max_number_of_blocks);
+	}
+
+	void reset(unsigned block_size, 
+			   unsigned max_number_of_blocks)
+	{
+		m_block_size = block_size;
+		m_max_number_of_blocks = max_number_of_blocks;
+
+		assert(m_block_size > 0);
+		assert(m_max_number_of_blocks > 0);
+
+		m_current_position = 0;
+
+		m_storage.reserve(max_number_of_blocks);
+		m_storage.resize(1);
+		m_storage[0].resize(block_size);
+
+		m_deleted.clear();
+		m_deleted.reserve(2*block_size);
+	};
+
+	pointer allocate()		//allocates single unit of memory
+	{
+		pointer result;
+		if(m_deleted.empty())
+		{
+			if(m_current_position + 1 >= m_block_size)
+			{
+				m_storage.push_back( std::vector<T>() );
+				m_storage.back().resize(m_block_size);
+				m_current_position = 0;
+			}
+			result = & m_storage.back()[m_current_position];
+			++m_current_position;
+		}
+		else
+		{
+			result = m_deleted.back();
+			m_deleted.pop_back();
+		}
+
+		return result;
+	};
+
+	void deallocate(pointer p)		//allocate n units
+	{
+		if(m_deleted.size() < m_deleted.capacity())
+		{
+			m_deleted.push_back(p);
+		}
+	};
+
+private:
+	std::vector<std::vector<T> > m_storage;
+	unsigned m_block_size;				//size of a single block
+	unsigned m_max_number_of_blocks;		//maximum allowed number of blocks
+	unsigned m_current_position;			//first unused element inside the current block
+
+	std::vector<pointer> m_deleted;			//pointers to deleted elemets
+};
+
+
+class OutputBuffer
+{
+public:
+	OutputBuffer():
+		m_num_bytes(0)
+	{}
+
+	void clear()
+	{
+		m_num_bytes = 0;
+        m_buffer = std::auto_ptr<Scalar>();
+	}
+
+	template<class T>
+	T* allocate(unsigned n)
+	{
+        Scalar wanted = n*sizeof(T);
+		if(wanted > m_num_bytes)
+		{
+            unsigned new_size = (unsigned) ceil(wanted / (Scalar)sizeof(Scalar));
+            m_buffer = std::auto_ptr<Scalar>(new Scalar[new_size]);
+            m_num_bytes = new_size*sizeof(Scalar);
+		}
+
+		return (T*)m_buffer.get();
+	}
+
+	template <class T>
+	T* get()
+	{
+		return (T*)m_buffer.get();
+	}
+
+	template<class T>
+	unsigned capacity()
+	{
+        return (unsigned)floor((Scalar)m_num_bytes/(Scalar)sizeof(T));
+	};
+
+private:
+
+    std::auto_ptr<Scalar> m_buffer;
+	unsigned m_num_bytes;
+};
+
+
+} //geodesic
+
+#endif

cpp/tools/median.h

@@ -0,0 +1,61 @@
+#ifndef MEDIAN_H
+#define MEDIAN_H
+// This file is part of libigl, a simple c++ geometry processing library.
+// 
+// Copyright (C) 2013 Alec Jacobson <[email protected]>
+// 
+// This Source Code Form is subject to the terms of the Mozilla Public License 
+// v. 2.0. If a copy of the MPL was not distributed with this file, You can 
+// obtain one at http://mozilla.org/MPL/2.0/.
+#include <Eigen/Dense>
+#include <vector>
+namespace igl
+{
+    template <typename DerivedM>
+void matrix_to_list(const Eigen::DenseBase<DerivedM>& M,
+                    std::vector<typename DerivedM::Scalar> &V)
+{
+    using namespace std;
+    V.resize(M.size());
+    // loop over cols then rows
+    for(int j =0; j<M.cols();j++)
+    {
+        for(int i = 0; i < M.rows();i++)
+        {
+            V[i+j*M.rows()] = M(i,j);
+        }
+    }
+}
+  // Compute the median of an eigen vector
+  //
+  // Inputs:
+  //   V  #V list of unsorted values
+  // Outputs:
+  //   m  median of those values
+  // Returns true on success, false on failure
+  template <typename DerivedV, typename mType>
+  bool median(
+    const Eigen::MatrixBase<DerivedV> & V, mType & m)
+  {
+    using namespace std;
+    if(V.size() == 0)
+    {
+      return false;
+    }
+    vector<typename DerivedV::Scalar> vV;
+    matrix_to_list(V,vV);
+    // http://stackoverflow.com/a/1719155/148668
+    size_t n = vV.size()/2;
+    nth_element(vV.begin(),vV.begin()+n,vV.end());
+    if(vV.size()%2==0)
+    {
+      nth_element(vV.begin(),vV.begin()+n-1,vV.end());
+      m = 0.5*(vV[n]+vV[n-1]);
+    }else
+    {
+      m = vV[n];
+    }
+    return true;
+  }
+}
+#endif

cpp/tools/nanoflann.h

@@ -0,0 +1,108 @@
+#pragma once
+#include <nanoflann.hpp>
+#include <vector>
+#include <Eigen/Dense>
+#include <iostream>
+#include "Types.h"
+
+//Collection of the nanoflann-related parts in the two methods
+
+namespace nanoflann {
+
+    /// KD-tree adaptor for working with data directly stored in an Eigen Matrix, without duplicating the data storage.
+    /// This code is adapted from the KDTreeEigenMatrixAdaptor class of nanoflann.hpp
+    template <class MatrixType, int DIM = -1, class Distance = nanoflann::metric_L2, typename IndexType = int>
+    struct KDTreeAdaptor {
+        typedef KDTreeAdaptor<MatrixType, DIM, Distance> self_t;
+        typedef typename MatrixType::Scalar              num_t;
+        typedef typename Distance::template traits<num_t, self_t>::distance_t metric_t;
+        typedef KDTreeSingleIndexAdaptor< metric_t, self_t, DIM, IndexType>  index_t;
+
+        index_t* index;
+        const MatrixType &m_data_matrix;
+
+        KDTreeAdaptor(const MatrixType &mat, const int leaf_max_size = 10) : m_data_matrix(mat) {
+            const size_t dims = mat.rows();
+            index = new index_t(dims, *this, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size, dims));
+            index->buildIndex();
+        }
+
+        ~KDTreeAdaptor() { delete index; }
+
+        /// Query for the num_closest closest points to a given point (entered as query_point[0:dim-1]).
+        inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq) const {
+            nanoflann::KNNResultSet<num_t, IndexType> resultSet(num_closest);
+            resultSet.init(out_indices, out_distances_sq);
+            index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
+        }
+
+        /// Query for the closest point to a given query.
+        inline IndexType closest(const num_t *query_point, num_t& out_distance_sq) const {
+            IndexType out_index;
+            query(query_point, 1, &out_index, &out_distance_sq);
+            return out_index;
+        }
+
+
+        inline IndexType closest(const num_t *query_point) const {
+            num_t dummy;
+            return closest(query_point, dummy);  // 调用上面的 2 参数版本
+        }
+
+        /// Radius search
+        inline size_t findInRadius(const num_t *query_point, const num_t radius, std::vector<std::pair<IndexType, num_t>>& out_idxdist) const {
+            out_idxdist.clear();
+            index->radiusSearch(query_point, radius, out_idxdist, nanoflann::SearchParams());
+            return out_idxdist.size();
+        }
+
+        const self_t & derived() const { return *this; }
+        self_t & derived() { return *this; }
+
+        inline size_t kdtree_get_point_count() const { return m_data_matrix.cols(); }
+
+        /// Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
+        inline num_t kdtree_distance(const num_t *p1, const size_t idx_p2, size_t size) const {
+            num_t s = 0;
+            for (size_t i = 0; i < size; i++) {
+                num_t d = p1[i] - m_data_matrix.coeff(i, idx_p2);
+                s += d * d;
+            }
+            return s;
+        }
+
+        /// Returns the dim'th component of the idx'th point in the class:
+        inline num_t kdtree_get_pt(const size_t idx, int dim) const {
+            return m_data_matrix.coeff(dim, idx);
+        }
+
+        /// Optional bounding-box computation
+        template <class BBOX> bool kdtree_get_bbox(BBOX&) const { return false; }
+    };
+
+
+    /// Adapter for std::vector<Eigen::Vector3d>
+    struct StdVecOfEigenVector3dRefAdapter {
+    public:
+        typedef double Scalar;  // or float, depending on your Vector3 type
+        typedef Eigen::Vector3d Vector3;
+
+        StdVecOfEigenVector3dRefAdapter(const std::vector<Vector3>& pps) : pts(pps) {};
+        const std::vector<Vector3>& pts;
+
+        inline size_t kdtree_get_point_count() const { return pts.size(); }
+
+        inline Scalar kdtree_get_pt(const size_t idx, int dim) const {
+            if (dim == 0) return pts[idx].x();
+            else if (dim == 1) return pts[idx].y();
+            else return pts[idx].z();
+        }
+
+        template <class BBOX>
+        bool kdtree_get_bbox(BBOX&) const { return false; }
+    };
+
+} // namespace nanoflann
+
+
+typedef nanoflann::KDTreeAdaptor<Matrix3X, -1, nanoflann::metric_L2_Simple> KDtree;

cpp/tools/nodeSampler.cpp

@@ -0,0 +1,638 @@
+//#pragma once
+#include "nodeSampler.h"
+#include "tools.h"
+#include "OmpHelper.h"
+#include "geodesic_algorithm_exact.h"
+#include "nanoflann.h"
+
+namespace svr
+{
+    //	Define helper functions
+    static auto square = [](const Scalar argu) { return argu * argu; };
+    static auto cube = [](const Scalar argu) { return argu * argu * argu; };
+    static auto max = [](const Scalar lhs, const Scalar rhs) { return lhs > rhs ? lhs : rhs; };
+
+    //------------------------------------------------------------------------
+    //	Node Sampling based on geodesic distance metric
+    //
+    //	Note that this member function samples nodes along some axis.
+    //	Each node is not covered by any other node. And distance between each
+    //	pair of nodes is at least sampling radius.
+    //------------------------------------------------------------------------
+    // Local geodesic calculation
+    Scalar nodeSampler::SampleAndConstuctAxis(Mesh &mesh, Scalar sampleRadiusRatio, sampleAxis axis)
+    {
+        //	Save numbers of vertex and edge
+        m_meshVertexNum = mesh.n_vertices();
+        m_meshEdgeNum = mesh.n_edges();
+        m_mesh = & mesh;
+
+        //	Calculate average edge length of bound mesh
+        for (size_t i = 0; i < m_meshEdgeNum; ++i)
+        {
+            OpenMesh::EdgeHandle eh = mesh.edge_handle(i);
+            Scalar edgeLen = mesh.calc_edge_length(eh);
+            m_averageEdgeLen += edgeLen;
+        }
+        m_averageEdgeLen /= m_meshEdgeNum;
+
+        //	Sampling radius is calculated as averageEdgeLen multiplied by sampleRadiusRatio
+        m_sampleRadius = sampleRadiusRatio * m_averageEdgeLen;
+
+        //	Reorder mesh vertex along axis
+        std::vector<size_t> vertexReorderedAlongAxis(m_meshVertexNum);
+        size_t vertexIdx = 0;
+        std::generate(vertexReorderedAlongAxis.begin(), vertexReorderedAlongAxis.end(), [&vertexIdx]() -> size_t { return vertexIdx++; });
+        std::sort(vertexReorderedAlongAxis.begin(), vertexReorderedAlongAxis.end(), [&mesh, axis](const size_t &lhs, const size_t &rhs) -> bool {
+            size_t lhsIdx = lhs;
+            size_t rhsIdx = rhs;
+            OpenMesh::VertexHandle vhl = mesh.vertex_handle(lhsIdx);
+            OpenMesh::VertexHandle vhr = mesh.vertex_handle(rhsIdx);
+            Mesh::Point vl = mesh.point(vhl);
+            Mesh::Point vr = mesh.point(vhr);
+            return vl[axis] > vr[axis];
+        });
+
+        //	Sample nodes using radius of m_sampleRadius
+        size_t firstVertexIdx = vertexReorderedAlongAxis[0];
+        VertexNodeIdx.resize(m_meshVertexNum);
+        VertexNodeIdx.setConstant(-1);
+        VertexNodeIdx[firstVertexIdx] = 0;
+        size_t cur_node_idx = 0;
+
+        m_vertexGraph.resize(m_meshVertexNum);
+        VectorX weight_sum = VectorX::Zero(m_meshVertexNum);
+
+        for (auto &vertexIdx : vertexReorderedAlongAxis)
+        {
+            if(VertexNodeIdx[vertexIdx] < 0 && m_vertexGraph.at(vertexIdx).empty())
+            {
+                m_nodeContainer.emplace_back(cur_node_idx, vertexIdx);
+                VertexNodeIdx[vertexIdx] = cur_node_idx;
+
+                std::vector<size_t> neighbor_verts;
+                geodesic::GeodesicAlgorithmExact geoalg(&mesh, vertexIdx, m_sampleRadius);
+                geoalg.propagate(vertexIdx, neighbor_verts);
+                for(size_t i = 0; i < neighbor_verts.size(); i++)
+                {
+                    int neighIdx = neighbor_verts[i];
+                    Scalar geodist = mesh.data(mesh.vertex_handle(neighIdx)).geodesic_distance;
+                    if(geodist < m_sampleRadius)
+                    {
+                        Scalar weight = std::pow(1-std::pow(geodist/m_sampleRadius, 2), 3);
+                        m_vertexGraph.at(neighIdx).emplace(std::pair<int, Scalar>(cur_node_idx, weight));
+                        weight_sum[neighIdx] += weight;
+                    }
+                }
+                cur_node_idx++;
+            }
+        }
+
+        m_nodeGraph.resize(cur_node_idx);
+        for (auto &vertexIdx : vertexReorderedAlongAxis)
+        {
+            for(auto &node: m_vertexGraph[vertexIdx])
+            {
+                size_t nodeIdx = node.first;
+                for(auto &neighNode: m_vertexGraph[vertexIdx])
+                {
+                    size_t neighNodeIdx = neighNode.first;
+                    if(nodeIdx != neighNodeIdx)
+                    {
+                        m_nodeGraph.at(nodeIdx).emplace(std::pair<int, Scalar>(neighNodeIdx, 1.0));
+                    }
+                }
+                m_vertexGraph.at(vertexIdx).at(nodeIdx) /= weight_sum[vertexIdx];
+            }
+        }
+        return m_sampleRadius;
+    }
+
+    //------------------------------------------------------------------------
+    //	Node Sampling based on geodesic distance metric
+    //
+    //	Note that this member function samples nodes along some axis.
+    //	Each node is not covered by any other node. And distance between each
+    //	pair of nodes is at least sampling radius.
+    //------------------------------------------------------------------------
+    // Local geodesic calculation
+    Scalar nodeSampler::SampleAndConstuct(Mesh &mesh, Scalar sampleRadiusRatio, const Matrix3X & src_points)
+    {
+        //	Save numbers of vertex and edge
+        m_meshVertexNum = mesh.n_vertices();
+        m_meshEdgeNum = mesh.n_edges();
+        m_mesh = & mesh;
+        
+
+        //	Calculate average edge length of bound mesh
+        for (size_t i = 0; i < m_meshEdgeNum; ++i)
+        {
+            OpenMesh::EdgeHandle eh = mesh.edge_handle(i);
+            Scalar edgeLen = mesh.calc_edge_length(eh);
+            m_averageEdgeLen += edgeLen;
+        }
+        m_averageEdgeLen /= m_meshEdgeNum;
+
+        //	Sampling radius is calculated as averageEdgeLen multiplied by sampleRadiusRatio
+        m_sampleRadius = sampleRadiusRatio * m_averageEdgeLen;
+        
+        // de-mean
+        Vector3 means = src_points.rowwise().mean();
+        Matrix3X demean_src_points = src_points.colwise() - means;
+        Matrix33 covariance = demean_src_points * demean_src_points.transpose();
+
+        Eigen::SelfAdjointEigenSolver<Matrix33> eigensolver(covariance);
+        Vector3 eigen_values = eigensolver.eigenvalues();
+        int max_idx = 2;
+        if(eigen_values[0] > eigen_values[max_idx])    max_idx = 0;
+        if(eigen_values[1] > eigen_values[max_idx])    max_idx = 1;
+
+        Vector3 main_axis = eigensolver.eigenvectors().col(max_idx);
+
+        VectorX projection = demean_src_points.transpose() * main_axis;
+        
+        projection_indices.resize(projection.size());
+        projection_indices.setLinSpaced(projection.size(), 0, projection.size() - 1);
+
+        auto compare = [&projection](int i, int j) {
+            return projection(i) < projection(j);
+        };
+
+        std::sort(projection_indices.data(), projection_indices.data() + projection_indices.size(), compare);
+
+
+        //	Sample nodes using radius of m_sampleRadius
+        size_t firstVertexIdx = projection_indices[0];
+        VertexNodeIdx.resize(m_meshVertexNum);
+        VertexNodeIdx.setConstant(-1);
+        VertexNodeIdx[firstVertexIdx] = 0;
+        size_t cur_node_idx = 0;
+
+        m_vertexGraph.resize(m_meshVertexNum);
+        VectorX weight_sum = VectorX::Zero(m_meshVertexNum);
+
+        for(int idx = 0; idx < projection_indices.size(); idx++)
+        {
+            int vertexIdx = projection_indices[idx];
+            if(VertexNodeIdx[vertexIdx] < 0 && m_vertexGraph.at(vertexIdx).empty())
+            {
+                m_nodeContainer.emplace_back(cur_node_idx, vertexIdx);
+                VertexNodeIdx[vertexIdx] = cur_node_idx;
+
+                std::vector<size_t> neighbor_verts;
+                geodesic::GeodesicAlgorithmExact geoalg(&mesh, vertexIdx, m_sampleRadius);
+                geoalg.propagate(vertexIdx, neighbor_verts);
+                for(size_t i = 0; i < neighbor_verts.size(); i++)
+                {
+                    int neighIdx = neighbor_verts[i];
+                    Scalar geodist = mesh.data(mesh.vertex_handle(neighIdx)).geodesic_distance;
+                    if(geodist < m_sampleRadius)
+                    {
+                        Scalar weight = std::pow(1-std::pow(geodist/m_sampleRadius, 2), 3);
+                        m_vertexGraph.at(neighIdx).emplace(std::pair<int, Scalar>(cur_node_idx, weight));
+                        weight_sum[neighIdx] += weight;
+                    }
+                }
+                cur_node_idx++;
+            }
+        }
+
+        m_nodeGraph.resize(cur_node_idx);
+        for(int idx = 0; idx < projection_indices.size(); idx++)
+        {
+            int vertexIdx = projection_indices[idx];
+            for(auto &node: m_vertexGraph[vertexIdx])
+            {
+                size_t nodeIdx = node.first;
+                for(auto &neighNode: m_vertexGraph[vertexIdx])
+                {
+                    size_t neighNodeIdx = neighNode.first;
+                    if(nodeIdx != neighNodeIdx)
+                    {
+                        m_nodeGraph.at(nodeIdx).emplace(std::pair<int, Scalar>(neighNodeIdx, 1.0));
+                    }
+                }
+                m_vertexGraph.at(vertexIdx).at(nodeIdx) /= weight_sum[vertexIdx];
+            }
+        }
+
+        
+
+        // // check neighbors 
+        // for(int nodeIdx = 0; nodeIdx < cur_node_idx; nodeIdx++)
+        // {
+        //     int num_neighbors =  m_nodeGraph[nodeIdx].size();
+        //     // for (auto &eachNeighbor : m_nodeGraph[nodeIdx])
+        //     // {
+        //     //     num_neighbors++;
+        //     // }
+        //     if(num_neighbors<2)
+        //     {
+                
+        //         VectorX dists(cur_node_idx);
+        //         #pragma omp parallel for
+        //         for(int ni = 0; ni < cur_node_idx; ni++)
+        //         {
+        //             int vidx0 = getNodeVertexIdx(nodeIdx);
+        //             int vidx1 = getNodeVertexIdx(ni);
+        //             Scalar dist = (src_points.col(vidx0) - src_points.col(vidx1)).squaredNorm();
+        //             dists[ni] = dist;
+        //         }
+        //         dists[nodeIdx] = 1e10;
+        //         for(int k = 0; k < 6; k++)
+        //         {
+        //             int min_idx = -1;
+        //             dists.minCoeff(&min_idx);
+        //             m_nodeGraph.at(nodeIdx).emplace(std::pair<int, Scalar>(min_idx, 1.0));
+        //             dists[min_idx] = 1e10;
+        //         }
+        //     }
+        // }
+        // Eigen::VectorXi vnn(cur_node_idx);
+        // // check vertex neighbors 
+        // for(int vidx = 0; vidx < cur_node_idx; vidx++)
+        // {
+        //     // std::cout << "vidx.neighbor_node = " << m_vertexGraph.at(vidx).size() << std::endl;
+        //     vnn[vidx] = m_nodeGraph.at(vidx).size();
+        // }
+        // std::cout << "v neighbor max = " << vnn.maxCoeff() << " min = " << vnn.minCoeff() << std::endl;
+        return m_sampleRadius;
+    }
+
+
+    Scalar nodeSampler::SampleAndConstuctForSrcPoints(Mesh &mesh, Scalar sampleRadiusRatio, const Matrix3X & src_points, const Eigen::MatrixXi & src_knn_indices)
+    {
+        //	Save numbers of vertex and edge
+        m_meshVertexNum = src_points.cols();
+        int knn_num_neighbor = src_knn_indices.rows(); 
+        m_meshEdgeNum = m_meshVertexNum*knn_num_neighbor;
+        m_mesh = & mesh;
+
+        //	Calculate average edge length of bound mesh
+        for (size_t i = 0; i < m_meshVertexNum; ++i)
+        {
+            for(size_t j = 0; j < knn_num_neighbor; j++)
+            {
+                Scalar edgeLen = (src_points.col(i) - src_points.col(src_knn_indices(j, i))).norm();
+                m_averageEdgeLen += edgeLen;
+            }
+        }
+        m_averageEdgeLen /= m_meshEdgeNum;
+
+
+        //	Sampling radius is calculated as averageEdgeLen multiplied by sampleRadiusRatio
+        m_sampleRadius = sampleRadiusRatio * m_averageEdgeLen;
+
+        // de-mean
+        Vector3 means = src_points.rowwise().mean();
+        Matrix3X demean_src_points = src_points.colwise() - means;
+        Matrix33 covariance = demean_src_points * demean_src_points.transpose();
+
+        Eigen::SelfAdjointEigenSolver<Matrix33> eigensolver(covariance);
+        Vector3 eigen_values = eigensolver.eigenvalues();
+        int max_idx = 2;
+        if(eigen_values[0] > eigen_values[max_idx])    max_idx = 0;
+        if(eigen_values[1] > eigen_values[max_idx])    max_idx = 1;
+
+        Vector3 main_axis = eigensolver.eigenvectors().col(max_idx);
+
+        VectorX projection = demean_src_points.transpose() * main_axis;
+        
+        projection_indices.resize(projection.size());
+        projection_indices.setLinSpaced(projection.size(), 0, projection.size() - 1);
+
+        auto compare = [&projection](int i, int j) {
+            return projection(i) < projection(j);
+        };
+
+        std::sort(projection_indices.data(), projection_indices.data() + projection_indices.size(), compare);
+
+
+        //	Sample nodes using radius of m_sampleRadius
+        size_t firstVertexIdx = projection_indices[0];
+        VertexNodeIdx.resize(m_meshVertexNum);
+        VertexNodeIdx.setConstant(-1);
+        VertexNodeIdx[firstVertexIdx] = 0;
+        size_t cur_node_idx = 0;
+
+        m_vertexGraph.resize(m_meshVertexNum);
+        VectorX weight_sum = VectorX::Zero(m_meshVertexNum);
+
+        for(int idx = 0; idx < projection_indices.size(); idx++)
+        {
+            int vertexIdx = projection_indices[idx];
+            if(VertexNodeIdx[vertexIdx] < 0 && m_vertexGraph.at(vertexIdx).empty())
+            {
+                m_nodeContainer.emplace_back(cur_node_idx, vertexIdx);
+                VertexNodeIdx[vertexIdx] = cur_node_idx;
+                
+                std::queue<size_t> neighbor_verts;
+                neighbor_verts.push(vertexIdx);
+                Eigen::VectorXi visited(m_meshVertexNum);
+                visited.setZero();
+                visited[vertexIdx] = 1;
+                while(!neighbor_verts.empty())
+                {
+                    size_t vidx = neighbor_verts.front();
+                    neighbor_verts.pop();
+                    Scalar dist = (src_points.col(vertexIdx) - src_points.col(vidx)).norm();
+
+                    if(dist < m_sampleRadius)
+                    {
+                        Scalar weight = std::pow(1-std::pow(dist/m_sampleRadius, 2), 3);
+                        m_vertexGraph.at(vidx).emplace(std::pair<int, Scalar>(cur_node_idx, weight));
+                        weight_sum[vidx] += weight;
+                        for(int j = 0; j < knn_num_neighbor; j++)
+                        {
+                            int neighbor_vidx = src_knn_indices(j, vidx);
+                            if(visited[neighbor_vidx]==0)
+                            {
+                                neighbor_verts.push(neighbor_vidx);
+                                visited[neighbor_vidx]= 1;
+                            }
+                        }
+                    }      
+                }
+
+                cur_node_idx++;             
+            }
+        }
+
+        m_nodeGraph.resize(cur_node_idx);
+        for(int idx = 0; idx < projection_indices.size(); idx++)
+        {
+            int vertexIdx = projection_indices[idx];
+            for(auto &node: m_vertexGraph[vertexIdx])
+            {
+                size_t nodeIdx = node.first;
+                for(auto &neighNode: m_vertexGraph[vertexIdx])
+                {
+                    size_t neighNodeIdx = neighNode.first;
+                    if(nodeIdx != neighNodeIdx)
+                    {
+                        m_nodeGraph.at(nodeIdx).emplace(std::pair<int, Scalar>(neighNodeIdx, 1.0));
+                    }
+                }
+                m_vertexGraph.at(vertexIdx).at(nodeIdx) /= weight_sum[vertexIdx];
+            }
+        }
+
+        // // check neighbors 
+        // for(int nodeIdx = 0; nodeIdx < cur_node_idx; nodeIdx++)
+        // {
+        //     int num_neighbors = 0;
+        //     for (auto &eachNeighbor : m_nodeGraph[nodeIdx])
+        //     {
+        //         num_neighbors++;
+        //     }
+        //     if(num_neighbors==0)
+        //     {
+                
+        //         VectorX dists(cur_node_idx);
+        //         #pragma omp parallel for
+        //         for(int ni = 0; ni < cur_node_idx; ni++)
+        //         {
+        //             int vidx0 = getNodeVertexIdx(nodeIdx);
+        //             int vidx1 = getNodeNeighborSize(ni);
+        //             Scalar dist = (src_points.col(vidx0) - src_points.col(vidx1)).squaredNorm();
+        //             dists[ni] = dist;
+        //         }
+        //         dists[nodeIdx] = 1e10;
+        //         int min_idx = -1;
+        //         dists.minCoeff(&min_idx);
+        //         m_nodeGraph.at(nodeIdx).emplace(std::pair<int, Scalar>(min_idx, 1.0));
+        //     }
+        // }
+        return m_sampleRadius;
+    }
+
+
+    Scalar nodeSampler::SampleAndConstuctFPS(Mesh &mesh, Scalar sampleRadiusRatio, const Matrix3X & src_points, const Eigen::MatrixXi & src_knn_indices, int num_vn, int num_nn)
+    {
+        m_meshVertexNum = src_points.cols();
+        int knn_num_neighbor = src_knn_indices.rows(); 
+        m_meshEdgeNum = m_meshVertexNum*knn_num_neighbor;
+        m_mesh = & mesh;
+
+        //	Calculate average edge length of bound mesh
+        for (size_t i = 0; i < m_meshVertexNum; ++i)
+        {
+            for(size_t j = 0; j < knn_num_neighbor; j++)
+            {
+                Scalar edgeLen = (src_points.col(i) - src_points.col(src_knn_indices(j, i))).norm();
+                m_averageEdgeLen += edgeLen;
+            }
+        }
+        m_averageEdgeLen /= m_meshEdgeNum;
+
+        //	Sampling radius is calculated as averageEdgeLen multiplied by sampleRadiusRatio
+        m_sampleRadius = sampleRadiusRatio * m_averageEdgeLen;
+
+        // start points
+        size_t startIndex = 0;
+        // FPS to get sampling points in align term
+        VectorX minDistances(m_meshVertexNum);
+        minDistances.setConstant(std::numeric_limits<Scalar>::max());
+        minDistances[startIndex] = 0;
+        m_nodeContainer.emplace_back(startIndex, 0);
+    
+        Scalar minimal_dist = 1e10;
+        int cur_node_idx = 1;
+        // repeat select farthest points
+        while (minimal_dist > m_sampleRadius) {
+            // calculate the distance between each point with the sampling points set.         
+            #pragma omp parallel for
+            for (size_t i = 0; i < m_meshVertexNum; ++i) {
+                if(i==startIndex)
+                    continue;
+                
+                Scalar dist = (src_points.col(startIndex) - src_points.col(i)).norm();
+                if(dist < minDistances[i])
+                    minDistances[i] = dist;
+            }
+
+            // choose farthest point
+            int maxDistanceIndex;
+            minimal_dist = minDistances.maxCoeff(&maxDistanceIndex);
+            minDistances[maxDistanceIndex] = 0;
+
+            // add the farthest point into the sampling points set.
+            startIndex = maxDistanceIndex;
+            m_nodeContainer.emplace_back(cur_node_idx, startIndex);
+            cur_node_idx++;
+        }
+
+        Matrix3X node_positions(3, cur_node_idx);
+        for(int i = 0; i < cur_node_idx; i++)
+        {
+            int vidx = getNodeVertexIdx(i);
+            node_positions.col(i) = src_points.col(vidx);
+        }
+        KDtree node_tree(node_positions);
+
+        // For each vertex, find num_vn-closest nodes
+        m_vertexGraph.resize(m_meshVertexNum);
+        VectorX weight_sum(m_meshVertexNum);
+        weight_sum.setZero();
+        #pragma omp parallel for 
+        for(int vidx = 0; vidx < m_meshVertexNum; vidx++)
+        {
+            std::vector<int> out_indices(num_vn);
+            std::vector<Scalar> out_dists(num_vn);
+            node_tree.query(src_points.col(vidx).data(), num_vn, out_indices.data(), out_dists.data());
+            for(int k = 0; k < num_vn; k++)
+            {
+                Scalar weight = std::pow(1-std::pow(out_dists[k]/m_sampleRadius, 2), 3);
+                m_vertexGraph.at(vidx).emplace(std::pair<int, Scalar>(out_indices[k], weight));
+                weight_sum[vidx] += weight;
+            }
+        }
+
+        #pragma omp parallel for 
+        for(int vidx = 0; vidx < m_meshVertexNum; vidx++)
+        {
+            for(auto &neighNode: m_vertexGraph[vidx])
+            {
+                size_t neighNodeIdx = neighNode.first;
+                m_vertexGraph.at(vidx).at(neighNodeIdx) /= weight_sum[vidx];
+            }
+        }
+
+
+        // For each node, find num_nn-closest nodes
+        m_nodeGraph.resize(cur_node_idx);
+        #pragma omp parallel for 
+        for(int nidx = 0; nidx < cur_node_idx; nidx++)
+        {
+            std::vector<int> out_indices(num_nn+1);
+            std::vector<Scalar> out_dists(num_nn+1);
+            int vidx = getNodeVertexIdx(nidx);
+            node_tree.query(src_points.col(vidx).data(), num_nn+1, out_indices.data(), out_dists.data());
+            for(int k = 0; k < num_nn; k++)
+            {
+                m_nodeGraph.at(nidx).emplace(std::pair<int, Scalar>(out_indices[k+1], 1.0));
+            }
+        }        
+        return m_sampleRadius;
+    }
+
+
+    
+
+
+    void nodeSampler::initWeight(RowMajorSparseMatrix& matPV, VectorX & matP, RowMajorSparseMatrix& matB, VectorX& matD, VectorX& smoothw)
+    {
+        Timer time;
+        std::vector<Eigen::Triplet<Scalar>> coeff;
+        matP.setZero();
+        Eigen::VectorXi nonzero_num = Eigen::VectorXi::Zero(m_mesh->n_vertices());
+        // data coeff
+        for (size_t vertexIdx = 0; vertexIdx < m_meshVertexNum; ++vertexIdx)
+        {
+            Mesh::Point vi = m_mesh->point(m_mesh->vertex_handle(vertexIdx));
+            for (auto &eachNeighbor : m_vertexGraph[vertexIdx])
+            {
+                size_t nodeIdx = eachNeighbor.first;
+                Scalar weight = m_vertexGraph.at(vertexIdx).at(nodeIdx);
+                Mesh::Point pj = m_mesh->point(m_mesh->vertex_handle(getNodeVertexIdx(nodeIdx)));
+
+				for (int k = 0; k < 3; k++)
+				{
+					coeff.push_back(Eigen::Triplet<Scalar>(3 * vertexIdx + k, nodeIdx * 12 + k, weight * (vi[0] - pj[0])));
+					coeff.push_back(Eigen::Triplet<Scalar>(3 * vertexIdx + k, nodeIdx * 12 + k + 3, weight * (vi[1] - pj[1])));
+					coeff.push_back(Eigen::Triplet<Scalar>(3 * vertexIdx + k, nodeIdx * 12 + k + 6, weight * (vi[2] - pj[2])));
+					coeff.push_back(Eigen::Triplet<Scalar>(3 * vertexIdx + k, nodeIdx * 12 + k + 9, weight * 1.0));
+				}
+                
+				matP[vertexIdx * 3] += weight * pj[0];
+				matP[vertexIdx * 3 + 1] += weight * pj[1];
+				matP[vertexIdx * 3 + 2] += weight * pj[2];
+            }
+            nonzero_num[vertexIdx] = m_vertexGraph[vertexIdx].size();
+        }
+        matPV.setFromTriplets(coeff.begin(), coeff.end());
+
+
+        // smooth coeff
+        coeff.clear();
+        int max_edge_num = nodeSize() * (nodeSize()-1);
+		matB.resize(max_edge_num * 3, 12 * nodeSize());
+		matD.resize(max_edge_num * 3);
+        smoothw.resize(max_edge_num*3);
+		smoothw.setZero();
+        matD.setZero();
+        int edge_id = 0;
+
+        for (size_t nodeIdx = 0; nodeIdx < m_nodeContainer.size(); ++nodeIdx)
+        {
+            size_t vIdx0 = getNodeVertexIdx(nodeIdx);
+            Mesh::VertexHandle vh0 = m_mesh->vertex_handle(vIdx0);
+            Mesh::Point v0 = m_mesh->point(vh0);
+
+            for (auto &eachNeighbor : m_nodeGraph[nodeIdx])
+            {
+                size_t neighborIdx = eachNeighbor.first;
+                size_t vIdx1 = getNodeVertexIdx(neighborIdx);
+                Mesh::Point v1 = m_mesh->point(m_mesh->vertex_handle(vIdx1));
+                Mesh::Point dv = v0 - v1;
+                int k = edge_id;
+
+				for (int t = 0; t < 3; t++)
+				{
+					coeff.push_back(Eigen::Triplet<Scalar>(k * 3 + t, neighborIdx * 12 + t, dv[0]));
+					coeff.push_back(Eigen::Triplet<Scalar>(k * 3 + t, neighborIdx * 12 + t + 3, dv[1]));
+					coeff.push_back(Eigen::Triplet<Scalar>(k * 3 + t, neighborIdx * 12 + t + 6, dv[2]));
+					coeff.push_back(Eigen::Triplet<Scalar>(k * 3 + t, neighborIdx * 12 + t + 9, 1.0));
+					coeff.push_back(Eigen::Triplet<Scalar>(k * 3 + t, nodeIdx * 12 + t + 9, -1.0));
+				}
+
+                Scalar dist = dv.norm();
+                if(dist > 0)
+                {
+					smoothw[k * 3] = smoothw[k * 3 + 1] = smoothw[k * 3 + 2] = 1.0 / dist;
+                }
+                else
+                {
+					//smoothw[k * 3] = 0.0;
+                    std::cout << "node repeat";
+                    exit(1);
+                }
+				matD[k * 3] = dv[0];
+				matD[k * 3 + 1] = dv[1];
+				matD[k * 3 + 2] = dv[2];
+                edge_id++;
+            }
+        }
+        matB.setFromTriplets(coeff.begin(), coeff.end());
+        matD.conservativeResize(edge_id*3);
+        matB.conservativeResize(edge_id*3, matPV.cols());
+        smoothw.conservativeResize(edge_id*3);
+        smoothw *= edge_id/(smoothw.sum()/3.0);
+    }
+
+
+    void nodeSampler::print_nodes(Mesh & mesh, std::string file_path)
+    {
+        std::string namev = file_path + "nodes.obj";
+        std::ofstream out1(namev);
+        for (size_t i = 0; i < m_nodeContainer.size(); i++)
+        {
+            int vexid = m_nodeContainer[i].second;
+            out1 << "v " << mesh.point(mesh.vertex_handle(vexid))[0] << " " << mesh.point(mesh.vertex_handle(vexid))[1]
+                << " " << mesh.point(mesh.vertex_handle(vexid))[2] << std::endl;
+        }
+        Eigen::VectorXi nonzero_num = Eigen::VectorXi::Zero(m_nodeContainer.size());
+        for (size_t nodeIdx = 0; nodeIdx < m_nodeContainer.size(); ++nodeIdx)
+        {
+            for (auto &eachNeighbor : m_nodeGraph[nodeIdx])
+            {
+                size_t neighborIdx = eachNeighbor.first;
+                out1 << "l " << nodeIdx+1 << " " << neighborIdx+1 << std::endl;
+            }
+            nonzero_num[nodeIdx] = m_nodeGraph[nodeIdx].size();
+        }
+        std::cout << "node neighbor min = " << nonzero_num.minCoeff() << " max = "
+                  << nonzero_num.maxCoeff() << " average = " << nonzero_num.mean() << std::endl;
+        out1.close();
+    }
+}

cpp/tools/nodeSampler.h

@@ -0,0 +1,94 @@
+#ifndef NODESAMPLER_H
+#define NODESAMPLER_H
+#include "Types.h"
+
+namespace svr
+{
+    //------------------------------------------------------------------------
+    //	Define neighborIter class
+    //------------------------------------------------------------------------
+    class neighborIter
+    {
+    public:
+        neighborIter(const std::map<size_t, Scalar> &nodeNeighbors)
+        {
+            m_neighborIter = nodeNeighbors.begin();
+            m_neighborEnd = nodeNeighbors.end();
+        }
+
+        neighborIter& operator++()
+        {
+            if (m_neighborIter != m_neighborEnd)
+                ++m_neighborIter;
+            return *this;
+        }
+
+        neighborIter operator++(int)
+        {
+            neighborIter tempIter(*this);
+            ++*this;
+            return tempIter;
+        }
+
+        const std::pair<const size_t, Scalar>& operator*() { return *m_neighborIter; }
+        std::map<size_t, Scalar>::const_iterator operator->() { return m_neighborIter; }
+        bool is_valid() { return m_neighborIter != m_neighborEnd; }
+        size_t getIndex() { return m_neighborIter->first; }
+        Scalar getWeight() { return m_neighborIter->second; }
+
+    private:
+        std::map<size_t, Scalar>::const_iterator m_neighborIter;
+        std::map<size_t, Scalar>::const_iterator m_neighborEnd;
+    };
+
+    //------------------------------------------------------------------------
+    //	Define node sampling class
+    //------------------------------------------------------------------------
+    class nodeSampler
+    {
+    public:
+        enum sampleAxis { X_AXIS, Y_AXIS, Z_AXIS };
+        nodeSampler() {};
+
+        // return sample radius
+        Scalar SampleAndConstuct(Mesh &mesh, Scalar sampleRadiusRatio, const Matrix3X & src_points);
+        Scalar SampleAndConstuctAxis(Mesh &mesh, Scalar sampleRadiusRatio, sampleAxis axis);
+
+        Scalar SampleAndConstuctForSrcPoints(Mesh &mesh, Scalar sampleRadiusRatio, const Matrix3X & src_points, const Eigen::MatrixXi & src_knn_indices);
+        Scalar SampleAndConstuctFPS(Mesh &mesh, Scalar sampleRadiusRatio, const Matrix3X & src_points, const Eigen::MatrixXi & src_knn_indices, int num_vn, int num_nn);
+
+
+        void updateWeight(Mesh &mesh);
+        void constructGraph(bool is_uniform);
+
+        size_t nodeSize() const { return m_nodeContainer.size(); }
+
+        neighborIter getNodeNodeIter(size_t nodeIdx) const { return neighborIter(m_nodeGraph[nodeIdx]); }
+        neighborIter getVertexNodeIter(size_t vertexIdx) const { return neighborIter(m_vertexGraph[vertexIdx]); }
+        size_t getNodeVertexIdx(size_t nodeIdx) const { return m_nodeContainer.at(nodeIdx).second; }
+
+        size_t getVertexNeighborSize(size_t vertexIdx) const { return m_vertexGraph.at(vertexIdx).size(); }
+        size_t getNodeNeighborSize(size_t nodeIdx) const { return m_nodeGraph.at(nodeIdx).size(); }
+        void initWeight(RowMajorSparseMatrix& matPV, VectorX & matP,
+			RowMajorSparseMatrix& matB, VectorX& matD, VectorX& smoothw);
+        void print_nodes(Mesh & mesh, std::string file_path);
+
+    private:
+        size_t m_meshVertexNum = 0;
+        size_t m_meshEdgeNum = 0;
+        Scalar m_averageEdgeLen = 0.0f;
+        Scalar m_sampleRadius = 0.0f;
+        std::vector<Scalar> non_unisamples_Radius;
+        Mesh * m_mesh;
+
+    public:
+        std::vector<std::pair<size_t, size_t>> m_nodeContainer;
+        std::vector<std::map<size_t, Scalar>> m_vertexGraph; // vertex node graph
+        std::vector<std::map<size_t, Scalar>> m_nodeGraph;
+        std::vector<VectorX> m_geoDistContainer;
+        Eigen::VectorXi      VertexNodeIdx;
+        Eigen::VectorXi      projection_indices;
+
+    };
+}
+#endif

cpp/tools/parameters.h

@@ -0,0 +1,92 @@
+#ifndef PARAMETERS_H
+#define PARAMETERS_H
+
+#include "Types.h"
+
+//Parameter repository for different methods
+
+// parameters for FRICP
+namespace ICP{
+    struct Parameters {
+    double p;       /// paramter of the robust function/// para k
+    int max_icp;    /// max ICP iteration
+    double stop;    /// stopping criteria
+    std::string out_path;
+    int anderson_m;
+    MatrixXX init_trans;
+    MatrixXX gt_trans;
+    bool has_groundtruth;
+    double convergence_energy;
+    double convergence_gt_mse;
+    MatrixXX res_trans;
+    double nu_begin_k;
+    double nu_end_k;
+    bool use_init;
+    double nu_alpha;
+    Parameters() :
+        p(0.1),
+        max_icp(100),
+        stop(1e-5),
+        anderson_m(5),
+        has_groundtruth(false),
+        convergence_energy(0.0),
+        convergence_gt_mse(0.0),
+        nu_begin_k(3),
+        nu_end_k(1.0 / (3.0 * sqrt(3.0))),
+        use_init(false),
+        nu_alpha(1.0 / 2)
+    {
+        gt_trans = Eigen::Matrix4d::Identity();
+        init_trans = Eigen::MatrixXd(); // 或指定大小
+        res_trans = Eigen::MatrixXd();
+    }
+    };
+}
+
+namespace spare{
+// parameters for spare non-rigid registration
+struct Parameters
+{
+    int		max_outer_iters;    // nonrigid max iters
+    Scalar	w_smo;              // smoothness weight 
+    Scalar	w_rot;               // rotation matrix weight 
+	Scalar  w_arap_coarse;      // ARAP weight for coarse alignment
+    Scalar  w_arap_fine;        // ARAP weight for fine alignment 
+    bool	use_landmark;
+    bool    calc_gt_err;         // calculate ground truth error (DEBUG)
+    std::vector<int> landmark_src;
+    std::vector<int> landmark_tar;
+    Scalar  Data_nu;
+    Scalar  Data_initk;
+    Scalar  stop_coarse;
+    Scalar  stop_fine;
+	// Sample para
+    Scalar  uni_sample_radio;       // uniform sample radio
+    bool    use_geodesic_dist;
+    int     num_sample_nodes;
+    // record the initial error
+    Scalar  init_gt_mean_errs;
+    Scalar  init_gt_max_errs;
+    Scalar  mesh_scale;
+    Parameters() // default
+    {
+        max_outer_iters = 30;
+        w_smo = 0.01;  // smooth
+        w_rot = 1e-4;   // orth
+		w_arap_coarse = 500; // 10;
+        w_arap_fine = 200;
+        use_landmark = false;
+        calc_gt_err = true;
+        Data_nu = 0.0;
+        Data_initk = 1;
+        stop_coarse = 1e-3;
+        stop_fine = 1e-4;
+        // Sample para
+        uni_sample_radio = 10;
+        use_geodesic_dist = true;
+        init_gt_mean_errs = .0;
+    }
+};
+};
+
+#endif // PARAMETERS_H

cpp/tools/robust_norm.h

@@ -0,0 +1,37 @@
+#ifndef ROBUST_NORM_H_
+#define ROBUST_NORM_H_
+
+#include "registration.h" 
+
+inline void welsch_weight(Eigen::VectorXd& r, double p)
+{
+    #pragma omp parallel for
+    for (int i = 0; i < r.size(); ++i)
+    {
+        r[i] = (r[i] >= 0) ? std::exp(-r[i] / (2 * p * p)) : 0.0;
+    }
+}
+
+inline double welsch_energy(const Eigen::VectorXd& r, double p)
+{
+    double energy = 0.0;
+    #pragma omp parallel for reduction(+:energy)
+    for (int i = 0; i < r.size(); ++i)
+    {
+        energy += 1.0 - std::exp(-r[i]  / (2 * p * p));
+    }
+    return energy;
+}
+
+inline void robust_weight(Eigen::VectorXd& r, double p)
+{
+    welsch_weight(r, p);
+}
+
+inline double get_energy(const Eigen::VectorXd& r, double p)
+{
+    return welsch_energy(r, p);
+}
+
+
+#endif

cpp/tools/tools.cpp

@@ -0,0 +1,85 @@
+#include "tools.h"
+#include <iostream>
+#include "pch.h" 
+#ifdef __linux__		
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#endif
+
+
+Scalar mesh_scaling(Mesh& src_mesh, Mesh& tar_mesh)
+{
+    Vec3 max(-1e12, -1e12, -1e12);
+    Vec3 min(1e12, 1e12, 1e12);
+    for(auto it = src_mesh.vertices_begin(); it != src_mesh.vertices_end(); it++)
+    {
+        for(int j = 0; j < 3; j++)
+        {
+            if(src_mesh.point(*it)[j] > max[j])
+            {
+                max[j] = src_mesh.point(*it)[j];
+            }
+            if(src_mesh.point(*it)[j] < min[j])
+            {
+                min[j] = src_mesh.point(*it)[j];
+            }
+        }
+    }
+
+    for(auto it = tar_mesh.vertices_begin(); it != tar_mesh.vertices_end(); it++)
+    {
+        for(int j = 0; j < 3; j++)
+        {
+            if(tar_mesh.point(*it)[j] > max[j])
+            {
+                max[j] = tar_mesh.point(*it)[j];
+            }
+            if(tar_mesh.point(*it)[j] < min[j])
+            {
+                min[j] = tar_mesh.point(*it)[j];
+            }
+        }
+    }
+    Scalar scale = (max-min).norm();
+
+    for(auto it = src_mesh.vertices_begin(); it != src_mesh.vertices_end(); it++)
+    {
+        Vec3 p = src_mesh.point(*it);
+        p = p/scale;
+        src_mesh.set_point(*it, p);
+    }
+
+    for(auto it = tar_mesh.vertices_begin(); it != tar_mesh.vertices_end(); it++)
+    {
+        Vec3 p = tar_mesh.point(*it);
+        p = p/scale;
+        tar_mesh.set_point(*it, p);
+    }
+
+    return scale;
+}
+
+
+
+Vector3 Vec2Eigen(Vec3 s)
+{
+    return Vector3(s[0], s[1], s[2]);
+}
+
+
+#ifdef __linux__
+bool my_mkdir(std::string file_path)
+{
+    if(access(file_path.c_str(), 06))
+   {
+       std::cout << "file_path : (" << file_path << ") didn't exist or no write ability!!" << std::endl;
+       if(mkdir(file_path.c_str(), S_IRWXU))
+       {
+           std::cout << "mkdir " << file_path << " is wrong! please check upper path " << std::endl;
+           exit(0);
+       }
+       std::cout<< "mkdir " << file_path << " success!! " << std::endl;
+   }
+}
+#endif

cpp/tools/tools.h

@@ -0,0 +1,12 @@
+#ifndef TOOL_H_
+#define TOOL_H_
+#include "Types.h"
+// Convert Mesh to libigl format to calculate geodesic distance
+
+Scalar mesh_scaling(Mesh& src_mesh, Mesh& tar_mesh);
+Vector3 Vec2Eigen(Vec3 s);
+#ifdef __linux__
+bool my_mkdir(std::string file_path);
+#endif
+
+#endif

docs/.gitkeep

@@ -0,0 +1 @@
+0

examples/.gitkeep

@@ -0,0 +1 @@
+0

examples/c++/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.16)
+project(MyExternalApp LANGUAGES CXX)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type" FORCE)
+endif()
+
+
+
+
+add_subdirectory(002hugging_face)
+add_executable(MyApp main.cpp)
+target_link_libraries(MyApp PRIVATE Lightweight3DRegLib)
+

examples/c++/main.cpp

@@ -0,0 +1,128 @@
+#include <filesystem>
+#include <iostream>
+#include <string>
+
+#include <OpenMesh/Core/IO/MeshIO.hh>
+#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
+#include <Eigen/Dense>
+
+#include "nonrigid_spare_registration.h"
+#include "rigid_fricp_registration.h"
+#include "Types.h"
+
+namespace fs = std::filesystem;
+typedef OpenMesh::TriMesh_ArrayKernelT<> MeshType;
+using Matrix3X = Eigen::Matrix<double, 3, Eigen::Dynamic>;
+
+// ---------------------- 辅助函数 ----------------------
+bool EnsureDirectoryExists(const std::string& path)
+{
+    if (!fs::exists(path)) {
+        if (!fs::create_directories(path)) {
+            std::cerr << "Failed to create directory: " << path << std::endl;
+            return false;
+        }
+    }
+    return true;
+}
+
+bool ReadMeshVerticesNormals(const std::string& filename, Matrix3X& vertices, Matrix3X& normals)
+{
+    MeshType mesh;
+    OpenMesh::IO::Options opt = OpenMesh::IO::Options::VertexNormal;
+
+    if (!OpenMesh::IO::read_mesh(mesh, filename, opt)) {
+        std::cerr << "Failed to read mesh: " << filename << std::endl;
+        return false;
+    }
+
+    // 如果没有法向量，自动计算
+    if (!mesh.has_vertex_normals()) {
+        mesh.request_face_normals();
+        mesh.request_vertex_normals();
+        mesh.update_normals();
+    }
+
+    size_t n_vertices = mesh.n_vertices();
+    if (n_vertices == 0) {
+        std::cerr << "Error: mesh has no vertices!" << std::endl;
+        return false;
+    }
+
+    vertices.resize(3, n_vertices);
+    normals.resize(3, n_vertices);
+
+    for (auto v : mesh.vertices()) {
+        auto point = mesh.point(v);
+        auto normal = mesh.normal(v);
+        vertices.col(v.idx()) << point[0], point[1], point[2];
+        normals.col(v.idx()) << normal[0], normal[1], normal[2];
+    }
+
+    return true;
+}
+
+// ---------------------- main ----------------------
+int main()
+{
+    // 定义输出路径
+    std::string outpath0 = "./outpath0/";
+    std::string outpath1 = "./outpath1/";
+    std::string outpath2 = "./outpath2/";
+
+    // 创建输出文件夹
+    for (const auto& path : {outpath0, outpath1, outpath2}) {
+        if (!EnsureDirectoryExists(path)) return -1;
+    }
+
+    Matrix3X target_points, target_normals;
+    Matrix3X source_points, source_normals;
+    Matrix3X deformed_points;
+
+    // ------------------- 直接从文件注册 -------------------
+    {
+        RigidFricpRegistration fricp;
+        NonrigidSpareRegistration spare;
+
+        fricp.Reg("./data/target.ply","./data/source.ply", outpath0);
+        spare.Reg("./data/target.obj","./data/source.obj", outpath0);
+    } // fricp 和 spare 对象作用域结束，自动销毁
+
+    // ------------------- 带参数注册 -------------------
+    {
+        RigidFricpRegistration fricp;
+        NonrigidSpareRegistration spare;
+
+        fricp.Paras_init(false,"",80,1e-5);
+        fricp.Reg("./data/target.ply","./data/source.ply", outpath1);
+
+        spare.Paras_init(50,1e-4,1e-5);
+        spare.Reg("./data/target.obj","./data/source.obj", outpath1);
+    }
+
+    // ------------------- 使用顶点和法向量直接注册 -------------------
+    if (!ReadMeshVerticesNormals("./data/target.ply", target_points, target_normals)) return -1;
+    if (!ReadMeshVerticesNormals("./data/source.ply", source_points, source_normals)) return -1;
+
+    {
+        RigidFricpRegistration fricp;
+        fricp.Read_data(target_points, source_points, target_normals, source_normals);
+        fricp.Register();
+        deformed_points = fricp.deformed_points_3X_;
+        fricp.Output_data(outpath2, "fricp");
+    }
+
+    if (!ReadMeshVerticesNormals("./data/target.obj", target_points, target_normals)) return -1;
+    if (!ReadMeshVerticesNormals("./data/source.obj", source_points, source_normals)) return -1;
+
+    {
+        NonrigidSpareRegistration spare;
+        spare.Read_data(target_points, source_points, target_normals, source_normals);
+        spare.Register();
+        deformed_points = spare.deformed_points_3X_;
+        spare.Output_data(outpath2, "spare");
+    }
+
+    std::cout << "Registration finished successfully!" << std::endl;
+    return 0;
+}

examples/data/fricp/fricpm3trans.txt

@@ -0,0 +1,4 @@
+ 0.804418  0.359408 -0.473008 -0.156803
+-0.509781  0.826441 -0.238997  0.480169
+ 0.305016  0.433384  0.848023 0.0139896
+        0         0         0         1

examples/python/example_python.py

@@ -0,0 +1,172 @@
+import os
+import numpy as np
+import trimesh
+from plyfile import PlyData
+import pyregister
+
+
+def read_vertices_normals(file_path):
+    """
+    Read vertices and normals from a point cloud or mesh file.
+    Supports formats such as .ply, .obj, .xyz, etc.
+    """
+    ext = os.path.splitext(file_path)[1].lower()
+
+    if ext == ".ply":
+        plydata = PlyData.read(file_path)
+        vertex_data = plydata['vertex']
+        vertices = np.vstack([vertex_data['x'], vertex_data['y'], vertex_data['z']]).astype(np.float64)
+
+        normals = None
+        if {'nx', 'ny', 'nz'}.issubset(vertex_data.data.dtype.names):
+            normals = np.vstack([vertex_data['nx'], vertex_data['ny'], vertex_data['nz']]).astype(np.float64)
+
+    else:
+        mesh = trimesh.load(file_path, process=False)
+        vertices = mesh.vertices.T.astype(np.float64)
+        normals = getattr(mesh, "vertex_normals", None)
+        if normals is not None and len(normals) > 0:
+            normals = normals.T.astype(np.float64)
+        else:
+            normals = np.empty((0, 0))
+
+    return vertices, normals
+
+
+def run_rigid_file(file_target, file_source, output_path):
+    """
+    Perform rigid registration (FRICP method) by directly reading files.
+
+    Parameters
+    ----------
+    file_target : str
+        Path to the target point cloud or mesh file.
+    file_source : str
+        Path to the source point cloud or mesh file.
+    output_path : str
+        Output file path for the registered result.
+    """
+    print(f"\n[RIGID-FILE] Registering {file_source} → {file_target}")
+    reg = pyregister.RigidFricpRegistration()
+    reg.Paras_init(useinit=False, maxiter=100, stop=1e-5)
+    reg.Reg(file_target, file_source, output_path)
+    print(f"[RIGID-FILE] Completed → {output_path}")
+    print("Deformed points shape:", reg.deformed_points_3X_.shape)
+
+
+def run_rigid_numpy(target_pts, source_pts, target_n, source_n, output_path):
+    """
+    Perform rigid registration (FRICP method) using NumPy matrices.
+
+    Parameters
+    ----------
+    target_pts : np.ndarray, shape (3, N)
+        Target point cloud coordinates.
+    source_pts : np.ndarray, shape (3, N)
+        Source point cloud coordinates.
+    target_n : np.ndarray, shape (3, N) or (0, 0)
+        Target normals.
+    source_n : np.ndarray, shape (3, N) or (0, 0)
+        Source normals.
+    output_path : str
+        Output file path for the registered result.
+    """
+    print(f"\n[RIGID-NUMPY] Registering from NumPy matrices")
+    reg = pyregister.RigidFricpRegistration()
+    reg.Paras_init()
+    reg.Read_data(target_pts, source_pts, target_n, source_n)
+    reg.Register()
+    reg.Output_data(output_path, "FRICP")
+    print(f"[RIGID-NUMPY] Completed → {output_path}")
+    print("Deformed points shape:", reg.deformed_points_3X_.shape)
+
+
+def run_nonrigid_file(file_target, file_source, output_path):
+    """
+    Perform non-rigid registration (SPARE method) by directly reading files.
+
+    Parameters
+    ----------
+    file_target : str
+        Path to the target point cloud or mesh file.
+    file_source : str
+        Path to the source point cloud or mesh file.
+    output_path : str
+        Output file path for the registered result.
+    """
+    print(f"\n[NONRIGID-FILE] Registering {file_source} → {file_target}")
+    reg = pyregister.NonrigidSpareRegistration()
+    reg.Paras_init(iters = 30 ,stopcoarse=1e-3,stopfine=1e-4, uselandmark = False);
+    reg.Reg(file_target, file_source, output_path)
+    print(f"[NONRIGID-FILE] Completed → {output_path}")
+    print("Deformed points shape:", reg.deformed_points_3X_.shape)
+
+
+
+def run_nonrigid_numpy(target_pts, source_pts, target_n, source_n, output_path):
+    """
+    Perform non-rigid registration (SPARE method) using NumPy matrices.
+
+    Parameters
+    ----------
+    target_pts : np.ndarray, shape (3, N)
+        Target point cloud coordinates.
+    source_pts : np.ndarray, shape (3, N)
+        Source point cloud coordinates.
+    target_n : np.ndarray, shape (3, N) or (0, 0)
+        Target normals.
+    source_n : np.ndarray, shape (3, N) or (0, 0)
+        Source normals.
+    output_path : str
+        Output file path for the registered result.
+    """
+    print(f"\n[NONRIGID-NUMPY] Registering from NumPy matrices")
+    reg = pyregister.NonrigidSpareRegistration()
+    reg.Paras_init()
+    reg.Read_data(target_pts, source_pts, target_n, source_n)
+    reg.Register()
+    reg.Output_data(output_path, "SPARE")
+    print(f"[NONRIGID-NUMPY] Completed → {output_path}")
+    print("Deformed points shape:", reg.deformed_points_3X_.shape)
+
+
+
+def main():
+    folder = "data"
+    output_folder = os.path.join(folder, "results_register")
+    os.makedirs(output_folder, exist_ok=True)
+    target_ply = os.path.join(folder, "target.ply")
+
+    source_ply = os.path.join(folder, "source.ply")
+
+    target_obj = os.path.join(folder, "target.obj")
+
+    source_obj = os.path.join(folder, "source.obj")
+
+    # Rigid registration (file mode)
+    if os.path.exists(target_ply) and os.path.exists(source_ply):
+        run_rigid_file(target_ply, source_ply, os.path.join(output_folder, "rigid_file_result"))
+
+        # Load into NumPy and run in-memory registration
+        target_pts, target_n = read_vertices_normals(target_ply)
+        source_pts, source_n = read_vertices_normals(source_ply)
+        target_n = target_n if target_n is not None else np.empty((0, 0))
+        source_n = source_n if source_n is not None else np.empty((0, 0))
+        run_rigid_numpy(target_pts, source_pts, target_n, source_n,
+                        os.path.join(output_folder, "rigid_numpy_result"))
+
+    # Non-rigid registration (file mode)
+    if os.path.exists( target_obj ) and os.path.exists(source_obj):
+        run_nonrigid_file( target_obj , source_obj, os.path.join(output_folder, "nonrigid_file_result"))
+
+        # Load into NumPy and run in-memory registration
+        target_pts, target_n = read_vertices_normals( target_obj )
+        source_pts, source_n = read_vertices_normals(source_obj)
+        target_n = target_n if target_n is not None else np.empty((0, 0))
+        source_n = source_n if source_n is not None else np.empty((0, 0))
+        run_nonrigid_numpy(target_pts, source_pts, target_n, source_n,
+                           os.path.join(output_folder, "nonrigid_numpy_result"))
+
+
+if __name__ == "__main__":
+    main()

output/.gitkeep

@@ -0,0 +1 @@
+0

python/pyregister.cpp

@@ -0,0 +1,88 @@
+#include "rigid_fricp_registration.h"
+#include "nonrigid_spare_registration.h"  
+#include "registration.h"
+#include <pybind11/pybind11.h>
+#include <pybind11/eigen.h>
+#include <pybind11/stl.h>   // ✅ 支持 std::vector<int>
+#include <Eigen/Dense>
+
+namespace py = pybind11;
+
+// ======================= 模块定义 ==========================
+PYBIND11_MODULE(pyregister, m) {
+
+    // -------- Registration 基类 --------
+    py::class_<Registration>(m, "Registration")
+        .def("Read_data",
+             py::overload_cast<const std::string&, const std::string&>(&Registration::Read_data),
+             py::arg("file_target"),
+             py::arg("file_source"))
+        .def("Read_data",
+             [](Registration &self,
+                const Eigen::Ref<const Eigen::MatrixXd> &target_p,
+                const Eigen::Ref<const Eigen::MatrixXd> &source_p,
+                const Eigen::Ref<const Eigen::MatrixXd> &target_n,
+                const Eigen::Ref<const Eigen::MatrixXd> &source_n)
+             {
+                 std::cout << "[pybind] entering Registration::Read_data" << std::endl;
+
+                 Matrix3X tp = target_p.topRows(3);
+                 Matrix3X sp = source_p.topRows(3);
+                 Matrix3X tn = target_n.topRows(3);
+                 Matrix3X sn = source_n.topRows(3);
+
+                 self.Read_data(tp, sp, tn, sn);
+
+                 std::cout << "[pybind] leaving Registration::Read_data" << std::endl;
+             },
+             py::arg("target_p"),
+             py::arg("source_p"),
+             py::arg("target_n") = Eigen::MatrixXd(),
+             py::arg("source_n") = Eigen::MatrixXd()
+             )
+
+
+        .def("Register", &Registration::Register)
+        .def("Output_data", &Registration::Output_data,
+             py::arg("out_path"),
+             py::arg("method_name"));
+
+    // -------- 暴露 spare::Parameters --------
+    py::class_<spare::Parameters>(m, "SpareParameters")
+        .def(py::init<>())
+        .def_readwrite("use_landmark", &spare::Parameters::use_landmark)
+        .def_readwrite("landmark_src", &spare::Parameters::landmark_src)
+        .def_readwrite("landmark_tar", &spare::Parameters::landmark_tar);
+
+    // -------- Rigid FRICP Registration --------
+    py::class_<RigidFricpRegistration, Registration>(m, "RigidFricpRegistration")
+        .def(py::init<>())
+        .def("Reg", &RigidFricpRegistration::Reg,
+             py::arg("file_target"),
+             py::arg("file_source"),
+             py::arg("out_path"))
+        .def("Paras_init", &RigidFricpRegistration::Paras_init,
+             py::arg("useinit") = false,
+             py::arg("fileinit") = " ",
+             py::arg("maxiter") = 100,
+             py::arg("stop") = 1e-5)
+        .def_readwrite("deformed_points_3X_", &RigidFricpRegistration::deformed_points_3X_);
+
+    // -------- Nonrigid Spare Registration --------
+    py::class_<NonrigidSpareRegistration, Registration>(m, "NonrigidSpareRegistration")
+        .def(py::init<>())
+        .def("Reg", &NonrigidSpareRegistration::Reg,
+             py::arg("file_target"),
+             py::arg("file_source"),
+             py::arg("out_path"))
+        .def("Paras_init", &NonrigidSpareRegistration::Paras_init,
+     py::arg("iters") = 30,
+     py::arg("stopcoarse") = 1e-3,
+     py::arg("stopfine") = 1e-4,
+     py::arg("uselandmark") = false,
+     py::arg("src") = std::vector<int>(),
+     py::arg("tar") = std::vector<int>())
+
+        .def_readwrite("deformed_points_3X_", &NonrigidSpareRegistration::deformed_points_3X_)
+        .def_readwrite("paras", &NonrigidSpareRegistration::paras);  // ✅ 暴露 paras
+}

requirements.txt

@@ -0,0 +1,5 @@
+numpy
+gradio
+trimesh
+plotly
+plyfile

scripts/compile.sh

@@ -0,0 +1,5 @@
+cd ..
+mkdir build
+cd build
+cmake -DCMAKE_BUILD_TYPE=Release ..
+make -j4