Adding new cpp wrappers

.gitignore

@@ -112,6 +112,7 @@ venv/
 ENV/
 env.bak/
 venv.bak/
+.vscode
 
 # Spyder project settings
 .spyderproject

CPP_files/doc

@@ -0,0 +1 @@
+CPP files to wrap

CPP_files/pyransac.cpp

@@ -0,0 +1,175 @@
+#include <Python.h>
+#include <numpy/arrayobject.h>
+#include <iostream>
+#include <vector>
+#include <math.h>
+#include <random>
+#include <numeric>
+#include <algorithm>
+// #include <thread>
+// #include <omp.h>
+#define PY_SSIZE_T_CLEAN
+#define NPY_NO_DEPRECATED_API NPY_1_9_API_VERSION
+
+/*
+
+Author: Guilherme Ferrari Fortino
+
+Adaptation from https://github.com/jczamorac/Tracking_RANSAC
+
+*/
+
+extern "C"{
+
+#define M_PI 3.14159265358979323846
+
+double norm(double* A){
+	return (double)sqrt((double)pow(A[0], 2.) + (double)pow(A[1], 2.) + (double)pow(A[2], 2.));
+}
+
+void cross_prod(double* C, double* A, double* B){
+	C[0] = (double) A[1]*B[2] - A[2]*B[1];
+	C[1] = (double) -(A[0]*B[2] - A[2]*B[0]);
+	C[2] = (double) A[0]*B[1] - A[1]*B[0];
+}
+
+int ransac_line(std::vector<std::vector<double>>& data, PyObject* PyInliers, PyObject* PyVersors, PyObject* PyPoints, int number_it, double min_dist, int min_inlier){
+	int indice1, indice2, loop, j, size = data.size(), num_inliers_1 = 0, best = 0;
+	std::vector <int> parcial;
+	double versor[3], point[3];
+	std::vector<double> pesos;
+	double parcial_versor[3];
+	double norma1, norma2;
+	double point_A[3];
+	double point_B[3];
+	double aux[3];
+	double cross[3];
+	parcial.reserve(size);
+
+	for(int i = 0; i < number_it; i++){
+
+		indice1 = rand() % size;
+		indice2 = rand() % size;
+		while (indice1 == indice2) indice2 = rand() % size;
+		for(loop = 0; loop < 3; loop++){
+			point_A[loop]        = data[indice1][loop];
+			point_B[loop]        = data[indice2][loop];
+			parcial_versor[loop] = (double) point_B[loop] - point_A[loop];
+		}
+		norma1 = norm(parcial_versor);
+		for(loop = 0; loop < 3; loop++) parcial_versor[loop] = (double) parcial_versor[loop]/norma1;
+		for(j = 0; j < size; j++){
+			for(loop = 0; loop < 3; loop++) aux[loop] = point_A[loop] - data[j][loop];
+
+			cross_prod(cross, parcial_versor, aux);
+			norma2 = norm(cross);
+
+			if(fabs(norma2) <= min_dist){
+				// parcial.push_back(j);
+				num_inliers_1 += 1;
+			} 
+		}
+		if (num_inliers_1 >= min_inlier && num_inliers_1 > best){
+			best = num_inliers_1;
+			for (loop = 0; loop < 3; loop++){
+				versor[loop] = parcial_versor[loop];
+				point[loop] = point_A[loop];
+			}
+		}
+		num_inliers_1 = 0;
+	}
+
+	if (best == 0){
+		return -1;
+	}
+
+	for(j = 0; j < size; j++){
+		cross_prod(cross, versor, point);
+		norma2 = norm(cross);
+		if(fabs(norma2) <= min_dist) parcial.push_back(j); 
+	}					
+
+	// Guarda inliers, versor e ponto
+	for(int m = 0; m < parcial.size(); m++) PyList_Append(PyInliers, PyLong_FromLong((long int) parcial[m]));
+	for (loop = 0; loop < 3; loop++){
+		PyList_Append(PyVersors, PyFloat_FromDouble(versor[loop]));
+		PyList_Append(PyPoints, PyFloat_FromDouble(point[loop]));
+	}
+	return 0;
+}
+
+static PyObject* Ransac(PyObject* self, PyObject* args){
+	// PyObject* list;
+	PyArrayObject *p;
+	int number_it, min_inliers;
+	double min_dist;
+	NpyIter *in_iter;
+	// if(!PyArg_ParseTuple(args, "Oidii", &list, &number_it, &min_dist, &min_inliers, &mode)){
+	if(!PyArg_ParseTuple(args, "O!idi", &PyArray_Type, &p, &number_it, &min_dist, &min_inliers)){
+		return NULL;
+	}
+	if (PyArray_DESCR(p)->type_num != NPY_DOUBLE){
+		PyErr_SetString(PyExc_TypeError, "Type np.float64 expected for array.");
+		return NULL;
+	}
+
+    if (PyArray_NDIM(p)!=2){
+		PyErr_SetString(PyExc_TypeError, "Array must be two dimensional.");
+		return NULL;
+	}
+	// Py_ssize_t size = PyList_GET_SIZE(list);
+	int rows = PyArray_DIM(p, 0);
+	int cols = PyArray_DIM(p, 1);
+	if (cols != 3){
+		PyErr_SetString(PyExc_TypeError, "Array must have three columns.");
+		return NULL;
+	}
+	std::vector<std::vector<double>> data;
+	std::vector<long int> inliers;
+	in_iter = NpyIter_New(p, NPY_ITER_READONLY, NPY_KEEPORDER, NPY_NO_CASTING, NULL);
+	double ** in_dataptr = (double **) NpyIter_GetDataPtrArray(in_iter);
+	NpyIter_IterNextFunc *in_iternext = NpyIter_GetIterNext(in_iter, NULL);
+	// for(int i = 0; i < (int) size; i++){
+	for(int i = 0; i < rows; i++){
+		// PyObject* Point3D = PyList_GetItem(list, i);
+		std::vector<double> PartVec(rows);
+		for(int j = 0; j < cols; j++){
+			// PartVec[j] = PyFloat_AsDouble(PyList_GetItem(Point3D, j));
+			PartVec[j] = **in_dataptr;
+			in_iternext(in_iter);
+		}
+		// charge[i] = PyFloat_AsDouble(PyList_GetItem(Point3D, 3));
+		data.push_back(PartVec);
+	};
+	NpyIter_Deallocate(in_iter);
+	PyObject* PyInliers = PyList_New(0);
+	PyObject* PyVersor  = PyList_New(0);
+	PyObject* PyPb      = PyList_New(0);
+	ransac_line(data, PyInliers, PyVersor, PyPb, number_it, min_dist, min_inliers);
+	PyObject * NPInliers = PyArray_FROM_OTF(PyInliers, NPY_INT, NPY_ARRAY_IN_ARRAY);
+	Py_DecRef(PyInliers);
+	return Py_BuildValue("(OOO)", NPInliers,
+	 PyArray_FROM_OTF(PyVersor, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY),
+	 PyArray_FROM_OTF(PyPb, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY));
+}
+
+static PyMethodDef myMethods[] = {
+	{"ransac_line", Ransac, METH_VARARGS, "3D Line Ransac CPP implementation."},
+    {NULL, NULL, 0, NULL}
+};
+
+static struct PyModuleDef pyransac3d_wrapper = {
+    PyModuleDef_HEAD_INIT,
+    "pyransac3d_wrapper",
+    "Wrapper for pyransac3D.",
+    -1,
+    myMethods    
+};
+
+PyMODINIT_FUNC PyInit_pyransac3d_wrapper(void){
+	import_array();
+    return PyModule_Create(&pyransac3d_wrapper);
+}
+
+
+}

