diff --git a/ros-aicp-mapping/DESCR b/ros-aicp-mapping/DESCR
new file mode 100644
index 0000000000000000000000000000000000000000..6fcc6ca3d61a288c6bae75c11fa3fc5baf3751f4
--- /dev/null
+++ b/ros-aicp-mapping/DESCR
@@ -0,0 +1 @@
+Auto-tuned Iterative Closest Point (AICP) is a module for laser-based localization and mapping (Nobili et al., ICRA 2017). The implementation of AICP includes a module for localization failure prediction (Nobili et al., ICRA 2018). The registration strategy is based on the libpointmatcher framework (Pomerleau et al., AR 2012).
diff --git a/ros-aicp-mapping/Makefile b/ros-aicp-mapping/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..5b5bc51d537a44788407887f28c502bca4de5414
--- /dev/null
+++ b/ros-aicp-mapping/Makefile
@@ -0,0 +1,31 @@
+# robotpkg Makefile for: wip/ros-aicp-mapping
+# Created: Guilhem Saurel on Fri, 24 Jan 2020
+#
+
+ROS_PKG= aicp_mapping
+ROS_METAPKG= yes
+ROS_VERSION= 0.0.1
+ROS_REPO= ori-drs
+
+HOMEPAGE= ${MASTER_SITE_GITHUB:=${ROS_REPO}/${ROS_PKG}}
+MASTER_SITES= ${HOMEPAGE}/archive/
+MASTER_REPOSITORY= git ${HOMEPAGE}.git
+
+CATEGORIES= wip
+ROS_COMMENT= Auto-tuned Iterative Closest Point -- AICP
+
+include ../../meta-pkgs/ros-base/Makefile.common
+
+include ../../wip/libnabo/depend.mk
+include ../../wip/libpointmatcher/depend.mk
+
+include ../../interfaces/ros-common-msgs/depend.mk
+include ../../interfaces/ros-std-msgs/depend.mk
+include ../../lang/ros-message-generation/depend.mk
+include ../../math/ros-geometry/depend.mk
+include ../../middleware/ros-comm/depend.mk
+
+include ../../devel/ros-catkin/depend.mk
+include ../../mk/sysdep/python.mk
+include ../../mk/language/c++11.mk
+include ../../mk/robotpkg.mk
diff --git a/ros-aicp-mapping/PLIST b/ros-aicp-mapping/PLIST
new file mode 100644
index 0000000000000000000000000000000000000000..3f6bbfbe07fea40628cf357e9e94967af003f2ee
--- /dev/null
+++ b/ros-aicp-mapping/PLIST
@@ -0,0 +1,50 @@
+@comment Fri Jan 24 19:27:04 CET 2020
+include/aicp_core/abstract_classification.hpp
+include/aicp_core/abstract_overlapper.hpp
+include/aicp_core/abstract_registrator.hpp
+include/aicp_core/aligned_cloud.hpp
+include/aicp_core/aligned_clouds_graph.hpp
+include/aicp_core/app.hpp
+include/aicp_core/classification.hpp
+include/aicp_core/common.hpp
+include/aicp_core/filteringUtils.hpp
+include/aicp_core/octrees_overlap.hpp
+include/aicp_core/overlap.hpp
+include/aicp_core/pointmatcher_registration.hpp
+include/aicp_core/poseFileReader.hpp
+include/aicp_core/registration.hpp
+include/aicp_core/svm.hpp
+include/aicp_core/timing.hpp
+include/aicp_core/visualizer.hpp
+include/aicp_core/yaml_configurator.hpp
+include/aicp_ros/app_ros.hpp
+include/aicp_ros/talker_ros.hpp
+include/aicp_ros/velodyne_accumulator.hpp
+include/aicp_ros/visualizer_ros.hpp
+include/aicp_srv/ProcessFile.h
+include/aicp_srv/ProcessFileRequest.h
+include/aicp_srv/ProcessFileResponse.h
+lib/libaicp_core.so
+lib/libaicp_ros.so
+lib/pkgconfig/aicp_core.pc
+lib/pkgconfig/aicp_ros.pc
+lib/pkgconfig/aicp_srv.pc
+${PYTHON_SITELIB}/aicp_srv/__init__.py
+${PYTHON_SITELIB}/aicp_srv/srv/_ProcessFile.py
+${PYTHON_SITELIB}/aicp_srv/srv/__init__.py
+share/aicp_core/cmake/aicp_coreConfig-version.cmake
+share/aicp_core/cmake/aicp_coreConfig.cmake
+share/aicp_core/package.xml
+share/aicp_ros/cmake/aicp_rosConfig-version.cmake
+share/aicp_ros/cmake/aicp_rosConfig.cmake
+share/aicp_ros/package.xml
+share/aicp_srv/cmake/aicp_srv-msg-extras.cmake
+share/aicp_srv/cmake/aicp_srv-msg-paths.cmake
+share/aicp_srv/cmake/aicp_srvConfig-version.cmake
+share/aicp_srv/cmake/aicp_srvConfig.cmake
+share/aicp_srv/package.xml
+share/aicp_srv/srv/ProcessFile.srv
+share/common-lisp/ros/aicp_srv/srv/ProcessFile.lisp
+share/common-lisp/ros/aicp_srv/srv/_package.lisp
+share/common-lisp/ros/aicp_srv/srv/_package_ProcessFile.lisp
+share/common-lisp/ros/aicp_srv/srv/aicp_srv-srv.asd
diff --git a/ros-aicp-mapping/depend.mk b/ros-aicp-mapping/depend.mk
new file mode 100644
index 0000000000000000000000000000000000000000..5489c1dc97202e97762361b06cf8b35514fe5b2b
--- /dev/null
+++ b/ros-aicp-mapping/depend.mk
@@ -0,0 +1,37 @@
+# robotpkg depend.mk for: wip/ros-aicp-mapping
+# Created: Guilhem Saurel on Fri, 24 Jan 2020
+#
+
+DEPEND_DEPTH:= ${DEPEND_DEPTH}+
+ROS_AICP_MAPPING_DEPEND_MK:= ${ROS_AICP_MAPPING_DEPEND_MK}+
+
+ifeq (+,$(DEPEND_DEPTH))
+DEPEND_PKG+= ros-aicp-mapping
+endif
+
+ifeq (+,$(ROS_AICP_MAPPING_DEPEND_MK)) # --------------------------------------
+
+include ../../meta-pkgs/ros-base/depend.common
+include ../../mk/robotpkg.prefs.mk
+ifeq (18.04,${OS_VERSION})
+ PREFER.ros-aicp-mapping?= robotpkg
+else
+ PREFER.ros-aicp-mapping?= ${PREFER.ros-base}
+ SYSTEM_PREFIX.ros-aicp-mapping?= ${SYSTEM_PREFIX.ros-base}
+endif
+
+DEPEND_USE+= ros-aicp-mapping
+ROS_DEPEND_USE+= ros-aicp-mapping
+
+DEPEND_ABI.ros-aicp-mapping?= ros-aicp-mapping>=0.0.1
+DEPEND_DIR.ros-aicp-mapping?= ../../wip/ros-aicp-mapping
+
+SYSTEM_SEARCH.ros-aicp-mapping=\
+ include/aicp_core/common.hpp \
+ 'lib/pkgconfig/aicp_core.pc:/Version/s/[^0-9.]//gp'
+
+CMAKE_PREFIX_PATH.ros-aicp-mapping= ${PREFIX.ros-aicp-mapping}
+
+endif # ROS_AICP_MAPPING_DEPEND_MK --------------------------------------------
+
+DEPEND_DEPTH:= ${DEPEND_DEPTH:+=}
diff --git a/ros-aicp-mapping/distinfo b/ros-aicp-mapping/distinfo
new file mode 100644
index 0000000000000000000000000000000000000000..ac5ad3e067baa61b7224bdd1602ecbc1019dafc6
--- /dev/null
+++ b/ros-aicp-mapping/distinfo
@@ -0,0 +1,4 @@
+SHA1 (ros/aicp_mapping/0.0.1.tar.gz) = a03b4f7338ad7af573fbe9e5418e3c1d2bc90a16
+RMD160 (ros/aicp_mapping/0.0.1.tar.gz) = 2100555f38ca65b1b0af43486c9e6dc8b92fd429
+Size (ros/aicp_mapping/0.0.1.tar.gz) = 7201053 bytes
+SHA1 (patch-aa) = 6b56a225faf9f008a290b21fa2657cda020dc18a
diff --git a/ros-aicp-mapping/patches/patch-aa b/ros-aicp-mapping/patches/patch-aa
new file mode 100644
index 0000000000000000000000000000000000000000..586cb39daa5abf8cb58ecc75865ab61c7bbf7eed
--- /dev/null
+++ b/ros-aicp-mapping/patches/patch-aa
@@ -0,0 +1,33 @@
+create() provides an std::shared_ptr instead of a ptr.
+
+--- aicp_core/src/utils/icpMonitor.cpp.orig 2019-05-05 17:52:19.000000000 +0200
++++ aicp_core/src/utils/icpMonitor.cpp 2020-01-24 19:24:13.338056526 +0100
+@@ -26,7 +26,7 @@
+ params["knn"] = toParam(1); // for Hausdorff distance, we only need the first closest point
+ params["epsilon"] = toParam(0);
+
+- PM::Matcher* matcherHausdorff = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
++ auto matcherHausdorff = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
+
+ float quantile = 0.60;
+
+@@ -102,7 +102,7 @@
+ params["knn"] = toParam(1); // for Hausdorff distance, we only need the first closest point
+ params["epsilon"] = toParam(0);
+
+- PM::Matcher* matcher = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
++ auto matcher = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
+
+ // from reading to reference
+ matcher->init(A);
+--- aicp_core/src/registration/pointmatcher_registration.cpp.orig 2019-05-05 17:52:19.000000000 +0200
++++ aicp_core/src/registration/pointmatcher_registration.cpp 2020-01-24 19:24:44.765800055 +0100
+@@ -72,7 +72,7 @@
+ {
+ PM::TransformationParameters init_transform = parseTransformationDeg(params_.pointmatcher.initialTransform, 3);
+
+- PM::Transformation* rigid_transform = PM::get().REG(Transformation).create("RigidTransformation");
++ auto rigid_transform = PM::get().REG(Transformation).create("RigidTransformation");
+
+ if (!rigid_transform->checkParameters(init_transform)) {
+ cerr << endl