pyransac3d/__init__.py

@@ -4,7 +4,8 @@
 from .plane import Plane
 from .cuboid import Cuboid
 from .line import Line
+from .line_cpp import Line_cpp
 from .circle import Circle
 from .point import Point
 from .sphere import Sphere
-#from pyRANSAC_3D import Cylinder, Cuboid, Plane
+#from pyRANSAC_3D import Cylinder, Cuboid, Plane

pyransac3d/line_cpp.py

@@ -0,0 +1,59 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Sun Jan 10 17:58:45 2021
+
+@author: Guilherme Ferrari Fortino
+
+Adaptation from https://github.com/jczamorac/Tracking_RANSAC
+
+"""
+
+import numpy as np
+import pyransac3d_wrapper as pyrsc_w
+
+class Line_cpp:
+    """ 
+    Implementation for 3D Line RANSAC.
+
+    This object finds the equation of a line in 3D space using RANSAC method. 
+    This method uses 2 points from 3D space and computes a line. The selected candidate will be the line with more inliers inside the radius theshold. 
+
+    ![3D line](https://raw.githubusercontent.com/leomariga/pyRANSAC-3D/master/doc/line.gif "3D line")
+
+    ---
+    """
+
+    def __init__(self):
+        self.inliers = []
+        self.A = []
+        self.B = []
+
+    def fit(self, pts, thresh=0.2, maxIteration=1000):
+        """ 
+        Find the best equation for the 3D line. The line in a 3d enviroment is defined as y = Ax+B, but A and B are vectors intead of scalars.
+
+        :param pts: 3D point cloud as a `np.array (N,3)`.
+        :param thresh: Threshold distance from the line which is considered inlier.
+        :param maxIteration: Number of maximum iteration which RANSAC will loop over.
+        :returns:
+        - `A`: 3D slope of the line (angle) `np.array (1, 3)`
+        - `B`: Axis interception as `np.array (1, 3)`
+        - `inliers`: Inlier's index from the original point cloud. `np.array (1, M)`
+        ---
+        """
+        self.inliers, self.A, self.B = pyrsc_w.ransac_line(pts, maxIteration, thresh, 2)
+        return self.A, self.B, self.inliers
+
+
+
+
+
+
+
+
+
+
+
+
+

setup.py

@@ -1,11 +1,18 @@
 import setuptools
+import os
+import numpy
+
+BASEDIR  = os.path.abspath(os.path.dirname(__file__))
+CPP_FILE = os.path.join(BASEDIR, 'CPP_files', 'pyransac.cpp')
+
+module = setuptools.Extension("pyransac3d_wrapper", sources = [CPP_FILE], include_dirs=[numpy.get_include()])
 
 with open("README.md", "r") as fh:
     long_description = fh.read()
 
 setuptools.setup(
     name="pyransac3d", # Replace with your own username
-    version="0.5.0",
+    version="0.6.1",
     author="Leonardo Mariga",
     author_email="[email protected]",
     description="A python tool for fitting primitives 3D shapes in point clouds using RANSAC algorithm ",
@@ -24,4 +31,5 @@
         "Operating System :: OS Independent",
     ],
     python_requires='>=3.6',
+    ext_modules=[module]
 )

tests/test_line_cpp.py

@@ -0,0 +1,34 @@
+import open3d as o3d
+import numpy as np
+import random
+import copy 
+import pyransac3d as pyrsc
+
+
+print('create noisy mesh')
+mesh_in = o3d.geometry.TriangleMesh.create_cylinder(radius=1, height=500.0)
+vertices = np.asarray(mesh_in.vertices)
+noise = 15
+vertices += np.random.logistic(0,noise, size=vertices.shape)
+mesh_in.vertices = o3d.utility.Vector3dVector(vertices)
+mesh_in.compute_vertex_normals()
+mesh_in.paint_uniform_color([0.2, 0.2, 0.8])
+o3d.visualization.draw_geometries([mesh_in])
+pcd_load=mesh_in.sample_points_uniformly(number_of_points=2000)
+o3d.visualization.draw_geometries([pcd_load])
+
+points = np.asarray(pcd_load.points)
+
+line = pyrsc.Line_cpp()
+
+A, B, inliers = line.fit(points, thresh=15)
+
+R = pyrsc.get_rotationMatrix_from_vectors([0, 0, 1], A)
+plane = pcd_load.select_by_index(inliers).paint_uniform_color([1, 0, 0])
+
+mesh_cylinder = o3d.geometry.TriangleMesh.create_cylinder(radius=1, height=1000)
+mesh_cylinder.compute_vertex_normals()
+mesh_cylinder.paint_uniform_color([1, 0, 0])
+mesh_cylinder = mesh_cylinder.rotate(R, center=[0, 0, 0])
+mesh_cylinder = mesh_cylinder.translate((B[0], B[1], B[2]))
+o3d.visualization.draw_geometries([pcd_load, plane, mesh_cylinder])