Merge pull request #1 from elephantrobotics/humble-imu

Add IMU support, auto recharge, RViz tool, and rtabmap demos

agv_pro_autocharge/README.md

@@ -0,0 +1,134 @@
+# AGV AutoCharge ROS2 Package
+
+这是一个为ROS2 Humble设计的AGV自动充电系统软件包。
+
+## 功能特性
+
+- 监听充电桩位置更新
+- 持续发布可视化标记
+- 键盘交互启动导航功能
+- 导航成功后启动串口控制
+- 支持充电状态检测和控制
+
+## 安装依赖
+
+确保您的系统已安装以下依赖：
+
+```bash
+# ROS2 Humble基础包
+sudo apt install ros-humble-rclpy
+sudo apt install ros-humble-geometry-msgs
+sudo apt install ros-humble-std-msgs
+sudo apt install ros-humble-nav-msgs
+sudo apt install ros-humble-visualization-msgs
+sudo apt install ros-humble-nav2-simple-commander
+
+# Python依赖
+pip3 install pyserial
+```
+
+## 编译安装
+
+```bash
+# 进入ROS2工作空间
+cd /path/to/your/ros2_ws/src
+
+# 复制软件包到工作空间
+cp -r agv_autocharge_ros2 .
+
+# 编译软件包
+cd ..
+colcon build --packages-select agv_autocharge_ros2
+
+# 加载环境变量
+source install/setup.bash
+```
+
+## 使用方法
+
+### 启动节点
+
+```bash
+# 启动自动充电控制器节点
+ros2 run agv_autocharge_ros2 combined_auto_recharger
+```
+
+### 节点功能
+
+- 监听 `/charger_position_update` 话题，接收充电桩位置更新
+- 发布 `/goal_marker` 话题，在RViz中显示充电桩位置标记
+- 发布 `/cmd_vel` 话题，控制机器人运动
+- 按键 `q` 启动导航到充电桩
+- 导航成功后自动启动串口控制
+
+### 配置文件
+
+充电桩位置配置文件位于：
+```
+config/charger_position.json
+```
+
+文件格式：
+```json
+{
+  "p_x": 1.329015495451769,
+  "p_y": 0.31961151635354823,
+  "orien_z": 0.4981823289472456,
+  "orien_w": 0.8670722963655905
+}
+```
+
+### 话题接口
+
+#### 订阅话题
+- `/charger_position_update` (geometry_msgs/PoseStamped): 充电桩位置更新
+
+#### 发布话题
+- `/goal_marker` (visualization_msgs/MarkerArray): 充电桩位置可视化标记
+- `/cmd_vel` (geometry_msgs/Twist): 机器人运动控制
+- `/chassis_security` (std_msgs/Int8): 底盘安全控制
+
+### 串口配置
+
+默认串口配置：
+- 端口: `/dev/ttyCH341USB0`
+- 波特率: 9600
+- 超时: 1秒
+
+可以根据需要修改代码中的串口参数。
+
+## 操作说明
+
+1. 启动节点后，系统会自动加载充电桩位置配置
+2. 系统会定期发布充电桩标记到RViz进行可视化
+3. 按下键盘上的 `q` 键启动导航到充电桩
+4. 导航成功后，系统会自动启动串口控制功能
+5. 串口控制会根据接收到的数据控制机器人运动
+6. 按 `Ctrl+C` 退出程序
+
+## 故障排除
+
+### 常见问题
+
+1. **串口无法打开**
+   - 检查串口设备是否连接
+   - 确认串口权限设置
+   - 验证串口设备名称
+
+2. **导航失败**
+   - 确认Nav2导航系统正常运行
+   - 检查充电桩位置配置是否正确
+   - 验证地图和定位系统状态
+
+3. **RViz中看不到标记**
+   - 确认RViz已订阅 `/goal_marker` 话题
+   - 检查MarkerArray显示设置
+   - 验证坐标系设置是否为'map'
+
+## 许可证
+
+MIT License
+
+## 维护者
+
+请联系维护者获取技术支持。

agv_pro_autocharge/agv_pro_autocharge/__init__.py

@@ -0,0 +1,10 @@
+"""
+AGV AutoCharge ROS2 Package
+
+This package provides automatic charging functionality for AGV robots using ROS2 Humble.
+It includes position management, visualization, navigation, and serial control features.
+"""
+
+__version__ = '1.0.0'
+__author__ = 'Your Name'
+__email__ = 'your-email@example.com'

agv_pro_autocharge/agv_pro_autocharge/combined_auto_recharger.py

@@ -0,0 +1,612 @@
+#!/usr/bin/env python3 
+# coding=utf-8
+
+"""
+合并的自动充电控制器 - 结合位置管理、可视化和导航功能
+- 监听充电桩位置更新
+- 持续发布可视化标记
+- 按键'q'启动导航功能
+- 导航成功后启动串口控制
+"""
+
+# 引用ros库
+import rclpy
+from rclpy.node import Node
+from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
+from rclpy.duration import Duration
+
+# 用到的变量定义
+from std_msgs.msg import Bool 
+from std_msgs.msg import Int8 
+from std_msgs.msg import UInt8
+from std_msgs.msg import Float32
+
+# 用于记录充电桩位置、发布导航点
+from geometry_msgs.msg import PoseStamped, Twist
+
+# rviz可视化相关
+from visualization_msgs.msg import Marker
+from visualization_msgs.msg import MarkerArray
+
+# 里程计话题相关
+from nav_msgs.msg import Odometry
+
+# 键盘控制相关
+import sys
+import select
+import termios
+import tty
+
+# 延迟相关
+import time
+import threading
+
+# 读写充电桩位置文件
+import json
+import math
+import yaml
+import os
+
+# 导入串口解析模块
+from .serial_can_parser import SerialCANParser
+
+# 存放充电桩位置的文件位置 - 参考原始auto_recharger.py的路径设置方式
+def find_config_files():
+    """查找配置文件路径"""
+    # 首先尝试几个可能的位置
+    possible_paths = [
+        # 开发环境路径
+        '/home/elephant/agv_pro_ros2/src/agv_pro_autocharge/config',
+        # 你的工作空间路径
+        '/home/elephant/agv_pro_ros2/src/agv_pro_autocharge/config',
+        # 当前包的相对路径
+        os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), 'config'),
+        # 安装路径
+        '/home/elephant/agv_pro_ros2/install/agv_pro_autocharge/share/agv_pro_autocharge/config'
+    ]
+    
+    for config_dir in possible_paths:
+        yaml_path = os.path.join(config_dir, 'nav_goal_params.yaml')
+        json_path = os.path.join(config_dir, 'charger_position.json')
+        
+        print(f"Checking config directory: {config_dir}")
+        if os.path.exists(yaml_path) and os.path.exists(json_path):
+            print(f"Found config files in: {config_dir}")
+            return yaml_path, json_path
+    
+    # 如果都找不到，直接报错
+    print("ERROR: Could not find config files in any of the following locations:")
+    for path in possible_paths:
+        print(f"  - {path}")
+    print("Please ensure the config files exist in one of these directories.")
+    
+    # 返回第一个路径作为默认值，但文件可能不存在
+    return os.path.join(possible_paths[0], 'nav_goal_params.yaml'), os.path.join(possible_paths[0], 'charger_position.json')
+
+# 获取配置文件路径
+yaml_file, json_file = find_config_files()
+
+# print_and_fixRetract相关，用于打印带颜色的信息
+RESET = '\033[0m'
+RED   = '\033[1;31m'
+GREEN = '\033[1;32m'
+YELLOW= '\033[1;33m'
+BLUE  = '\033[1;34m'
+PURPLE= '\033[1;35m'
+CYAN  = '\033[1;36m'
+
+# 圆周率
+PI = 3.1415926535897
+
+if os.name == 'nt':
+    import msvcrt
+else:
+    import termios
+    import tty
+	
+settings = None
+if os.name != 'nt' and sys.stdin.isatty():
+    settings = list(termios.tcgetattr(sys.stdin))
+
+def get_key(settings):
+    if os.name == 'nt':
+        return msvcrt.getch().decode('utf-8')
+    else:
+        if sys.stdin.isatty():
+            tty.setraw(sys.stdin.fileno())
+        rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
+        if rlist:
+            key = sys.stdin.read(1)
+        else:
+            key = ''
+        if sys.stdin.isatty() and settings:
+            termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+        return key
+
+def print_and_fixRetract(str):
+    global settings
+    '''键盘控制会导致回调函数内使用print()出现自动缩进的问题，此函数可以解决该现象'''
+    if sys.stdin.isatty() and settings:
+        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+    print(str)
+
+class CombinedAutoRecharger(Node):
+    def __init__(self):
+        
+        # 创建节点
+        super().__init__("combined_auto_recharger")
+
+        print_and_fixRetract('Combined Auto Recharger Node Started!')
+
+        # 导航状态标记
+        self.navigation_active = False
+        
+        # 串口控制相关
+        self.parser = None
+        self.serial_control_active = False
+        self.navigation_requested = False  # 添加导航请求标志
+        
+        # 创建导航器
+        self.navigator = BasicNavigator()
+
+        # 加载充电桩位置信息
+        self.load_charger_position()
+        # 加载导航参数
+        self.load_nav_goal_params()
+
+        # 创建发布者
+        self.robot_security_off_pub = self.create_publisher(Int8, '/chassis_security', 10) 
+        self.Charger_marker_pub = self.create_publisher(MarkerArray, '/goal_marker', 10)
+        self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
+
+        # 创建订阅者 - 只订阅充电桩位置更新
+        self.Charger_Position_Update_sub = self.create_subscription(
+            PoseStamped, "/charger_position_update", 
+            self.Position_Update_callback, 10)
+
+        # 创建定时器，定期发布充电桩标记（每2秒发布一次）
+        self.marker_timer = self.create_timer(2.0, self.timer_callback)
+        
+        # 创建导航检查定时器（每0.5秒检查一次导航请求）
+        self.navigation_timer = self.create_timer(0.5, self.check_navigation_request)
+
+        # 发布初始充电桩位置标记
+        self.update_charger_visualization()
+
+        print_and_fixRetract('Combined auto recharger node initialized successfully!')
+        print_and_fixRetract(f'{GREEN}Press "q" to start navigation, Ctrl+C to exit{RESET}')
+
+    def load_nav_goal_params(self):
+        """加载导航目标参数（前方距离和角度）"""
+        print_and_fixRetract(f"Attempting to load nav goal params from: {yaml_file}")
+        print_and_fixRetract(f"File exists? {os.path.exists(yaml_file)}")
+        
+        try:
+            with open(yaml_file, 'r', encoding='utf-8') as f:
+                params = yaml.safe_load(f)
+            
+            print_and_fixRetract(f"Raw params from file: {params}")
+            
+            self.forward_distance = float(params.get('forward_distance', 1.0))
+            self.yaw_offset_deg = float(params.get('yaw_offset_deg', 0.0))
+            
+            print_and_fixRetract(f"Successfully loaded nav goal params: forward_distance={self.forward_distance}, yaw_offset_deg={self.yaw_offset_deg}")
+            
+        except FileNotFoundError:
+            print_and_fixRetract(f"{RED}Nav goal params file not found: {yaml_file}{RESET}")
+            print_and_fixRetract(f"{RED}Please create the configuration file with the required parameters{RESET}")
+            # 使用默认值
+            self.forward_distance = 1.0
+            self.yaw_offset_deg = 0.0
+        except Exception as e:
+            print_and_fixRetract(f"{RED}Failed to load nav goal params: {e}{RESET}")
+            self.forward_distance = 1.0
+            self.yaw_offset_deg = 0.0
+
+    def timer_callback(self):
+        '''定时器回调函数，定期发布充电桩标记'''
+        # 始终发布当前JSON文件中的位置信息
+        if hasattr(self, 'json_data') and self.json_data:
+            self.Pub_Charger_marker(
+                self.json_data['p_x'],
+                self.json_data['p_y'], 
+                self.json_data['orien_z'],
+                self.json_data['orien_w']
+            )
+
+    def check_navigation_request(self):
+        '''检查是否有导航请求'''
+        if self.navigation_requested and not self.navigation_active:
+            self.navigation_requested = False
+            self.navigation_active = True
+            print_and_fixRetract(f"{BLUE}Processing navigation request...{RESET}")
+            
+            # 在ROS2线程中执行导航
+            result = self.execute_navigation_internal()
+            
+            if result:
+                print_and_fixRetract(f"{GREEN}Navigation successful! Starting serial control...{RESET}")
+                # 在ROS2线程中启动串口控制
+                self.start_serial_control_async()
+            else:
+                print_and_fixRetract(f"{RED}Navigation failed{RESET}")
+                self.navigation_active = False
+
+    def request_navigation(self):
+        '''请求开始导航'''
+        if not self.navigation_active:
+            self.navigation_requested = True
+            print_and_fixRetract(f"{BLUE}Navigation request queued...{RESET}")
+        else:
+            print_and_fixRetract(f"{YELLOW}Navigation already in progress{RESET}")
+
+    def load_charger_position(self):
+        '''加载充电桩位置信息'''
+        try:
+            with open(json_file, 'r', encoding='utf-8') as fp:
+                self.json_data = json.load(fp)
+            print_and_fixRetract(f"Loaded charger position: x={self.json_data['p_x']:.3f}, y={self.json_data['p_y']:.3f}")
+        except FileNotFoundError:
+            print_and_fixRetract(f"{RED}Charger position file {json_file} not found{RESET}")
+            print_and_fixRetract(f"{RED}Please create the configuration file with default charger position{RESET}")
+            # 使用默认位置
+            self.json_data = {
+                'p_x': 0.0,
+                'p_y': 0.0, 
+                'orien_z': 0.0,
+                'orien_w': 1.0
+            }
+        except Exception as e:
+            print_and_fixRetract(f"Error loading charger position: {e}")
+            self.json_data = {
+                'p_x': 0.0,
+                'p_y': 0.0,
+                'orien_z': 0.0, 
+                'orien_w': 1.0
+            }
+
+    def save_charger_position(self):
+        '''保存充电桩位置信息到JSON文件'''
+        try:
+            with open(json_file, 'w', encoding='utf-8') as fp:
+                json.dump(self.json_data, fp, ensure_ascii=False, indent=2)
+            print_and_fixRetract(f"{GREEN}Charger position saved to {json_file}{RESET}")
+        except Exception as e:
+            print_and_fixRetract(f"{RED}Error saving charger position: {e}{RESET}")
+
+    def Pub_Charger_Position(self):
+        '''更新充电桩位置信息并保存到JSON文件'''
+        # 发布充电桩位置的可视化
+        self.Pub_Charger_marker(
+            self.json_data['p_x'], 
+            self.json_data['p_y'], 
+            self.json_data['orien_z'], 
+            self.json_data['orien_w'])
+        
+        # 保存当前充电桩位置到JSON文件
+        position_data = {
+            'p_x': self.json_data['p_x'],
+            'p_y': self.json_data['p_y'],
+            'orien_z': self.json_data['orien_z'],
+            'orien_w': self.json_data['orien_w']
+        }
+        
+        self.json_data = position_data
+        self.save_charger_position()
+        print_and_fixRetract(f"Position: x={self.json_data['p_x']:.3f}, y={self.json_data['p_y']:.3f}")
+
+    def Pub_Charger_marker(self, p_x, p_y, o_z, o_w):
+        '''发布目标点可视化话题'''
+        
+        markerArray = MarkerArray()
+        
+        # 获取当前时间戳
+        current_time = self.get_clock().now().to_msg()
+
+        marker_shape = Marker()  # 创建marker对象
+        marker_shape.id = 0  # 必须赋值id
+        marker_shape.header.frame_id = 'map'  # 以哪一个TF坐标为原点
+        marker_shape.header.stamp = current_time  # 添加时间戳
+        marker_shape.type = Marker.ARROW  # TEXT_VIEW_FACING #一直面向屏幕的字符格式
+        marker_shape.action = Marker.ADD  # 添加marker
+        marker_shape.scale.x = 0.5  # marker大小
+        marker_shape.scale.y = 0.05  # marker大小
+        marker_shape.scale.z = 0.05  # marker大小，对于字符只有z起作用
+        marker_shape.pose.position.x = p_x  # 字符位置
+        marker_shape.pose.position.y = p_y  # 字符位置
+        marker_shape.pose.position.z = 0.1  # msg.position.z #字符位置
+        marker_shape.pose.orientation.z = o_z  # 字符位置
+        marker_shape.pose.orientation.w = o_w  # 字符位置
+        marker_shape.color.r = 1.0  # 字符颜色R(红色)通道
+        marker_shape.color.g = 0.0  # 字符颜色G(绿色)通道
+        marker_shape.color.b = 0.0  # 字符颜色B(蓝色)通道
+        marker_shape.color.a = 1.0  # 字符透明度
+        markerArray.markers.append(marker_shape)  # 添加元素进数组
+        
+        marker_string = Marker()  # 创建marker对象
+        marker_string.id = 1  # 必须赋值id
+        marker_string.header.frame_id = 'map'  # 以哪一个TF坐标为原点
+        marker_string.header.stamp = current_time  # 添加时间戳
+        marker_string.type = Marker.TEXT_VIEW_FACING  # 一直面向屏幕的字符格式
+        marker_string.action = Marker.ADD  # 添加marker
+        marker_string.scale.x = 0.5  # marker大小
+        marker_string.scale.y = 0.5  # marker大小
+        marker_string.scale.z = 0.5  # marker大小，对于字符只有z起作用
+        marker_string.color.a = 1.0  # 字符透明度
+        marker_string.color.r = 1.0  # 字符颜色R(红色)通道
+        marker_string.color.g = 0.0  # 字符颜色G(绿色)通道
+        marker_string.color.b = 0.0  # 字符颜色B(蓝色)通道
+        marker_string.pose.position.x = p_x  # 字符位置
+        marker_string.pose.position.y = p_y  # 字符位置
+        marker_string.pose.position.z = 0.1  # msg.position.z #字符位置
+        marker_string.pose.orientation.z = o_z  # 字符位置
+        marker_string.pose.orientation.w = o_w  # 字符位置
+        marker_string.text = 'Charger'  # 字符内容
+        markerArray.markers.append(marker_string)  # 添加元素进数组
+        self.Charger_marker_pub.publish(markerArray)  # 发布markerArray，rviz订阅并进行可视化
+
+    def Position_Update_callback(self, topic):
+        '''更新json文件中的充电桩位置'''
+        position_dic = {'p_x': 0, 'p_y': 0, 'orien_z': 0, 'orien_w': 0}
+        position_dic['p_x'] = topic.pose.position.x
+        position_dic['p_y'] = topic.pose.position.y
+        position_dic['orien_z'] = topic.pose.orientation.z
+        position_dic['orien_w'] = topic.pose.orientation.w
+
+        # 保存最新的充电桩位置到json文件
+        self.json_data = position_dic
+        self.save_charger_position()
+        print_and_fixRetract("New charging pile position saved.")
+        
+        # 位置更新后立即发布一次新的标记，然后继续定时发布
+        self.update_charger_visualization()
+        print_and_fixRetract(f"{GREEN}Charger position updated and will be published continuously{RESET}")
+
+    def update_charger_visualization(self):
+        '''更新充电桩可视化标记'''
+        if hasattr(self, 'json_data'):
+            self.Pub_Charger_marker(
+                self.json_data['p_x'],
+                self.json_data['p_y'], 
+                self.json_data['orien_z'],
+                self.json_data['orien_w']
+            )
+
+    def execute_navigation(self):
+        """外部调用的导航接口"""
+        self.request_navigation()
+        return True  # 返回True表示请求已提交
+
+    def execute_navigation_internal(self):
+        """内部执行导航任务"""
+        print_and_fixRetract(f"{BLUE}Starting navigation...{RESET}")
+        # 从JSON文件读取充电桩位置
+        try:
+            with open(json_file, 'r', encoding='utf-8') as f:
+                charger_data = json.load(f)
+            px = charger_data['p_x']
+            py = charger_data['p_y']
+            # 充电桩姿态四元数转欧拉角
+            orien_z = charger_data['orien_z']
+            orien_w = charger_data['orien_w']
+            yaw = 2 * math.atan2(orien_z, orien_w)  # 只考虑z/w分量
+        except Exception as e:
+            print_and_fixRetract(f"{RED}Failed to read charger position file: {e}{RESET}")
+            self.navigation_active = False
+            return False
+
+        # 计算目标点位置
+        x_offset = self.forward_distance * math.cos(yaw)
+        y_offset = self.forward_distance * math.sin(yaw)
+        goal_x = px + x_offset
+        goal_y = py + y_offset
+
+        # 计算目标点姿态（z轴顺时针yaw_offset_deg）
+        goal_yaw = yaw - math.radians(self.yaw_offset_deg)
+        goal_qz = math.sin(goal_yaw / 2)
+        goal_qw = math.cos(goal_yaw / 2)
+
+        print_and_fixRetract(f"Nav goal: x={goal_x:.3f}, y={goal_y:.3f}, yaw={math.degrees(goal_yaw):.1f}°")
+        goal_pose = self.create_pose(goal_x, goal_y, goal_qz, goal_qw)
+
+        # 执行导航
+        print_and_fixRetract(f"{BLUE}Executing navigation...{RESET}")
+        result1 = self.nav_through_pose([goal_pose], verbose=False)
+        print_and_fixRetract(f"Navigation result: {result1}")
+        self.navigation_active = False
+        return result1
+
+    def create_pose(self, x, y, z, w):
+        """创建单个目标点的位姿信息"""
+        pose = PoseStamped()
+        pose.header.frame_id = 'map'
+        pose.header.stamp = self.get_clock().now().to_msg()
+        pose.pose.position.x = x
+        pose.pose.position.y = y
+        pose.pose.orientation.z = z
+        pose.pose.orientation.w = w
+        return pose
+
+    def nav_through_pose(self, goal_poses, verbose: bool = False) -> bool:
+        """执行多点导航任务"""
+        # 开始执行多点导航任务
+        self.navigator.goThroughPoses(goal_poses)
+
+        # 等待导航任务完成，监控导航状态
+        while not self.navigator.isTaskComplete():
+            feedback = self.navigator.getFeedback()
+            if feedback and verbose:
+                remaining = Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9
+                print_and_fixRetract(f"预计到达时间: {remaining:.0f} 秒")
+
+        # 根据导航结果返回相应状态
+        result = self.navigator.getResult()
+        if result == TaskResult.SUCCEEDED:
+            print_and_fixRetract(f'{GREEN}Navigation successful!{RESET}')
+            return True
+        elif result == TaskResult.CANCELED:
+            print_and_fixRetract(f'{YELLOW}Navigation canceled!{RESET}')
+        elif result == TaskResult.FAILED:
+            print_and_fixRetract(f'{RED}Navigation failed!{RESET}')
+        else:
+            print_and_fixRetract(f'{RED}Invalid navigation result!{RESET}')
+        return False
+
+    def start_serial_control_async(self):
+        """异步启动串口控制功能"""
+        def serial_control_thread():
+            self.start_serial_control()
+        
+        # 在新线程中启动串口控制，避免阻塞ROS2主线程
+        serial_thread = threading.Thread(target=serial_control_thread, daemon=True)
+        serial_thread.start()
+
+    def start_serial_control(self):
+        """启动串口控制功能"""
+        print_and_fixRetract(f"{BLUE}Starting serial control...{RESET}")
+        try:
+            self.parser = SerialCANParser('/dev/agvpro_ec130', 9600, 1)
+            self.parser.open_serial()  # 打开usb串口
+            
+            # 发送AT 命令从透传模式进入AT指令模式
+            self.parser.send_at_commands(["AT+CG", "AT+AT"])
+            
+            self.serial_control_active = True
+            print_and_fixRetract(f'{GREEN}Serial control started successfully{RESET}')
+            
+            while self.serial_control_active:
+                # 开始读取数据
+                x_speed, z_speed, which_mode, infrared_bits = self.parser.read_serial_data()
+                
+                if infrared_bits[7] == 0:  # 无障碍物
+                    if which_mode == 0x01:  # 正常模式
+                        # 直接发布ROS2 Twist消息
+                        twist_msg = Twist()
+                        twist_msg.linear.x = float(x_speed)
+                        twist_msg.linear.y = 0.0
+                        twist_msg.angular.z = float(z_speed)
+                        self.cmd_vel_pub.publish(twist_msg)
+                        print_and_fixRetract(f'Normal mode - Speed: x={x_speed}, z={z_speed}')
+                        
+                    elif which_mode == 0xBB:  # 测压区
+                        # 停止运动
+                        stop_msg = Twist()
+                        self.cmd_vel_pub.publish(stop_msg)
+                        print_and_fixRetract(f'{YELLOW}Pressure zone - Stop movement{RESET}')
+                        
+                    elif which_mode == 0xAA:  # 充电区
+                        # 停止运动
+                        stop_msg = Twist()
+                        self.cmd_vel_pub.publish(stop_msg)
+                        print_and_fixRetract(f'{GREEN}Charging zone - Stop movement{RESET}')
+                        break  # 充电完成后退出
+                        
+                    elif which_mode == 0xCF:  # 急停模式
+                        emergency_stop_msg = Twist()  # 所有速度都为0
+                        self.cmd_vel_pub.publish(emergency_stop_msg)
+                        print_and_fixRetract(f'{RED}Emergency stop mode - Immediate stop{RESET}')
+                        break
+                        
+                else:  # 检测到障碍物
+                    obstacle_stop_msg = Twist()  # 所有速度都为0
+                    self.cmd_vel_pub.publish(obstacle_stop_msg)
+                    print_and_fixRetract(f'{RED}Obstacle detected - Stop movement{RESET}')
+                    break
+                    
+        except KeyboardInterrupt:
+            print_and_fixRetract("Serial control interrupted by user")
+            # 发布停止消息
+            emergency_stop = Twist()
+            self.cmd_vel_pub.publish(emergency_stop)
+        except Exception as e:
+            print_and_fixRetract(f"{RED}Serial control error: {e}{RESET}")
+            # 发布停止消息
+            emergency_stop = Twist()
+            self.cmd_vel_pub.publish(emergency_stop)
+        finally:
+            if self.parser:
+                self.parser.close_serial()
+            print_and_fixRetract("Serial control stopped")
+
+    def stop_serial_control(self):
+        """停止串口控制功能"""
+        self.serial_control_active = False
+
+    def get_charger_info(self):
+        '''获取充电桩位置信息'''
+        if hasattr(self, 'json_data'):
+            return {
+                'position': {
+                    'x': self.json_data['p_x'],
+                    'y': self.json_data['p_y']
+                },
+                'orientation': {
+                    'z': self.json_data['orien_z'],
+                    'w': self.json_data['orien_w']
+                }
+            }
+        return None
+
+
+def main(args=None):
+    '''主函数'''
+    rclpy.init(args=args)
+    
+    combined_recharger = None
+    try:
+        combined_recharger = CombinedAutoRecharger()
+        
+        print_and_fixRetract("Combined auto recharger node is running...")
+        print_and_fixRetract("Node functions:")
+        print_and_fixRetract("- Listening for charger position updates on /charger_position_update")
+        print_and_fixRetract("- Publishing visualization markers on /goal_marker every 2 seconds")
+        print_and_fixRetract("- Press 'q' to start navigation to charger position")
+        print_and_fixRetract("- Navigation success will trigger serial control")
+        
+        # 启动ROS2事件循环线程
+        def ros2_spin():
+            try:
+                rclpy.spin(combined_recharger)
+            except Exception as spin_error:
+                print_and_fixRetract(f"ROS2 spin error: {spin_error}")
+
+        ros2_thread = threading.Thread(target=ros2_spin, daemon=True)
+        ros2_thread.start()
+        
+        # 键盘监听循环
+        print_and_fixRetract(f"{GREEN}Press 'q' to start navigation, Ctrl+C to exit{RESET}")
+        print_and_fixRetract("Waiting for keyboard input...")
+        
+        while True:
+            try:
+                key = get_key(settings)
+                if key:
+                    print_and_fixRetract(f"Key pressed: {repr(key)}")  # 调试信息
+                    if key.lower() == 'q':
+                        print_and_fixRetract(f"{BLUE}Navigation command received!{RESET}")
+                        
+                        # 请求导航任务（异步执行）
+                        combined_recharger.execute_navigation()
+                    
+                    elif key == '\x03':  # Ctrl+C
+                        break
+                
+                time.sleep(0.1)  # 避免过度占用CPU
+            except KeyboardInterrupt:
+                break
+            except Exception as e:
+                print_and_fixRetract(f"Keyboard input error: {e}")
+                time.sleep(0.5)
+            
+    except KeyboardInterrupt:
+        print_and_fixRetract("\nShutting down combined auto recharger node...")
+    finally:
+        if combined_recharger:
+            combined_recharger.stop_serial_control()
+            combined_recharger.destroy_node()
+        rclpy.shutdown()
+        print_and_fixRetract("Program exited safely")
+
+if __name__ == '__main__':
+    main()
+

agv_pro_autocharge/agv_pro_autocharge/serial_can_parser.py

@@ -0,0 +1,173 @@
+#!/usr/bin/env python3                                                                                                           
+#coding=UTF-8
+import serial
+import time
+
+class SerialCANParser:
+    def __init__(self, serial_port='/dev/ttyCH341USB0', baudrate=9600, timeout=1):
+        self.serial_port = serial_port  # 串口名称
+        self.baudrate = baudrate  # 波特率
+        self.timeout = timeout  # 超时设置
+        self.ser = None  # 串口对象
+        self.buffer = bytearray()  # 存储当前读取的字节
+        self.max_retries = 3  # 最大重试次数
+        # 存储实时数据
+        self.x_speed = 0.0
+        self.z_speed = 0.0
+        self.infrared_bits = []
+        
+    def open_serial(self):
+        """打开串口"""
+        try:
+            self.ser = serial.Serial(self.serial_port, self.baudrate, timeout=self.timeout)
+            print(f"串口 {self.serial_port} 已打开，波特率：{self.baudrate}")
+        except Exception as e:
+            print(f"打开串口失败: {e}")
+            
+    def close_serial(self):
+        """关闭串口"""
+        if self.ser and self.ser.is_open:
+            self.ser.close()
+            print("串口已关闭。")
+        else:
+            print("串口未打开或已关闭。")
+            
+    def can_id_check(self, date):
+        high_byte, low_byte = date[0:2]
+        # 高字节左移 3 位
+        can_id = (high_byte << 3)
+        # 低字节右移 5 位
+        can_id |= (low_byte >> 5)
+        return can_id
+        
+    def parse_can_data(self, data):
+        """解析8字节CAN数据帧"""
+        if len(data) != 8:
+            print("数据帧长度不正确")
+            return None
+            
+        # 解析 X、Y 和 Z 速度
+        x_speed_raw = ((data[0] << 8) | data[1])  # X速度的原始数据
+        z_speed_raw = ((data[4] << 8) | data[5])  # Z速度的原始数据
+        
+        # 将原始数据转换为浮动数值，并考虑正负
+        if x_speed_raw & 0x8000:  # 如果最高位为1，表示负数
+            x_speed_raw = -((65536 - x_speed_raw) & 0xFFFF)  # 补码转换为负数
+        if z_speed_raw & 0x8000:  # 如果最高位为1，表示负数
+            z_speed_raw = -((65536 - z_speed_raw) & 0xFFFF)  # 补码转换为负数
+            
+        # 转换单位为 m/s 和 rad/s
+        self.x_speed = x_speed_raw / 1000.0  # X速度单位为 m/s
+        self.y_speed = 0  # Y速度为0
+        self.z_speed = z_speed_raw / 1000.0  # Z速度单位为 rad/s
+        self.which_mode = data[2]
+        self.infrared = data[6]  # 红外数据
+        self.raw_current = data[7]  # 电流数据
+        
+        if self.raw_current > 32767:  # 无符号数大于 32767 表示负值（因为最大值是 65535）
+            # 转换为负数
+            self.actual_current = -(65536 - self.raw_current) * 30.0
+        else:
+            # 正数直接转换
+            self.actual_current = self.raw_current * 30.0
+            
+        # 处理红外数据
+        self.infrared_bits = [(self.infrared >> (7 - i)) & 0x01 for i in range(8)]
+        
+        # 打印或处理数据
+        print(f"X Speed: {self.x_speed:.3f}, Y Speed: {self.y_speed}, Z Speed: {self.z_speed:.3f}, "
+              f"Actual Current: {self.actual_current:.3f} mA, Infrared: {self.infrared}")
+              
+        # 打印或处理红外位信息
+        print(f"L_A: {self.infrared_bits[2]}, L_B: {self.infrared_bits[3]}, R_B: {self.infrared_bits[4]}, "
+              f"R_A: {self.infrared_bits[5]}, infrared_flag : {self.infrared_bits[6]}, "
+              f"Charging flag: {self.infrared_bits[7]}")
+              
+    def read_serial_data(self):
+        """读取串口数据并解析"""
+        while True:  # 修改为简单的无限循环，由上层控制退出
+            if self.ser.in_waiting > 0:
+                byte = self.ser.read(1)   # 读取一个字节
+                if len(self.buffer) < 2:
+                    self.buffer.extend(byte)  
+                    if len(self.buffer) == 2:
+                        # 如果帧头为 0x41 0x54，表示为AT帧头，则开始接收数据
+                        if self.buffer[0] != 0x41 or self.buffer[1] != 0x54:
+                            # 如果不是有效的帧头，则清空缓冲区并跳到下次循环
+                            self.buffer.clear()
+                            continue  # 继续等待下一个字节                 
+                else:
+                    self.buffer.extend(byte)
+                    # print("缓冲区内容:", ' '.join(f'{b:02x}' for b in self.buffer)) # debug
+                    # 如果缓冲区字节长度大于等于 17 字节（数据帧长度）
+                    if len(self.buffer) >= 17:
+                        # print("Received Frame (Hex):", ' '.join(f'{byte:02x}' for byte in self.buffer)) # debug
+                        # 解析帧头、CAN帧ID、格式、类型和数据
+                        # at_frame_header = self.buffer[0:2]  # AT帧头
+                        can_frame_id = self.can_id_check(self.buffer[2:4])  # CAN标准帧ID
+                        # can_frame_format = self.buffer[4]  # CAN帧格式（0,标准帧;1,扩展帧）
+                        # can_frame_type = self.buffer[5]  # CAN帧类型（0,数据帧;1,远程帧）
+                        data_length = self.buffer[6]  # 数据长度
+                        data = self.buffer[7:15]  # 数据帧
+                        # print(f"帧ID: 0x{can_frame_id:X}")    # debug
+                        if can_frame_id == 0x182 and data_length == 0x08: # 根据can帧id进行判断
+                            # 如果帧ID为0x182,校验通过,进行数据赋值
+                            self.parse_can_data(data)
+                            # 清空缓冲区，准备下一帧数据
+                            self.buffer.clear()
+                            return self.x_speed, self.z_speed, self.which_mode, self.infrared_bits   # 返回解析后的数据
+                        else:
+                            # 清空缓冲区，准备下一帧数据
+                            self.buffer.clear()
+                            
+    def read_serial_response(self):
+        """读取串口响应数据，直到接收到 '\r\n' 或超时"""
+        response = bytearray()  # 使用 bytearray 来存储原始字节流
+        while True:
+            if self.ser.in_waiting > 0:
+                byte = self.ser.read(1)
+                response += byte
+            # 检查是否已接收到完整响应
+            if b'\r\n' in response:
+                break
+            # 超时机制，防止死循环
+            if len(response) > 100:
+                break
+        return bytes(response)  # 返回原始字节流（bytes）
+        
+    def send_at_commands(self, commands):
+        """发送 AT 命令并等待响应"""
+        for command in commands:
+            retries = 0
+            while retries < self.max_retries:
+                self.ser.write(command.encode() + b'\r\n')
+                print(f"发送命令: {command}")
+                # 等待响应并读取数据
+                response = self.read_serial_response()
+                # 检查响应是否包含 "OK"
+                if b"OK" in response:
+                    print(f"收到响应: {response}")
+                    break  # 如果收到 OK，退出重试循环
+                else:
+                    retries += 1
+                    print(f"未收到预期的响应，收到: {response}")
+            if retries == self.max_retries:
+                print(f"重试 {self.max_retries} 次后仍未收到有效响应，请检查设备。")
+                break
+                
+    def start(self):
+        """开始读取和处理数据"""
+        self.open_serial()
+        try:
+            # 发送AT 命令从透传模式进入AT指令模式
+            self.send_at_commands(["AT+CG", "AT+AT"])
+            # 开始读取数据
+            self.read_serial_data()
+        except KeyboardInterrupt:
+            print("手动中止程序。")
+        finally:
+            self.close_serial()
+
+if __name__ == '__main__':
+    parser = SerialCANParser('/dev/ttyCH341USB0', 9600, 1)
+    parser.start()

agv_pro_autocharge/config/charger_position.json

@@ -0,0 +1,6 @@
+{
+  "p_x": -0.04373347759246826,
+  "p_y": -4.024151802062988,
+  "orien_z": 0.09378381732290733,
+  "orien_w": 0.9955925851513477
+}

agv_pro_autocharge/config/nav_goal_params.yaml

@@ -0,0 +1,3 @@
+# 导航参数配置
+forward_distance: 1  # 距离充电桩前方1米
+yaw_offset_deg: 10.0   # 顺时针旋转10度

agv_pro_autocharge/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>agv_pro_autocharge</name>
+  <version>1.0.0</version>
+  <description>AGV automatic charging system for ROS2 Humble</description>
+  <maintainer email="your-email@example.com">Your Name</maintainer>
+  <license>MIT</license>
+
+  <!-- Build dependencies -->
+  <buildtool_depend>ament_python</buildtool_depend>
+  
+  <!-- Runtime dependencies -->
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>nav2_simple_commander</depend>
+  
+  <!-- Test dependencies -->
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

agv_pro_autocharge/resource/agv_pro_autocharge

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

agv_pro_autocharge/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/agv_pro_autocharge
+[install]
+install_scripts=$base/lib/agv_pro_autocharge

agv_pro_autocharge/setup.py

@@ -0,0 +1,30 @@
+from setuptools import setup, find_packages
+import os
+from glob import glob
+
+package_name = 'agv_pro_autocharge'
+
+setup(
+    name=package_name,
+    version='1.0.0',
+    packages=find_packages(),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        # Include config files
+        (os.path.join('share', package_name, 'config'), glob('config/*')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Your Name',
+    maintainer_email='your-email@example.com',
+    description='AGV automatic charging system for ROS2 Humble',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'combined_auto_recharger = agv_pro_autocharge.combined_auto_recharger:main',
+        ],
+    },
+)

agv_pro_base/include/agv_pro_base/agv_pro_driver.h

@@ -2,7 +2,8 @@
 #define AGV_PRO_DRIVER_H
 
 #include <algorithm> 
-#include "serial_driver/serial_driver.hpp"
+#include <iostream>
+#include <boost/asio.hpp>
 
 #include "rclcpp/rclcpp.hpp"
 
@@ -13,7 +14,9 @@
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
-#define RECEIVE_DATA_SIZE 14         //The length of the data sent by the esp32
+#define SEND_DATA_SIZE 14                               // Total bytes in a command frame to ESP32(version>=V1.0.8)
+#define RECEIVE_FRAME_SIZE 31                           // Total bytes in a frame from ESP32(version>=V1.0.8)
+#define RECEIVE_PAYLOAD_SIZE (RECEIVE_FRAME_SIZE - 3)   // Payload length (excluding header)
 
 extern std::array<double, 36> odom_pose_covariance;
 extern std::array<double, 36> odom_twist_covariance;
@@ -21,20 +24,136 @@ extern std::array<double, 36> odom_twist_covariance;
 class AGV_PRO : public rclcpp::Node
 {
 public:
+  /**
+   * @brief Constructor
+   */
   AGV_PRO(std::string node_name);
+
+  /**
+   * @brief Destructor
+   */
   ~AGV_PRO();
+
 private:
+  /**
+   * @brief Main control loop for the AGV.
+   */
   void Control();
-  void print_hex(const std::string& label, const std::vector<uint8_t>& data, std::optional<size_t> override_size = std::nullopt);
+
+  /**
+   * @brief Print a vector of bytes in hexadecimal format to the ROS logger.
+   *
+   * @param[in] label A label to prepend to the printed data.
+   * @param[in] data The byte vector to print.
+   * @param[in] override_size Optional size to display instead of the full data length.
+   */
+  void print_hex(const std::string& label,
+    const std::vector<uint8_t>& data,
+    std::optional<size_t> override_size = std::nullopt);
+
+  /**
+   * @brief Send a serial frame to the AGV and optionally print it in hex.
+   *
+   * @param[in] frame The byte vector representing the serial frame to send.
+   * @param[in] debug If true, prints the transmitted frame using print_hex().
+   */
   void send_serial_frame(const std::vector<uint8_t>& frame, bool debug);
-  void is_power_on();
-  void set_auto_report();
+
+  /**
+   * @brief Query and print the current power status of the AGV.
+   * @return true if AGV is successfully power on; false otherwise.
+   */
+  bool is_power_on();
+
+  /**
+   * @brief Enable or disable AGV auto-reporting
+   * @param[in] enable 0 = disable, 1 = enable
+   */
+  void set_auto_report(bool enable);
+
+  /**
+   * @brief Clear the serial port input and output buffers
+   * @param[in] fd File descriptor of the serial port
+   */
+  void clearSerialBuffer(int fd);
+
+  /**
+   * @brief Disable the DTR (Data Terminal Ready) and RTS (Request To Send) lines of the serial port
+   * @param[in] fd File descriptor of the serial port
+   */
+  void disableDTR_RTS(int fd);
+
+  /**
+   * @brief Read sensor and motor data from the AGV via serial port.
+   * @return true if data is successfully read and verified; false otherwise.
+   */
   bool readData();
+
+  /**
+   * @brief Odometry publisher
+   * @param[in] dt Time difference (in seconds) since the last odometry update.
+   */
   void publisherOdom(double dt);
+
+  /**
+   * @brief Voltage publisher
+   */
   void publisherVoltage();
+
+  /**
+   * @brief ImuSensor publisher
+   */
+  void publisherImuSensor();
+
+  /**
+   * @brief Callback for velocity command updates
+   * @param[in] msg The Twist message containing desired linear and angular velocities
+   */
   void cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
+  
+  /**
+   * @brief Build a standard AGV serial frame with header, payload, and CRC.
+   *
+   * The frame has a fixed size of RECEIVE_DATA_SIZE, starts with 0xFE 0xFE 0x0B,
+   * includes a command ID and up to 8 payload bytes, and ends with a 16-bit CRC.
+   *
+   * @param[in] cmd_id The command ID for the serial frame.
+   * @param[in] payload The payload bytes to include (up to 8 bytes).
+   * @return A vector containing the complete serial frame ready to transmit.
+   */
   std::vector<uint8_t> build_serial_frame(uint8_t cmd_id, const std::vector<uint8_t>& payload);
-  std::vector<uint8_t> read_serial_response(const std::vector<uint8_t>& expected_header, size_t payload_size, double timeout_sec);
+
+  /**
+   * @brief Read a serial response from the AGV device, waiting for a specific header.
+   *
+   * This function reads bytes from the serial port until the expected header
+   * sequence is detected or the timeout expires. After detecting the header,
+   * it reads the remaining payload bytes along with a 2-byte CRC.
+   *
+   * @param[in] expected_header The byte sequence to identify the start of a valid frame.
+   * @param[in] payload_size The expected number of payload bytes following the header.
+   * @param[in] timeout_sec Maximum time (in seconds) to wait for the header.
+   * @return A vector containing the complete frame (header + payload + CRC).
+   *         Returns an empty vector if a timeout occurs or the full payload is not received.
+   */
+  std::vector<uint8_t> read_serial_response(
+    const std::vector<uint8_t>& expected_header,
+    size_t payload_size,
+    double timeout_sec);
+
+  /**
+   * @brief Compute the CRC-16-IBM checksum for a byte array.
+   *
+   * This function calculates the CRC using the standard IBM polynomial 0xA001.
+   *
+   * @param[in] data Pointer to the byte array.
+   * @param[in] length Number of bytes to include in the CRC calculation.
+   * @return The computed 16-bit CRC value.
+   */
+  uint16_t crc16_ibm(const uint8_t* data, size_t length);
+
+  boost::asio::io_service io_;
+  std::unique_ptr<boost::asio::serial_port> serial_port_;
 
   std::string frame_id_of_odometry_;
   std::string child_frame_id_of_odometry_;
@@ -54,6 +173,18 @@ private:
   double linearY = 0.0;
   double angularZ = 0.0;
 
+  double ax= 0.0;
+  double ay= 0.0;
+  double az= 0.0;
+
+  double wx= 0.0;
+  double wy= 0.0;
+  double wz= 0.0;
+
+  double roll = 0.0;
+  double pitch = 0.0;
+  double yaw = 0.0;
+
   int is_poweron_status = 0;
   int poweron_status = 0;
 
@@ -63,6 +194,22 @@ private:
   
   float battery_voltage = 0.0f;
 
+  std::array<double, 36> odom_pose_covariance = {
+    {1e-9, 0, 0, 0, 0, 0,
+    0, 1e-3, 1e-9, 0, 0, 0,
+    0, 0, 1e6, 0, 0, 0,
+    0, 0, 0, 1e6, 0, 0,
+    0, 0, 0, 0, 1e6, 0,
+    0, 0, 0, 0, 0, 1e-9} };
+
+  std::array<double, 36> odom_twist_covariance = {
+    {1e-9, 0, 0, 0, 0, 0,
+    0, 1e-3, 1e-9, 0, 0, 0,
+    0, 0, 1e6, 0, 0, 0,
+    0, 0, 0, 1e6, 0, 0,
+    0, 0, 0, 0, 1e6, 0,
+    0, 0, 0, 0, 0, 1e-9} };
+
   rclcpp::Time currentTime, lastTime;
   rclcpp::TimerBase::SharedPtr control_timer_;
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odom;
@@ -70,10 +217,8 @@ private:
   rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_voltage;
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub;
 
+  sensor_msgs::msg::Imu imu_data;
   std::unique_ptr<tf2_ros::TransformBroadcaster> odomBroadcaster;
-  std::shared_ptr<drivers::serial_driver::SerialDriver> serial_driver_;
-  std::shared_ptr<drivers::common::IoContext> io_context_;
-
 };
 
 #endif

agv_pro_base/src/agv_pro_ros.cpp

@@ -1,22 +1,6 @@
 #include "agv_pro_base/agv_pro_driver.h"
 
-std::array<double, 36> odom_pose_covariance = {
-  {1e-9, 0, 0, 0, 0, 0,
-  0, 1e-3, 1e-9, 0, 0, 0,
-  0, 0, 1e6, 0, 0, 0,
-  0, 0, 0, 1e6, 0, 0,
-  0, 0, 0, 0, 1e6, 0,
-  0, 0, 0, 0, 0, 1e-9} };
-
-std::array<double, 36> odom_twist_covariance = {
-  {1e-9, 0, 0, 0, 0, 0,
-  0, 1e-3, 1e-9, 0, 0, 0,
-  0, 0, 1e6, 0, 0, 0,
-  0, 0, 0, 1e6, 0, 0,
-  0, 0, 0, 0, 1e6, 0,
-  0, 0, 0, 0, 0, 1e-9} };
-
-uint16_t crc16_ibm(const uint8_t* data, size_t length) {
+uint16_t AGV_PRO::crc16_ibm(const uint8_t* data, size_t length) {
   uint16_t crc = 0xFFFF;
   for (size_t i = 0; i < length; ++i) {
     crc ^= static_cast<uint16_t>(data[i]);
@@ -32,7 +16,7 @@ uint16_t crc16_ibm(const uint8_t* data, size_t length) {
 
 std::vector<uint8_t> AGV_PRO::build_serial_frame(uint8_t cmd_id, const std::vector<uint8_t>& payload)
 {
-  std::vector<uint8_t> frame(RECEIVE_DATA_SIZE, 0x00);
+  std::vector<uint8_t> frame(SEND_DATA_SIZE, 0x00);
   frame[0] = 0xFE;
   frame[1] = 0xFE;
   frame[2] = 0x0B;
@@ -62,8 +46,7 @@ void AGV_PRO::print_hex(const std::string& label, const std::vector<uint8_t>& da
 void AGV_PRO::send_serial_frame(const std::vector<uint8_t>& frame, bool debug)
 {
   try {
-    auto port = serial_driver_->port();
-    size_t bytes_transmit_size = port->send(frame);
+    size_t bytes_transmit_size = boost::asio::write(*serial_port_, boost::asio::buffer(frame));
     if (debug) {
       print_hex("Sent", frame, bytes_transmit_size);
     }
@@ -72,9 +55,11 @@ void AGV_PRO::send_serial_frame(const std::vector<uint8_t>& frame, bool debug)
   }
 }
 
-std::vector<uint8_t> AGV_PRO::read_serial_response(const std::vector<uint8_t>& expected_header, size_t payload_size, double timeout_sec)
+std::vector<uint8_t> AGV_PRO::read_serial_response(
+  const std::vector<uint8_t>& expected_header,
+  size_t payload_size,
+  double timeout_sec)
 {
-  auto port = serial_driver_->port();
   std::vector<uint8_t> sliding_buf;
   uint8_t byte = 0;
 
@@ -82,21 +67,24 @@ std::vector<uint8_t> AGV_PRO::read_serial_response(const std::vector<uint8_t>& e
   rclcpp::Duration timeout = rclcpp::Duration::from_seconds(timeout_sec);
 
   while ((this->now() - start_time) < timeout) {
-    std::vector<uint8_t> temp_buf(1);
-    if (port->receive(temp_buf) == 1) {
-      byte = temp_buf[0];
+    boost::asio::mutable_buffers_1 buf(&byte, 1);
+    boost::system::error_code ec;
+    size_t n = serial_port_->read_some(buf, ec);
+    if (ec) {
+        RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+        return {};
+    }
+    if (n == 1) {
       sliding_buf.push_back(byte);
-
       if (sliding_buf.size() > expected_header.size()) {
-        sliding_buf.erase(sliding_buf.begin());
+          sliding_buf.erase(sliding_buf.begin());
       }
-
       if (sliding_buf == expected_header) {
-        break;
+          break;
       }
     }
   }
-
+  
   if (sliding_buf != expected_header) {
     RCLCPP_WARN(this->get_logger(), "Timeout waiting for header");
     return {};
@@ -104,7 +92,20 @@ std::vector<uint8_t> AGV_PRO::read_serial_response(const std::vector<uint8_t>& e
 
   size_t remain_len = payload_size + 2;
   std::vector<uint8_t> remain_buf(remain_len);
-  if (port->receive(remain_buf) != remain_len) {
+  size_t total_read = 0;
+
+  while (total_read < remain_len && (this->now() - start_time) < timeout) {
+    boost::asio::mutable_buffers_1 buf(&remain_buf[total_read], remain_len - total_read);
+    boost::system::error_code ec;
+    size_t n = serial_port_->read_some(buf, ec);
+    if (ec) {
+        RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+        return {};
+    }
+    total_read += n;
+  }
+
+  if (total_read != remain_len) {
     RCLCPP_WARN(this->get_logger(), "Timeout or incomplete data payload");
     return {};
   }
@@ -115,22 +116,22 @@ std::vector<uint8_t> AGV_PRO::read_serial_response(const std::vector<uint8_t>& e
   return full_buf;
 }
 
-void AGV_PRO::is_power_on(){
+bool AGV_PRO::is_power_on(){
   auto power_query_frame = build_serial_frame(0x12, {});
   send_serial_frame(power_query_frame,true);
 
   const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, 0x12};
-  auto power_query_response = read_serial_response(expected_header, 8, 1.0);
+  auto power_query_response = read_serial_response(expected_header, 8, 12.0);
 
   print_hex("recv_buf", power_query_response);
   
-  if (power_query_response.size() != 14) return;
+  if (power_query_response.size() != 14) return false;
 
   uint16_t received_crc = (power_query_response[12] << 8) | power_query_response[13];
   uint16_t computed_crc = crc16_ibm(power_query_response.data(), 12);
   if (received_crc != computed_crc) {
     RCLCPP_WARN(this->get_logger(), "CRC mismatch: received=0x%04X, expected=0x%04X", received_crc, computed_crc);
-    return;
+    return false;
   }
 
   int is_poweron_status = static_cast<int8_t>(power_query_response[4]);
@@ -146,13 +147,13 @@ void AGV_PRO::is_power_on(){
     auto status_query_response = read_serial_response(expected_header, 8, 5.0);// Read the serial response with the specified expected header, payload size, and timeout of 5 seconds
     print_hex("recv_buf", status_query_response);
   
-    if (status_query_response.size() != 14) return;
+    if (status_query_response.size() != 14) return false;
 
     uint16_t received_crc = (status_query_response[12] << 8) | status_query_response[13];
     uint16_t computed_crc = crc16_ibm(status_query_response.data(), 12);
     if (received_crc != computed_crc) {
       RCLCPP_WARN(this->get_logger(), "CRC mismatch: received=0x%04X, expected=0x%04X", received_crc, computed_crc);
-      return;
+      return false;
     }
 
     int poweron_status = static_cast<int8_t>(status_query_response[4]);
@@ -162,37 +163,61 @@ void AGV_PRO::is_power_on(){
       case 1:
         status_msg = "Motor is operating normally.";
         RCLCPP_INFO(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
-        break;
+        return true;
       case 2:
         status_msg = "Emergency stop button is not released.";
         RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
-        break;
+        return false;
       case 3:
         status_msg = "Battery voltage is below 19.5V.";
         RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
-        break;
+        return false;
       case 4:
         status_msg = "CAN initialization error.";
         RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
-        break;
+        return false;
       case 5:
         status_msg = "Motor initialization error.";
         RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
-        break;
+        return false;
       default:
         RCLCPP_WARN(this->get_logger(), "power_status: %d, Unknown power status code", poweron_status);
-        break;
+        return false;
     }
   }
-  else
+  else{
     RCLCPP_INFO(this->get_logger(), "Motor is operating normally.");
+    return true;
+  }
 }
 
-void AGV_PRO::set_auto_report(){
-  auto frame = build_serial_frame(0x23, {0x01});
+void AGV_PRO::set_auto_report(bool enable){
+  auto frame = build_serial_frame(0x23, {static_cast<uint8_t>(enable)});
   send_serial_frame(frame,true);
 }
 
+void AGV_PRO::clearSerialBuffer(int fd) {
+  if (tcflush(fd, TCIOFLUSH) < 0) {
+    RCLCPP_WARN(this->get_logger(), "Failed to flush serial buffer: %s", std::strerror(errno));
+  } else {
+    RCLCPP_INFO(this->get_logger(), "Serial buffer flushed.");
+  }
+}
+
+void AGV_PRO::disableDTR_RTS(int fd) {
+  int status;
+  if (::ioctl(fd, TIOCMGET, &status) == 0) {
+    status &= ~(TIOCM_DTR | TIOCM_RTS);
+    if (::ioctl(fd, TIOCMSET, &status) != 0) {
+      RCLCPP_WARN(this->get_logger(), "Failed to clear DTR and RTS: %s", std::strerror(errno));
+    } else {
+      RCLCPP_INFO(this->get_logger(), "DTR and RTS lines disabled successfully.");
+    }
+  } else {
+    RCLCPP_WARN(this->get_logger(), "Failed to read modem status: %s", std::strerror(errno));
+  }
+}
+
 void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
 {
   linearX = std::clamp(msg->linear.x, -1.5, 1.5);
@@ -220,11 +245,9 @@ void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
 
   std::vector<uint8_t> data_vec(buf, buf + sizeof(buf));
 
-  auto port = serial_driver_->port();
-
   try
   {
-    port->send(data_vec);
+    boost::asio::write(*serial_port_,boost::asio::buffer(data_vec));
     // print_hex("Sent", data_vec);//debug
   }
   catch(const std::exception &ex)
@@ -235,34 +258,46 @@ void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
 
 bool AGV_PRO::readData()
 {
-  std::vector<uint8_t> buf_header(1);
   std::vector<uint8_t> buf_length(1);
-  std::vector<uint8_t> data_buf(RECEIVE_DATA_SIZE-3);
+  std::vector<uint8_t> data_buf(RECEIVE_PAYLOAD_SIZE);
 
-  auto port = serial_driver_->port();
+  uint8_t byte = 0;
+  boost::system::error_code ec;
   
   while (true)
   {
-    size_t ret = port->receive(buf_header);
-    if (ret != 1 || buf_header[0] != 0xfe) {
+    size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec);
+    if (ec) {
+      RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+      return false;
+    }
+    if (ret != 1 || byte != 0xfe) {
       continue;
     }
 
-    ret = port->receive(buf_header);
-    if (ret == 1 && buf_header[0] == 0xfe) {
+    ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec);
+    if (ec) {
+      RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+      return false;
+    }
+    if (ret == 1 && byte == 0xfe) {
       break; 
     }
   }
 
-  size_t ret = port->receive(buf_length);
+  size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(buf_length), ec);
+  if (ec) {
+      RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+      return false;
+  }
 
-  if (buf_length[0] != 0x0b) {
-    RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%u", buf_length[0]);
+  if (buf_length[0] != RECEIVE_PAYLOAD_SIZE) {
+    //RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%u", buf_length[0]);
     return false;
   }
 
-  ret = port->receive(data_buf);
-  if (ret != data_buf.size())
+  ret = boost::asio::read(*serial_port_, boost::asio::buffer(data_buf), ec);
+  if (ec || ret != data_buf.size())
   {
     RCLCPP_ERROR(this->get_logger(), "Failed to receive full payload");
     return false;
@@ -271,18 +306,18 @@ bool AGV_PRO::readData()
   std::vector<uint8_t> recv_buf;
   recv_buf.push_back(0xFE);
   recv_buf.push_back(0xFE);
-  recv_buf.push_back(0x0B);
+  recv_buf.push_back(0x1C);
   recv_buf.insert(recv_buf.end(), data_buf.begin(), data_buf.end());
   
-  // print_hex("recv_buf", recv_buf); //debug
+  //print_hex("recv_buf", recv_buf); //debug
 
   if (recv_buf[3] != 0x25) {
-    // RCLCPP_WARN(this->get_logger(), "Command error:0x%02X", recv_buf[2]);
+    //RCLCPP_WARN(this->get_logger(), "Command error:0x%02X", recv_buf[2]); //debug
     return false;
   }
 
-  uint16_t received_crc = recv_buf[13] | (recv_buf[12] << 8);
-  uint16_t computed_crc = crc16_ibm(recv_buf.data(), 12);
+  uint16_t received_crc = recv_buf[RECEIVE_FRAME_SIZE-1] | (recv_buf[RECEIVE_FRAME_SIZE-2] << 8);
+  uint16_t computed_crc = crc16_ibm(recv_buf.data(), RECEIVE_FRAME_SIZE-2);
 
   if (received_crc != computed_crc) {
     RCLCPP_WARN(this->get_logger(), "CRC error: received 0x%04X, calculated 0x%04X", received_crc, computed_crc);
@@ -298,6 +333,18 @@ bool AGV_PRO::readData()
   battery_voltage = static_cast<float>(recv_buf[9]) / 10.0f;
   enable_status = recv_buf[10];
 
+  imu_data.linear_acceleration.x = static_cast<double>((static_cast<int16_t>(recv_buf[11]) << 8) | recv_buf[12]) * 0.01;
+  imu_data.linear_acceleration.y = static_cast<double>((static_cast<int16_t>(recv_buf[13]) << 8) | recv_buf[14]) * 0.01;
+  imu_data.linear_acceleration.z = static_cast<double>((static_cast<int16_t>(recv_buf[15]) << 8) | recv_buf[16]) * 0.01;
+
+  imu_data.angular_velocity.x = static_cast<double>((static_cast<int16_t>(recv_buf[17]) << 8) | recv_buf[18]) * 0.01;
+  imu_data.angular_velocity.y = static_cast<double>((static_cast<int16_t>(recv_buf[19]) << 8) | recv_buf[20]) * 0.01;
+  imu_data.angular_velocity.z = static_cast<double>((static_cast<int16_t>(recv_buf[21]) << 8) | recv_buf[22]) * 0.01;
+
+  roll  = static_cast<double>((static_cast<int16_t>(recv_buf[23]) << 8) | recv_buf[24]) * 0.01;
+  pitch = static_cast<double>((static_cast<int16_t>(recv_buf[25]) << 8) | recv_buf[26]) * 0.01;
+  yaw   = static_cast<double>((static_cast<int16_t>(recv_buf[27]) << 8) | recv_buf[28]) * 0.01;
+
   return true;
 }
 
@@ -308,6 +355,40 @@ void AGV_PRO::publisherVoltage()
   pub_voltage->publish(voltage_msg);
 }
 
+void AGV_PRO::publisherImuSensor()
+{
+  sensor_msgs::msg::Imu ImuSensor;
+
+  ImuSensor.header.stamp = this->get_clock()->now();
+  ImuSensor.header.frame_id = "imu_link";
+
+  tf2::Quaternion qua;
+  qua.setRPY(0, 0, yaw * M_PI / 180.0);
+
+  ImuSensor.orientation.x = qua[0];
+  ImuSensor.orientation.y = qua[1];
+  ImuSensor.orientation.z = qua[2];
+  ImuSensor.orientation.w = qua[3];
+
+  ImuSensor.angular_velocity.x = imu_data.angular_velocity.x;
+  ImuSensor.angular_velocity.y = imu_data.angular_velocity.y;
+  ImuSensor.angular_velocity.z = imu_data.angular_velocity.z;
+
+  ImuSensor.linear_acceleration.x = imu_data.linear_acceleration.x;
+  ImuSensor.linear_acceleration.y = imu_data.linear_acceleration.y;
+  ImuSensor.linear_acceleration.z = imu_data.linear_acceleration.z;
+
+  ImuSensor.orientation_covariance[0] = 1e6;
+  ImuSensor.orientation_covariance[4] = 1e6;
+  ImuSensor.orientation_covariance[8] = 1e-6;
+
+  ImuSensor.angular_velocity_covariance[0] = 1e6;
+  ImuSensor.angular_velocity_covariance[4] = 1e6;
+  ImuSensor.angular_velocity_covariance[8] = 1e-6;
+
+  pub_imu->publish(ImuSensor);
+}
+
 void AGV_PRO::publisherOdom(double dt)
 {
   currentTime = this->get_clock()->now();
@@ -345,12 +426,12 @@ void AGV_PRO::publisherOdom(double dt)
   odom.pose.pose.position.y = y;
   odom.pose.pose.position.z = 0.0;
   odom.pose.pose.orientation = odom_quat;
-  odom.pose.covariance = odom_pose_covariance;
+  odom.pose.covariance = this->odom_pose_covariance;
 
   odom.twist.twist.linear.x = vx;
   odom.twist.twist.linear.y = vy;
   odom.twist.twist.angular.z = vtheta;
-  odom.twist.covariance = odom_twist_covariance;
+  odom.twist.covariance = this->odom_twist_covariance;
 
   pub_odom->publish(odom);
 }
@@ -369,6 +450,7 @@ void AGV_PRO::Control()
     publisherOdom(dt);
     // RCLCPP_INFO(this->get_logger(), "dt:%f", dt);
     publisherVoltage();
+    publisherImuSensor();
   }
 }
 
@@ -401,64 +483,54 @@ AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name)
 
   lastTime = this->get_clock()->now();
       
-  drivers::serial_driver::SerialPortConfig config(
-    1000000,
-    drivers::serial_driver::FlowControl::NONE,
-    drivers::serial_driver::Parity::NONE,
-    drivers::serial_driver::StopBits::ONE
-  );
-
   try{
-    io_context_ = std::make_shared<drivers::common::IoContext>(1);
-    serial_driver_ = std::make_shared<drivers::serial_driver::SerialDriver>(*io_context_);
-    serial_driver_->init_port(device_name_, config);
-    serial_driver_->port()->open();
-    
-    RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
-    RCLCPP_INFO(this->get_logger(), "Using device: %s", serial_driver_->port().get()->device_name().c_str());
-    RCLCPP_INFO(this->get_logger(), "Baud_rate: %d", config.get_baud_rate());
+    serial_port_ = std::make_unique<boost::asio::serial_port>(io_);
 
-    AGV_PRO::is_power_on();
-    AGV_PRO::set_auto_report();
+    serial_port_->open(device_name_);
+    serial_port_->set_option(boost::asio::serial_port_base::baud_rate(1000000));
+    serial_port_->set_option(boost::asio::serial_port_base::character_size(8));
+    serial_port_->set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none));
+    serial_port_->set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one));
+    serial_port_->set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none));
+
+    int fd = serial_port_->native_handle();
+    this->clearSerialBuffer(fd);
+    this->disableDTR_RTS(fd);
+
+    rclcpp::sleep_for(std::chrono::milliseconds(3000));//esp32 Restart time
+
+    RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
+    RCLCPP_INFO(this->get_logger(), "Using device: %s", device_name_.c_str());
+
+    boost::asio::serial_port_base::baud_rate baud_option;
+    serial_port_->get_option(baud_option);
+    unsigned int current_baud = baud_option.value();
+    RCLCPP_INFO(this->get_logger(), "Baud_rate: %u", current_baud);
   }
   catch (const std::exception &ex){
     RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what());
     return;
   }
-  
-  control_timer_ = this->create_wall_timer(
-  std::chrono::milliseconds(20),
-  std::bind(&AGV_PRO::Control, this)
-  );
-  RCLCPP_INFO(this->get_logger(), "Control timer started");
 
+  if (this->is_power_on()) {
+    this->set_auto_report(1);
+
+    control_timer_ = this->create_wall_timer(
+    std::chrono::milliseconds(20),
+    std::bind(&AGV_PRO::Control, this)
+    );
+    RCLCPP_INFO(this->get_logger(), "Control timer started");
+  }
+  else {
+    RCLCPP_WARN(this->get_logger(), "Control timer not started.");
+  }
 }
 
 AGV_PRO::~AGV_PRO()
-{ 
-  std::array<uint8_t, 14> buf = {
-    0xFE, 0xFE, 0x0b, 0x22, 
-    0x01, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00
-  };
-
-  uint16_t crc = crc16_ibm(buf.data(), 12);
-  buf[12] = (crc >> 8) & 0xff;
-  buf[13] = crc & 0xff;
-
-  std::vector<uint8_t> data_vec(buf.begin(), buf.end());
-
-  auto port = serial_driver_->port();
-
-  try
-  {
-    port->send(data_vec);
-  }
-  catch(const std::exception &ex)
-  {
-    RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port:%s",ex.what());
-  }
-
-  serial_driver_->port()->close();
-  RCLCPP_INFO(this->get_logger(),"Shutting down");
+{
+  if (serial_port_ && serial_port_->is_open()) {
+    this->set_auto_report(0);
+    serial_port_->cancel();
+    serial_port_->close();
+  } 
 }

agv_pro_description/urdf/agv_pro.urdf

@@ -212,4 +212,21 @@
     <parent link="${namespace}base_link" />
     <child link="${namespace}laser_link" />
   </joint>
+
+  <link name="${namespace}camera_link"/>
+
+  <joint name="${namespace}camera_joint" type="fixed">
+    <origin xyz="0.23191 0 0.14928" rpy="0 0 0" />
+    <parent link="${namespace}base_link" />
+    <child link="${namespace}camera_link" />
+  </joint>
+
+  <link name="${namespace}imu_link"/>
+
+  <joint name="${namespace}imu_joint" type="fixed">
+    <origin xyz="-0.17181 -0.0270532 0.14928" rpy="0 0 1.5707" />
+    <parent link="${namespace}base_link" />
+    <child link="${namespace}imu_link" />
+  </joint>
+
 </robot>

agv_pro_navigation2/CMakeLists.txt

@@ -24,7 +24,7 @@ if(BUILD_TESTING)
 endif()
 
 install(
-  DIRECTORY launch map param rviz
+  DIRECTORY launch map param rviz scripts
   DESTINATION share/${PROJECT_NAME}
 )
 

agv_pro_navigation2/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>agv_pro_navigation2</name>
-  <version>1.0.0</version>
+  <version>1.0.1</version>
   <description>ROS2 launch scripts for navigation2</description>
   <maintainer email="weijun.xie@elephantrobotics.com">lanni</maintainer>
   <license>BSD-3-Clause license</license>

agv_pro_navigation2/scripts/example_nav_through_poses.py

@@ -0,0 +1,101 @@
+#! /usr/bin/env python3
+
+from geometry_msgs.msg import PoseStamped
+from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
+import rclpy
+from rclpy.duration import Duration
+
+
+"""
+Basic navigation demo to go to poses.
+"""
+
+def set_initial_pose(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float):
+    """
+    Set the initial pose of the robot for AMCL localization.
+
+    Args:
+        navigator (BasicNavigator): The navigator instance controlling the robot.
+        x (float): Initial X position in the map frame.
+        y (float): Initial Y position in the map frame.
+        oz (float): Orientation Z component (quaternion).
+        ow (float): Orientation W component (quaternion).
+    """
+    initial_pose = PoseStamped()
+    initial_pose.header.frame_id = 'map'
+    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
+    initial_pose.pose.position.x = x
+    initial_pose.pose.position.y = y
+    initial_pose.pose.orientation.z = oz
+    initial_pose.pose.orientation.w = ow
+    navigator.setInitialPose(initial_pose)
+
+def create_pose(navigator: BasicNavigator, x, y, z, w):
+    pose = PoseStamped()
+    pose.header.frame_id = 'map'
+    pose.header.stamp = navigator.get_clock().now().to_msg()
+    pose.pose.position.x = x
+    pose.pose.position.y = y
+    pose.pose.orientation.z = z
+    pose.pose.orientation.w = w
+    return pose
+
+def nav_through_pose(navigator: BasicNavigator, goal_poses, verbose: bool = False) -> bool:
+    
+    nav_start = navigator.get_clock().now()
+    navigator.goThroughPoses(goal_poses)
+
+    while not navigator.isTaskComplete():
+        feedback = navigator.getFeedback()
+        if feedback and verbose:
+            remaining = Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9
+            print(f"Estimated time of arrival: {remaining:.0f} seconds")
+
+    # Do something depending on the return code
+    result = navigator.getResult()
+    if result == TaskResult.SUCCEEDED:
+        print('Goal succeeded!')
+        return True
+    elif result == TaskResult.CANCELED:
+        print('Goal was canceled!')
+    elif result == TaskResult.FAILED:
+        print('Goal failed!')
+    else:
+        print('Goal has an invalid return status!')
+    return False
+
+if __name__ == '__main__':
+    rclpy.init()
+
+    navigator = BasicNavigator()
+
+    # Set robot initial pose
+    # set_initial_pose(navigator, x=-1.9248794317245483, y=-0.5366987586021423, oz=-1.8463129131030735e-06, ow=0.9999999999982956)\
+    
+    # Wait for navigation to fully activate, since autostarting nav2
+    # navigator.waitUntilNav2Active()
+
+    way1_goals = [
+        [0.5062479972839355, -0.5562516450881958, -0.011363976322727884, 0.9999354279362925],
+        [1.7874977588653564, -0.6250066757202148, 0.7002746726771927, 0.7138735061667792],
+        [1.3625057935714722, 1.5999948978424072, 0.9999809382375775, 0.006174395637980891],
+        [-1.4687445163726807, 1.487505555152893, -0.9025521753719631, 0.43058050435584877]
+    ]
+    way2_goals = [
+        [0.5062479972839355, -0.5562516450881958, -0.011363976322727884, 0.9999354279362925],
+        [1.7874977588653564, -0.6250066757202148, 0.7002746726771927, 0.7138735061667792],
+        [1.3625057935714722, 1.5999948978424072, 0.9999809382375775, 0.006174395637980891],
+        [-1.4687445163726807, 1.487505555152893, -0.9025521753719631, 0.43058050435584877]
+    ]
+
+    goal_poses_1 = [create_pose(navigator, *g) for g in way1_goals]
+    goal_poses_2 = [create_pose(navigator, *g) for g in way2_goals]
+    
+    result1 = nav_through_pose(navigator, goal_poses_1, verbose=True)
+    print(f"First segment navigation result: {result1}")
+    
+    if result1 ==True:
+        result2 = nav_through_pose(navigator, goal_poses_2, verbose=True)
+        print(f"Second segment navigation result: {result2}")
+
+    rclpy.shutdown()

agv_pro_navigation2/scripts/example_nav_to_pose.py

@@ -0,0 +1,97 @@
+#! /usr/bin/env python3
+
+from geometry_msgs.msg import PoseStamped
+from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
+import rclpy
+from rclpy.duration import Duration
+
+"""
+Basic navigation demo to go to pose.
+"""
+
+def set_initial_pose(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float):
+    """
+    Set the initial pose of the robot for AMCL localization.
+
+    Args:
+        navigator (BasicNavigator): The navigator instance controlling the robot.
+        x (float): Initial X position in the map frame.
+        y (float): Initial Y position in the map frame.
+        oz (float): Orientation Z component (quaternion).
+        ow (float): Orientation W component (quaternion).
+    """
+    initial_pose = PoseStamped()
+    initial_pose.header.frame_id = 'map'
+    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
+    initial_pose.pose.position.x = x
+    initial_pose.pose.position.y = y
+    initial_pose.pose.orientation.z = oz
+    initial_pose.pose.orientation.w = ow
+    navigator.setInitialPose(initial_pose)
+
+
+def navigate_to_goal(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float, verbose: bool = False) -> bool:
+    """
+    Navigate the robot to a target goal pose.
+
+    Args:
+        navigator (BasicNavigator): The navigator instance controlling the robot.
+        x (float): Goal X position in the map frame.
+        y (float): Goal Y position in the map frame.
+        oz (float): Orientation Z component (quaternion).
+        ow (float): Orientation W component (quaternion).
+        verbose (bool, optional): If True, prints navigation feedback such as estimated arrival time. Default is False.
+
+    Returns:
+        bool: True if navigation succeeded, False otherwise.
+    """
+    goal_pose = PoseStamped()
+    goal_pose.header.frame_id = 'map'
+    goal_pose.header.stamp = navigator.get_clock().now().to_msg()
+    goal_pose.pose.position.x = x
+    goal_pose.pose.position.y = y
+    goal_pose.pose.orientation.z = oz
+    goal_pose.pose.orientation.w = ow
+
+    navigator.goToPose(goal_pose)
+
+    while not navigator.isTaskComplete():
+        feedback = navigator.getFeedback()
+        if feedback and verbose:
+            remaining = Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9
+            print(f"Estimated time of arrival: {remaining:.0f} seconds")
+
+    result = navigator.getResult()
+    if result == TaskResult.SUCCEEDED:
+        print('Goal succeeded!')
+        return True
+    elif result == TaskResult.CANCELED:
+        print('Goal was canceled!')
+    elif result == TaskResult.FAILED:
+        print('Goal failed!')
+    else:
+        print('Goal has an invalid return status!')
+    return False
+
+if __name__ == '__main__':
+    rclpy.init()
+    navigator = BasicNavigator()
+
+    # Set robot initial pose
+    # set_initial_pose(navigator, x=-1.9248794317245483, y=-0.5366987586021423, oz=-1.8463129131030735e-06, ow=0.9999999999982956)
+
+    # Wait for navigation to fully activate, since autostarting nav2
+    # navigator.waitUntilNav2Active()
+
+    goal_A = [1.6766083240509033,0.37930558800697327,-0.03491306994337919, 0.9993903529387947]
+    goal_B = [-0.5062443017959595,1.559376835823059,0.6869307039904945,0.7267229237578264]
+
+    x_goal, y_goal, orientation_z, orientation_w = goal_A
+    success = navigate_to_goal(navigator, x_goal, y_goal, orientation_z, orientation_w)
+    print("Navigation result:", success)
+
+    x_goal, y_goal, orientation_z, orientation_w = goal_B
+    success = navigate_to_goal(navigator, x_goal, y_goal, orientation_z, orientation_w)
+    print("Navigation result:", success)
+
+    rclpy.shutdown()

agv_pro_navigation2/scripts/example_waypoint_follower.py

@@ -0,0 +1,108 @@
+#! /usr/bin/env python3
+
+from geometry_msgs.msg import PoseStamped
+from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
+import rclpy
+from rclpy.duration import Duration
+
+"""
+Basic navigation demo to go to poses.
+"""
+
+def set_initial_pose(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float):
+    """
+    Set the initial pose of the robot for AMCL localization.
+
+    Args:
+        navigator (BasicNavigator): The navigator instance controlling the robot.
+        x (float): Initial X position in the map frame.
+        y (float): Initial Y position in the map frame.
+        oz (float): Orientation Z component (quaternion).
+        ow (float): Orientation W component (quaternion).
+    """
+    initial_pose = PoseStamped()
+    initial_pose.header.frame_id = 'map'
+    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
+    initial_pose.pose.position.x = x
+    initial_pose.pose.position.y = y
+    initial_pose.pose.orientation.z = oz
+    initial_pose.pose.orientation.w = ow
+    navigator.setInitialPose(initial_pose)
+
+def create_pose(navigator: BasicNavigator, x, y, z, w):
+    pose = PoseStamped()
+    pose.header.frame_id = 'map'
+    pose.header.stamp = navigator.get_clock().now().to_msg()
+    pose.pose.position.x = x
+    pose.pose.position.y = y
+    pose.pose.orientation.z = z
+    pose.pose.orientation.w = w
+    return pose
+
+def nav_waypoint_follower(navigator: BasicNavigator, goal_poses, verbose: bool = False) -> bool:
+    
+    nav_start = navigator.get_clock().now()
+    navigator.followWaypoints(goal_poses)
+
+    i = 0
+    while not navigator.isTaskComplete():
+        # Do something with the feedback
+        i = i + 1
+        feedback = navigator.getFeedback()
+        if (feedback and i % 5) and verbose == 0:
+            print('Executing current waypoint: ' +
+                  str(feedback.current_waypoint + 1) + '/' + str(len(goal_poses)))
+            now = navigator.get_clock().now()
+
+            # Some navigation timeout to demo cancellation
+            if now - nav_start > Duration(seconds=600.0):
+                navigator.cancelTask()
+
+    # Do something depending on the return code
+    result = navigator.getResult()
+    if result == TaskResult.SUCCEEDED:
+        print('Goal succeeded!')
+        return True
+    elif result == TaskResult.CANCELED:
+        print('Goal was canceled!')
+    elif result == TaskResult.FAILED:
+        print('Goal failed!')
+    else:
+        print('Goal has an invalid return status!')
+    return False
+
+if __name__ == '__main__':
+    rclpy.init()
+
+    navigator = BasicNavigator()
+
+    # Set robot initial pose
+    # set_initial_pose(navigator, x=-1.9248794317245483, y=-0.5366987586021423, oz=-1.8463129131030735e-06, ow=0.9999999999982956)\
+    
+    # Wait for navigation to fully activate, since autostarting nav2
+    # navigator.waitUntilNav2Active()
+
+    way1_goals = [
+        [0.5062479972839355, -0.5562516450881958, -0.011363976322727884, 0.9999354279362925],
+        [1.7874977588653564, -0.6250066757202148, 0.7002746726771927, 0.7138735061667792],
+        [1.3625057935714722, 1.5999948978424072, 0.9999809382375775, 0.006174395637980891],
+        [-1.4687445163726807, 1.487505555152893, -0.9025521753719631, 0.43058050435584877]
+    ]
+    way2_goals = [
+        [0.5062479972839355, -0.5562516450881958, -0.011363976322727884, 0.9999354279362925],
+        [1.7874977588653564, -0.6250066757202148, 0.7002746726771927, 0.7138735061667792],
+        [1.3625057935714722, 1.5999948978424072, 0.9999809382375775, 0.006174395637980891],
+        [-1.4687445163726807, 1.487505555152893, -0.9025521753719631, 0.43058050435584877]
+    ]
+
+    goal_poses_1 = [create_pose(navigator, *g) for g in way1_goals]
+    goal_poses_2 = [create_pose(navigator, *g) for g in way2_goals]
+  
+    result1 = nav_waypoint_follower(navigator, goal_poses_1, verbose=False)
+    print(f"First segment navigation result: {result1}")
+    
+    if result1 ==True:
+        result2 = nav_waypoint_follower(navigator, goal_poses_2, verbose=False)
+        print(f"Second segment navigation result: {result2}")
+
+    rclpy.shutdown()

navigation2/nav2_rviz_plugins/CMakeLists.txt

@@ -36,6 +36,7 @@ set(nav2_rviz_plugins_headers_to_moc
   include/nav2_rviz_plugins/goal_pose_updater.hpp
   include/nav2_rviz_plugins/goal_common.hpp
   include/nav2_rviz_plugins/goal_tool.hpp
+  include/nav2_rviz_plugins/charger_tool.hpp
   include/nav2_rviz_plugins/nav2_panel.hpp
   include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp
   include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp
@@ -49,6 +50,7 @@ set(library_name ${PROJECT_NAME})
 
 add_library(${library_name} SHARED
   src/goal_tool.cpp
+  src/charger_tool.cpp
   src/nav2_panel.cpp
   src/particle_cloud_display/flat_weighted_arrows_array.cpp
   src/particle_cloud_display/particle_cloud_display.cpp

navigation2/nav2_rviz_plugins/include/nav2_rviz_plugins/charger_tool.hpp

@@ -0,0 +1,69 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_RVIZ_PLUGINS__CHARGER_TOOL_HPP_
+#define NAV2_RVIZ_PLUGINS__CHARGER_TOOL_HPP_
+
+#include <QObject>
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/qos.hpp"
+#include <memory>
+
+#include "rviz_default_plugins/tools/pose/pose_tool.hpp"
+#include "rviz_default_plugins/visibility_control.hpp"
+
+namespace rviz_common
+{
+
+class DisplayContext;
+
+namespace properties
+{
+class StringProperty;
+class QosProfileProperty;
+}  // namespace properties
+}  // namespace rviz_common
+
+namespace nav2_rviz_plugins
+{
+
+class RVIZ_DEFAULT_PLUGINS_PUBLIC ChargerTool : public rviz_default_plugins::tools::PoseTool
+{
+  Q_OBJECT
+
+public:
+  ChargerTool();
+  ~ChargerTool() override;
+
+  void onInitialize() override;
+
+protected:
+  void onPoseSet(double x, double y, double theta) override;
+
+private Q_SLOTS:
+  void updateTopic();
+
+private:
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr publisher_;
+  rclcpp::Clock::SharedPtr clock_;
+  rviz_common::properties::StringProperty * topic_property_;
+  rviz_common::properties::QosProfileProperty * qos_profile_property_;
+
+  rclcpp::QoS qos_profile_;
+};
+
+}  // namespace nav2_rviz_plugins
+
+#endif  // NAV2_RVIZ_PLUGINS__CHARGER_TOOL_HPP_

navigation2/nav2_rviz_plugins/plugins_description.xml

@@ -6,6 +6,12 @@
     <description>A tool used to specify the navigation goal pose.</description>
   </class>
 
+  <class name="nav2_rviz_plugins/ChargerTool"
+         type="nav2_rviz_plugins::GoalTool"
+         base_class_type="rviz_common::Tool">
+    <description>A tool used to specify the charging stations goal pose.</description>
+  </class>
+
   <class name="nav2_rviz_plugins/Navigation 2"
          type="nav2_rviz_plugins::Nav2Panel"
          base_class_type="rviz_common::Panel">

navigation2/nav2_rviz_plugins/src/charger_tool.cpp

@@ -0,0 +1,87 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_rviz_plugins/charger_tool.hpp"
+
+#include <memory>
+#include <string>
+
+#include "rviz_common/display_context.hpp"
+#include "rviz_common/load_resource.hpp"
+#include "rviz_common/properties/string_property.hpp"
+#include "rviz_common/properties/qos_profile_property.hpp"
+
+namespace nav2_rviz_plugins
+{
+
+ChargerTool::ChargerTool()
+: rviz_default_plugins::tools::PoseTool(), qos_profile_(5)
+{
+  shortcut_key_ = 'c';
+  topic_property_ = new rviz_common::properties::StringProperty(
+    "Topic", "charger_position_update",
+    "The topic on which to publish goals.",
+    getPropertyContainer(), SLOT(updateTopic()), this);
+
+  qos_profile_property_ = new rviz_common::properties::QosProfileProperty(
+    topic_property_, qos_profile_);
+}
+
+ChargerTool::~ChargerTool()
+{
+}
+
+void ChargerTool::onInitialize()
+{
+  PoseTool::onInitialize();
+  setName("Charger Update");
+  setIcon(rviz_common::loadPixmap("package://rviz_default_plugins/icons/classes/SetGoal.png"));
+  updateTopic();
+}
+
+void ChargerTool::updateTopic()
+{
+  rclcpp::Node::SharedPtr raw_node =
+    context_->getRosNodeAbstraction().lock()->get_raw_node();
+  // TODO(anhosi, wjwwood): replace with abstraction for publishers once available
+  publisher_ = raw_node->
+    template create_publisher<geometry_msgs::msg::PoseStamped>(
+    topic_property_->getStdString(), qos_profile_);
+  clock_ = raw_node->get_clock();
+}
+
+void
+ChargerTool::onPoseSet(double x, double y, double theta)
+{
+  std::string fixed_frame = context_->getFixedFrame().toStdString();
+
+  geometry_msgs::msg::PoseStamped goal;
+  goal.header.stamp = clock_->now();
+  goal.header.frame_id = fixed_frame;
+
+  goal.pose.position.x = x;
+  goal.pose.position.y = y;
+  goal.pose.position.z = 0.0;
+
+  goal.pose.orientation = orientationAroundZAxis(theta);
+
+  logPose("goal", goal.pose.position, goal.pose.orientation, theta, fixed_frame);
+
+  publisher_->publish(goal);
+}
+
+}  // namespace nav2_rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>  // NOLINT
+PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::ChargerTool, rviz_common::Tool)

rtabmap/.appveyor.yml

@@ -0,0 +1,152 @@
+
+branches:
+  only:
+    - master
+    - devel
+
+os: Visual Studio 2015
+
+clone_folder: c:\projects\rtabmap
+
+platform: x64
+configuration: Release
+
+init:
+  - cmake --version
+  - call "C:\Program Files\Microsoft SDKs\Windows\v7.1\Bin\SetEnv.cmd" /x64
+  - call "C:\Program Files (x86)\Microsoft Visual Studio 14.0\VC\vcvarsall.bat" x86_amd64
+
+install:
+  # To download from google drive
+  - set PATH=C:\Python38-x64;C:\Python38-x64\Scripts;%PATH%
+  - ps: py -m pip --disable-pip-version-check install gdown>=5.1.0 
+  # Qt
+  - set QTDIR=C:\Qt\5.10.1\msvc2015_64
+  # make sure Qt bin path is before cmake bin path to avoid copying qt5 dlls from cmake before qt installation
+  - set PATH=%QTDIR%\bin;%PATH%
+  # Boost
+  - set PATH=%PATH%;C:\Libraries\boost_1_62_0\lib64-msvc-14.0
+  # Openni2
+  - ps: wget 'https://dl.dropboxusercontent.com/s/d98jv79l6oy9fxz/OpenNI2.exe?dl=0' -outfile OpenNI2.exe
+  - cmd: OpenNI2.exe -o"C:\Program Files" -y
+  - ECHO "Installed OpenNI2:"
+  - ps: "ls \"C:/Program Files/OpenNI2\""
+  - set PATH=%PATH%;C:\Program Files\OpenNI2\Redist
+  - set OPENNI2_INCLUDE64=C:\Program Files\OpenNI2\Include
+  - set OPENNI2_LIB64=C:\Program Files\OpenNI2\Lib
+  - set OPENNI2_REDIST64=C:\Program Files\OpenNI2\Redist
+  # OpenCV
+  #- appveyor-retry appveyor DownloadFile http://downloads.sourceforge.net/project/opencvlibrary/4.5.2/opencv-4.5.2-vc14_vc15.exe
+  #- cmd: opencv-4.5.2-vc14_vc15.exe -o"C:\Program Files" -y
+  #- ECHO "Installed OpenCV:"
+  #- ps: "ls \"C:/Program Files/opencv/build\""
+  #- set PATH=%PATH%;C:\Program Files\opencv\build\x64\vc14\bin
+  - ps: wget 'https://dl.dropboxusercontent.com/s/o6ofn491bc0jso1/opencv450_vc14.exe?dl=0' -outfile opencv.exe
+  - cmd: opencv.exe -o"C:\Program Files" -y
+  - ECHO "Installed OpenCV:"
+  - ps: "ls \"C:/Program Files/opencv\""
+  - set PATH=%PATH%;C:\Program Files\opencv\x64\vc14\bin
+  # VTK (including QVTK)
+  - ps: wget 'https://dl.dropboxusercontent.com/s/1l33b5l3f3y52gf/VTK-6_3-msvc140.exe?dl=0' -outfile VTK-6_3.exe
+  - cmd: VTK-6_3.exe -o"C:\Program Files" -y
+  - ECHO "Installed VTK:"
+  - ps: "ls \"C:/Program Files/VTK\""
+  - set PATH=%PATH%;C:\Program Files\VTK\bin
+  # QHull 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/9widnk9msdsh2b8/Qhull-msvc140.exe?dl=0' -outfile Qhull.exe
+  - cmd: Qhull.exe -o"C:\Program Files" -y
+  - ECHO "Installed QHull:"
+  - ps: "ls \"C:/Program Files/Qhull\""
+  - set PATH=%PATH%;C:\Program Files\Qhull\bin
+  # FLANN 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/7k58jbmqa51sxmh/FLANN-msvc140.exe?dl=0' -outfile FLANN.exe
+  - cmd: FLANN.exe -o"C:\Program Files" -y
+  - ECHO "Installed FLANN:"
+  - ps: "ls \"C:/Program Files/FLANN\""
+  - set PATH=%PATH%;C:\Program Files\FLANN\bin
+  # Eigen 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/3v6i9i8dxj4o8ji/Eigen.exe?dl=0' -outfile Eigen.exe
+  - cmd: Eigen.exe -o"C:\Program Files" -y
+  - ECHO "Installed Eigen:"
+  - ps: "ls \"C:/Program Files/Eigen\""
+  # PCL
+  - ps: wget 'https://dl.dropboxusercontent.com/s/2iayr4lyqa50i9j/PCL_181_August2018_x64_vc14.exe?dl=0' -outfile PCL_1.8.1.exe
+  - cmd: PCL_1.8.1.exe -o"C:\Program Files" -y
+  - ECHO "Installed PCL:"
+  - ps: "ls \"C:/Program Files/PCL\""
+  - set PATH=%PATH%;C:\Program Files\PCL\bin
+  # zlib
+  - ps: gdown -q 0B46akLGdg-uaYm9MTTI4MUtUcmc
+  - ps: Expand-Archive zlib-1.2.8-vc2010-x64.zip -DestinationPath 'C:\Program Files'
+  - ECHO "Installed zlib:"
+  - ps: "ls \"C:/Program Files/zlib\""
+  - set PATH=%PATH%;C:\Program Files\zlib\bin
+  # g2o
+  - ps: wget 'https://dl.dropboxusercontent.com/s/ht74s5pa21wokzw/g2o.exe?dl=0' -outfile g2o.exe
+  - cmd: g2o.exe -o"C:\Program Files" -y
+  - ECHO "Installed g2o:"
+  - ps: "ls \"C:/Program Files/g2o\""
+  - set PATH=%PATH%;C:\Program Files\g2o\bin
+  # GTSAM
+  - ps: wget 'https://dl.dropboxusercontent.com/s/0fpr6r4cgsqmvhf/GTSAM-4_0_0_alpha2-msvc140.exe?dl=0' -outfile GTSAM.exe
+  - cmd: GTSAM.exe -o"C:\Program Files" -y
+  - ECHO "Installed GTSAM:"
+  - ps: "ls \"C:/Program Files/GTSAM\""
+  - set PATH=%PATH%;C:\Program Files\GTSAM\bin
+  # OctoMap
+  - ps: wget 'https://dl.dropboxusercontent.com/s/6jpxu0nm8ne6e54/octomap_x64_vc14.exe?dl=0' -outfile octomap.exe
+  - cmd: octomap.exe -o"C:\Program Files" -y
+  - ECHO "Installed OctoMap:"
+  - ps: "ls \"C:/Program Files/octomap-distribution\""
+  - set PATH=%PATH%;C:\Program Files\octomap-distribution\bin
+  # CPU-TSDF
+  - ps: wget 'https://dl.dropboxusercontent.com/s/mgges9va1uzxr0q/cpu_tsdf_sept2015_x64_vc14.exe?dl=0' -outfile cpu_tsdf.exe
+  - cmd: cpu_tsdf.exe -o"C:\Program Files" -y
+  - ECHO "Installed CPU-TSDF:"
+  - ps: "ls \"C:/Program Files/cpu_tsdf\""
+  - set PATH=%PATH%;C:\Program Files\cpu_tsdf\bin
+  # Open Chisel
+  - ps: wget 'https://dl.dropboxusercontent.com/s/0aaphcde4acrinm/open_chisel_x64_vc14.exe?dl=0' -outfile open_chisel.exe
+  - cmd: open_chisel.exe -o"C:\Program Files" -y
+  - ECHO "Installed Open Chisel:"
+  - ps: "ls \"C:/Program Files/open_chisel\""
+  - set PATH=%PATH%;C:\Program Files\open_chisel\bin
+  # yaml-cpp
+  - ps: wget 'https://dl.dropboxusercontent.com/s/22qfvftwj6zq8tj/yaml-cpp_x64_vc14.exe?dl=0' -outfile yaml-cpp.exe
+  - cmd: yaml-cpp.exe -o"C:\Program Files" -y
+  - ECHO "Installed yaml-cpp:"
+  - ps: "ls \"C:/Program Files/yaml-cpp\""
+  # RealSense2
+  - ps: wget 'https://github.com/IntelRealSense/librealsense/releases/download/v2.40.0/Intel.RealSense.SDK-WIN10-2.40.0.2482.exe' -outfile realsense2.exe
+  - cmd: realsense2.exe /VERYSILENT
+  - ECHO "Installed RealSense2:"
+  - ps: "ls \"C:/Program Files (x86)/Intel RealSense SDK 2.0\""
+  - set PATH=%PATH%;C:\Program Files (x86)\Intel RealSense SDK 2.0\bin\x64
+  - set RealSense2_ROOT_DIR=C:\Program Files (x86)\Intel RealSense SDK 2.0
+  # Kinect 4 Azure
+  - ps: wget 'https://download.microsoft.com/download/3/d/6/3d6d9e99-a251-4cf3-8c6a-8e108e960b4b/Azure%20Kinect%20SDK%201.4.1.exe' -outfile azure.exe
+  - cmd: azure.exe /quiet
+  - ECHO "Installed Kinect For Azure:"
+  - ps: "ls \"C:/Program Files/Azure Kinect SDK v1.4.1\""
+  - set PATH=%PATH%;C:\Program Files\Azure Kinect SDK v1.4.1\tools
+  - set K4A_ROOT_DIR=C:\Program Files\Azure Kinect SDK v1.4.1
+  
+before_build:
+  - cd c:\projects\rtabmap\build
+  - ECHO %PROGRAMFILES%
+  - ECHO %PATH%
+  - cmake -G "Visual Studio 14 2015 Win64" -DOpenCV_DIR="C:\Program Files\opencv\build" -DPCL_DIR="C:\Program Files\PCL\cmake" -DCPUTSDF_DIR="C:\Program Files\cpu_tsdf\share\cpu_tsdf" -Dyaml-cpp_DIR="C:\Program Files\yaml-cpp\CMake" -DBUILD_AS_BUNDLE=ON ..
+  
+after_build :
+  - cmake --build . --config Release --target package
+
+artifacts:
+  - path: build\RTABMap-*
+  
+notifications:
+  - provider: Email
+    to:
+      - matlabbe@gmail.com
+    on_build_success: false
+    on_build_failure: false
+    on_build_status_changed: true

rtabmap/.cproject

@@ -0,0 +1,610 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<?fileVersion 4.0.0?>
+
+<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
+	<storageModule moduleId="org.eclipse.cdt.core.settings">
+		<cconfiguration id="0.1790260204">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.1790260204" moduleId="org.eclipse.cdt.core.settings" name="Unix">
+				<externalSettings/>
+				<extensions>
+					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.PE" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.SOM" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.MachO" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.VCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
+				</extensions>
+			</storageModule>
+			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+				<configuration artifactName="RTAB-Map" buildProperties="" description="Ubuntu/Mac OS X" id="0.1790260204" name="Unix" parent="org.eclipse.cdt.build.core.prefbase.cfg">
+					<folderInfo id="0.1790260204." name="/" resourcePath="">
+						<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.379189688" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain">
+							<targetPlatform binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO;org.eclipse.cdt.core.SOM;org.eclipse.cdt.core.PE;org.eclipse.cdt.core.MachO64" id="org.eclipse.cdt.build.core.prefbase.toolchain.379189688.899900990" name=""/>
+							<builder arguments="-j4 -C ${ProjDirPath}/build VERBOSE=true" buildPath="${workspace_loc:/RTAB-Map}" command="make" id="org.eclipse.cdt.build.core.settings.default.builder.1868197384" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="org.eclipse.cdt.build.core.settings.default.builder"/>
+							<tool id="org.eclipse.cdt.build.core.settings.holder.libs.45793018" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/>
+							<tool id="org.eclipse.cdt.build.core.settings.holder.894008781" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder">
+								<option id="org.eclipse.cdt.build.core.settings.holder.undef.incpaths.584706788" name="Undefined Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.undef.incpaths" valueType="undefIncludePath">
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/ts/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/flann/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/core/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/features2d/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/imgproc/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/imgproc/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/highgui/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/ml/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/photo/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/legacy/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/include/opencv"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/include/opencv"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/calib3d/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/build"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/contrib/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/build"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/objdetect/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/gpu/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/stitching/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/core/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/calib3d/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/video/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/highgui/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/features2d/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/ml/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/nonfree/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/flann/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/gpu/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/videostab/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/video/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/legacy/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/contrib/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/objdetect/include"/>
+								</option>
+								<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1694566822" languageId="org.eclipse.cdt.core.assembly" languageName="Assembly" sourceContentType="org.eclipse.cdt.core.asmSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
+							</tool>
+							<tool id="org.eclipse.cdt.build.core.settings.holder.1789919121" name="GNU C++" superClass="org.eclipse.cdt.build.core.settings.holder">
+								<option id="org.eclipse.cdt.build.core.settings.holder.undef.incpaths.449413398" name="Undefined Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.undef.incpaths" valueType="undefIncludePath">
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/ts/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/flann/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/core/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/features2d/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/imgproc/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/imgproc/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/highgui/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/ml/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/photo/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/legacy/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/include/opencv"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/include/opencv"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/calib3d/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/build"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/contrib/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/build"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/objdetect/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/gpu/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/stitching/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/core/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/calib3d/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/video/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/highgui/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/features2d/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/ml/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/nonfree/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/flann/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/gpu/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/videostab/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/video/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/legacy/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/contrib/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/objdetect/include"/>
+								</option>
+								<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1666253491" languageId="org.eclipse.cdt.core.g++" languageName="GNU C++" sourceContentType="org.eclipse.cdt.core.cxxSource,org.eclipse.cdt.core.cxxHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
+							</tool>
+							<tool id="org.eclipse.cdt.build.core.settings.holder.1172725717" name="GNU C" superClass="org.eclipse.cdt.build.core.settings.holder">
+								<option id="org.eclipse.cdt.build.core.settings.holder.undef.incpaths.508497091" name="Undefined Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.undef.incpaths" valueType="undefIncludePath">
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/ts/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/flann/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/core/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/features2d/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/imgproc/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/imgproc/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/highgui/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/ml/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/photo/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/legacy/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/include/opencv"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/include/opencv"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/calib3d/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/build"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/contrib/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/build"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/objdetect/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/gpu/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/stitching/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/core/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/calib3d/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/video/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/highgui/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/features2d/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/ml/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/nonfree/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/flann/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/gpu/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/videostab/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/video/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/legacy/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/contrib/include"/>
+									<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/objdetect/include"/>
+								</option>
+								<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.230409752" languageId="org.eclipse.cdt.core.gcc" languageName="GNU C" sourceContentType="org.eclipse.cdt.core.cSource,org.eclipse.cdt.core.cHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
+							</tool>
+						</toolChain>
+					</folderInfo>
+					<sourceEntries>
+						<entry excluding="build" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
+					</sourceEntries>
+				</configuration>
+			</storageModule>
+			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
+			<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
+			<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
+		</cconfiguration>
+		<cconfiguration id="0.1790260204.1906025362">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.1790260204.1906025362" moduleId="org.eclipse.cdt.core.settings" name="MinGW">
+				<externalSettings/>
+				<extensions>
+					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.PE" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.SOM" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.MachO" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.VCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
+				</extensions>
+			</storageModule>
+			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+				<configuration artifactName="RTAB-Map" buildProperties="" description="Windows GCC" id="0.1790260204.1906025362" name="MinGW" parent="org.eclipse.cdt.build.core.prefbase.cfg">
+					<folderInfo id="0.1790260204.1906025362." name="/" resourcePath="">
+						<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.1655844623" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain">
+							<targetPlatform binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO;org.eclipse.cdt.core.SOM;org.eclipse.cdt.core.PE;org.eclipse.cdt.core.MachO64" id="org.eclipse.cdt.build.core.prefbase.toolchain.1655844623.223670391" name=""/>
+							<builder arguments="-C ${ProjDirPath}/build VERBOSE=true" buildPath="${workspace_loc:/RTAB-Map}" command="nmake" id="org.eclipse.cdt.build.core.settings.default.builder.1893174344" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="org.eclipse.cdt.build.core.settings.default.builder"/>
+							<tool id="org.eclipse.cdt.build.core.settings.holder.libs.359841287" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/>
+							<tool id="org.eclipse.cdt.build.core.settings.holder.2036478542" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder">
+								<option id="org.eclipse.cdt.build.core.settings.holder.undef.incpaths.562568401" name="Undefined Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.undef.incpaths"/>
+								<option id="org.eclipse.cdt.build.core.settings.holder.incpaths.1681573069" name="Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.incpaths" valueType="includePath">
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\utilite\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;M:\opencv-svn\build\install\include&quot;"/>
+								</option>
+								<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1080087738" languageId="org.eclipse.cdt.core.assembly" languageName="Assembly" sourceContentType="org.eclipse.cdt.core.asmSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
+							</tool>
+							<tool id="org.eclipse.cdt.build.core.settings.holder.2076089335" name="GNU C++" superClass="org.eclipse.cdt.build.core.settings.holder">
+								<option id="org.eclipse.cdt.build.core.settings.holder.incpaths.1846481663" name="Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.incpaths" valueType="includePath">
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\utilite\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;M:\opencv-svn\build\install\include&quot;"/>
+								</option>
+								<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.865357815" languageId="org.eclipse.cdt.core.g++" languageName="GNU C++" sourceContentType="org.eclipse.cdt.core.cxxSource,org.eclipse.cdt.core.cxxHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
+							</tool>
+							<tool id="org.eclipse.cdt.build.core.settings.holder.469283430" name="GNU C" superClass="org.eclipse.cdt.build.core.settings.holder">
+								<option id="org.eclipse.cdt.build.core.settings.holder.incpaths.1029977695" name="Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.incpaths" valueType="includePath">
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\utilite\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;M:\opencv-svn\build\install\include&quot;"/>
+								</option>
+								<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1452552582" languageId="org.eclipse.cdt.core.gcc" languageName="GNU C" sourceContentType="org.eclipse.cdt.core.cSource,org.eclipse.cdt.core.cHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
+							</tool>
+						</toolChain>
+					</folderInfo>
+					<sourceEntries>
+						<entry excluding="build" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
+					</sourceEntries>
+				</configuration>
+			</storageModule>
+			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
+			<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
+			<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
+		</cconfiguration>
+		<cconfiguration id="0.1790260204.1906025362.1064002412">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.1790260204.1906025362.1064002412" moduleId="org.eclipse.cdt.core.settings" name="NMake">
+				<externalSettings/>
+				<extensions>
+					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.PE" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.SOM" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.MachO" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.VCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
+				</extensions>
+			</storageModule>
+			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+				<configuration artifactName="RTAB-Map" buildProperties="" description="Windows Visual Studio" id="0.1790260204.1906025362.1064002412" name="NMake" parent="org.eclipse.cdt.build.core.prefbase.cfg">
+					<folderInfo id="0.1790260204.1906025362.1064002412." name="/" resourcePath="">
+						<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.2087892413" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain">
+							<targetPlatform binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO;org.eclipse.cdt.core.SOM;org.eclipse.cdt.core.PE;org.eclipse.cdt.core.MachO64" id="org.eclipse.cdt.build.core.prefbase.toolchain.2087892413.1761856661" name=""/>
+							<builder arguments="" autoBuildTarget="all" buildPath="${ProjDirPath}/build" cleanBuildTarget="clean" command="nmake" enableAutoBuild="false" enableCleanBuild="true" enabledIncrementalBuild="true" id="org.eclipse.cdt.build.core.settings.default.builder.826759435" incrementalBuildTarget="all" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="false" superClass="org.eclipse.cdt.build.core.settings.default.builder"/>
+							<tool id="org.eclipse.cdt.build.core.settings.holder.libs.1653679154" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/>
+							<tool id="org.eclipse.cdt.build.core.settings.holder.1610311642" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder">
+								<option id="org.eclipse.cdt.build.core.settings.holder.undef.incpaths.1896517824" name="Undefined Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.undef.incpaths"/>
+								<option id="org.eclipse.cdt.build.core.settings.holder.incpaths.700166558" name="Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.incpaths" valueType="includePath">
+									<listOptionValue builtIn="false" value="&quot;C:\opencv\build\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\Microsoft Visual Studio 10.0\VC\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\include\pcl-1.6&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\3rdParty\Boost\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\3rdParty\Eigen\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Qt\4.8.0\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\3rdParty\VTK\include\vtk-5.8&quot;"/>
+								</option>
+								<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.112516982" languageId="org.eclipse.cdt.core.assembly" languageName="Assembly" sourceContentType="org.eclipse.cdt.core.asmSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
+							</tool>
+							<tool id="org.eclipse.cdt.build.core.settings.holder.1962594580" name="GNU C++" superClass="org.eclipse.cdt.build.core.settings.holder">
+								<option id="org.eclipse.cdt.build.core.settings.holder.incpaths.2029836145" name="Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.incpaths" valueType="includePath">
+									<listOptionValue builtIn="false" value="&quot;C:\opencv\build\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\Microsoft Visual Studio 10.0\VC\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\include\pcl-1.6&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\3rdParty\Boost\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\3rdParty\Eigen\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Qt\4.8.0\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\3rdParty\VTK\include\vtk-5.8&quot;"/>
+								</option>
+								<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.2132965672" languageId="org.eclipse.cdt.core.g++" languageName="GNU C++" sourceContentType="org.eclipse.cdt.core.cxxSource,org.eclipse.cdt.core.cxxHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
+							</tool>
+							<tool id="org.eclipse.cdt.build.core.settings.holder.463224635" name="GNU C" superClass="org.eclipse.cdt.build.core.settings.holder">
+								<option id="org.eclipse.cdt.build.core.settings.holder.incpaths.119135709" name="Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.incpaths" valueType="includePath">
+									<listOptionValue builtIn="false" value="&quot;C:\opencv\build\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\Microsoft Visual Studio 10.0\VC\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\include\pcl-1.6&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\3rdParty\Boost\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\3rdParty\Eigen\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Qt\4.8.0\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\3rdParty\VTK\include\vtk-5.8&quot;"/>
+								</option>
+								<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1193489184" languageId="org.eclipse.cdt.core.gcc" languageName="GNU C" sourceContentType="org.eclipse.cdt.core.cSource,org.eclipse.cdt.core.cHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
+							</tool>
+						</toolChain>
+					</folderInfo>
+					<sourceEntries>
+						<entry excluding="build" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
+					</sourceEntries>
+				</configuration>
+			</storageModule>
+			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
+			<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
+			<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
+		</cconfiguration>
+		<cconfiguration id="0.1790260204.1906025362.1064002412.862027363">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.1790260204.1906025362.1064002412.862027363" moduleId="org.eclipse.cdt.core.settings" name="NMake JOM">
+				<externalSettings/>
+				<extensions>
+					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.PE" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.SOM" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.MachO" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.VCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
+				</extensions>
+			</storageModule>
+			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+				<configuration artifactName="RTAB-Map" buildProperties="" description="Windows Visual Studio" id="0.1790260204.1906025362.1064002412.862027363" name="NMake JOM" parent="org.eclipse.cdt.build.core.prefbase.cfg">
+					<folderInfo id="0.1790260204.1906025362.1064002412.862027363." name="/" resourcePath="">
+						<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.650919836" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain">
+							<targetPlatform binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO;org.eclipse.cdt.core.SOM;org.eclipse.cdt.core.PE;org.eclipse.cdt.core.MachO64" id="org.eclipse.cdt.build.core.prefbase.toolchain.650919836.781470244" name=""/>
+							<builder arguments="-j4" buildPath="${ProjDirPath}/build" command="jom" id="org.eclipse.cdt.build.core.settings.default.builder.241450996" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="org.eclipse.cdt.build.core.settings.default.builder"/>
+							<tool id="org.eclipse.cdt.build.core.settings.holder.libs.1417678663" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/>
+							<tool id="org.eclipse.cdt.build.core.settings.holder.1222553174" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder">
+								<option id="org.eclipse.cdt.build.core.settings.holder.undef.incpaths.745694290" name="Undefined Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.undef.incpaths"/>
+								<option id="org.eclipse.cdt.build.core.settings.holder.incpaths.1773576415" name="Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.incpaths" valueType="includePath">
+									<listOptionValue builtIn="false" value="&quot;C:\opencv-2.4.9\build\install\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files\OpenNI2\Include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\Microsoft Visual Studio 10.0\VC\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Qt\4.8.0\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\pcl-1.7.1\build\install\include\pcl-1.7&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files\Boost\include&quot;"/>
+								</option>
+								<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1835279712" languageId="org.eclipse.cdt.core.assembly" languageName="Assembly" sourceContentType="org.eclipse.cdt.core.asmSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
+							</tool>
+							<tool id="org.eclipse.cdt.build.core.settings.holder.1771312621" name="GNU C++" superClass="org.eclipse.cdt.build.core.settings.holder">
+								<option id="org.eclipse.cdt.build.core.settings.holder.incpaths.555639917" name="Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.incpaths" valueType="includePath">
+									<listOptionValue builtIn="false" value="&quot;C:\opencv-2.4.9\build\install\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files\OpenNI2\Include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\Microsoft Visual Studio 10.0\VC\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Qt\4.8.0\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\pcl-1.7.1\build\install\include\pcl-1.7&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files\Boost\include&quot;"/>
+								</option>
+								<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.469730673" languageId="org.eclipse.cdt.core.g++" languageName="GNU C++" sourceContentType="org.eclipse.cdt.core.cxxSource,org.eclipse.cdt.core.cxxHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
+							</tool>
+							<tool id="org.eclipse.cdt.build.core.settings.holder.1010336558" name="GNU C" superClass="org.eclipse.cdt.build.core.settings.holder">
+								<option id="org.eclipse.cdt.build.core.settings.holder.incpaths.227176509" name="Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.incpaths" valueType="includePath">
+									<listOptionValue builtIn="false" value="&quot;C:\opencv-2.4.9\build\install\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files\OpenNI2\Include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\Microsoft Visual Studio 10.0\VC\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Qt\4.8.0\include&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\pcl-1.7.1\build\install\include\pcl-1.7&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Program Files\Boost\include&quot;"/>
+								</option>
+								<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.867578216" languageId="org.eclipse.cdt.core.gcc" languageName="GNU C" sourceContentType="org.eclipse.cdt.core.cSource,org.eclipse.cdt.core.cHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
+							</tool>
+						</toolChain>
+					</folderInfo>
+					<sourceEntries>
+						<entry excluding="build" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
+					</sourceEntries>
+				</configuration>
+			</storageModule>
+			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
+			<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
+			<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
+		</cconfiguration>
+	</storageModule>
+	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+		<project id="CTAB-Map.null.2089313587" name="CTAB-Map"/>
+	</storageModule>
+	<storageModule moduleId="refreshScope" versionNumber="1">
+		<resource resourceType="PROJECT" workspacePath="/rtabmap"/>
+	</storageModule>
+	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
+	<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
+	<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
+		<buildTargets>
+			<target name="CMake-MinGW-Debug" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>cmake</buildCommand>
+				<buildArguments>-E chdir build/ cmake -G "MinGW Makefiles" -D CMAKE_BUILD_TYPE=Debug -D BUILD_TESTS=ON ../</buildArguments>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="CMake-MinGW-Release" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>cmake</buildCommand>
+				<buildArguments>-E chdir build/ cmake -G "MinGW Makefiles" -D CMAKE_BUILD_TYPE=Release -D BUILD_TESTS=ON ../</buildArguments>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="CMake-Unix-Debug" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>cmake</buildCommand>
+				<buildArguments>-E chdir build/ cmake -G "Unix Makefiles" -D CMAKE_BUILD_TYPE=Debug -D BUILD_TESTS=OFF ../</buildArguments>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="CMake-Unix-Release" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>cmake</buildCommand>
+				<buildArguments>-E chdir build/ cmake -G "Unix Makefiles" -D CMAKE_BUILD_TYPE=Release  -D BUILD_TESTS=OFF ../</buildArguments>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="CMake-NMake-Debug" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>cmake</buildCommand>
+				<buildArguments>-E chdir build/ cmake -G "NMake Makefiles" -D CMAKE_BUILD_TYPE=Debug -D BUILD_TESTS=ON ../</buildArguments>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="CMake-NMake-Release" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>cmake</buildCommand>
+				<buildArguments>-G "NMake Makefiles" -D CMAKE_BUILD_TYPE=Release ../</buildArguments>
+				<buildTarget/>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>false</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+		</buildTargets>
+	</storageModule>
+	<storageModule moduleId="scannerConfiguration">
+		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
+			<buildOutputProvider>
+				<openAction enabled="true" filePath=""/>
+				<parser enabled="true"/>
+			</buildOutputProvider>
+			<scannerInfoProvider id="specsFile">
+				<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
+				<parser enabled="true"/>
+			</scannerInfoProvider>
+		</profile>
+		<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
+			<buildOutputProvider>
+				<openAction enabled="true" filePath=""/>
+				<parser enabled="true"/>
+			</buildOutputProvider>
+			<scannerInfoProvider id="makefileGenerator">
+				<runAction arguments="-f ${project_name}_scd.mk" command="make" useDefault="true"/>
+				<parser enabled="true"/>
+			</scannerInfoProvider>
+		</profile>
+		<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
+			<buildOutputProvider>
+				<openAction enabled="true" filePath=""/>
+				<parser enabled="true"/>
+			</buildOutputProvider>
+			<scannerInfoProvider id="specsFile">
+				<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
+				<parser enabled="true"/>
+			</scannerInfoProvider>
+		</profile>
+		<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
+			<buildOutputProvider>
+				<openAction enabled="true" filePath=""/>
+				<parser enabled="true"/>
+			</buildOutputProvider>
+			<scannerInfoProvider id="specsFile">
+				<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
+				<parser enabled="true"/>
+			</scannerInfoProvider>
+		</profile>
+		<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
+			<buildOutputProvider>
+				<openAction enabled="true" filePath=""/>
+				<parser enabled="true"/>
+			</buildOutputProvider>
+			<scannerInfoProvider id="specsFile">
+				<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
+				<parser enabled="true"/>
+			</scannerInfoProvider>
+		</profile>
+		<scannerConfigBuildInfo instanceId="0.1790260204.1906025362.1064002412.862027363">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="0.1790260204">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"/>
+			<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
+				<buildOutputProvider>
+					<openAction enabled="true" filePath=""/>
+					<parser enabled="true"/>
+				</buildOutputProvider>
+				<scannerInfoProvider id="specsFile">
+					<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
+					<parser enabled="true"/>
+				</scannerInfoProvider>
+			</profile>
+			<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
+				<buildOutputProvider>
+					<openAction enabled="true" filePath=""/>
+					<parser enabled="true"/>
+				</buildOutputProvider>
+				<scannerInfoProvider id="makefileGenerator">
+					<runAction arguments="-f ${project_name}_scd.mk" command="make" useDefault="true"/>
+					<parser enabled="true"/>
+				</scannerInfoProvider>
+			</profile>
+			<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
+				<buildOutputProvider>
+					<openAction enabled="true" filePath=""/>
+					<parser enabled="true"/>
+				</buildOutputProvider>
+				<scannerInfoProvider id="specsFile">
+					<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
+					<parser enabled="true"/>
+				</scannerInfoProvider>
+			</profile>
+			<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
+				<buildOutputProvider>
+					<openAction enabled="true" filePath=""/>
+					<parser enabled="true"/>
+				</buildOutputProvider>
+				<scannerInfoProvider id="specsFile">
+					<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
+					<parser enabled="true"/>
+				</scannerInfoProvider>
+			</profile>
+			<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
+				<buildOutputProvider>
+					<openAction enabled="true" filePath=""/>
+					<parser enabled="true"/>
+				</buildOutputProvider>
+				<scannerInfoProvider id="specsFile">
+					<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
+					<parser enabled="true"/>
+				</scannerInfoProvider>
+			</profile>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="0.1790260204.1906025362.1064002412">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="0.1790260204.1906025362">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"/>
+			<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
+				<buildOutputProvider>
+					<openAction enabled="true" filePath=""/>
+					<parser enabled="true"/>
+				</buildOutputProvider>
+				<scannerInfoProvider id="specsFile">
+					<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
+					<parser enabled="true"/>
+				</scannerInfoProvider>
+			</profile>
+			<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
+				<buildOutputProvider>
+					<openAction enabled="true" filePath=""/>
+					<parser enabled="true"/>
+				</buildOutputProvider>
+				<scannerInfoProvider id="makefileGenerator">
+					<runAction arguments="-f ${project_name}_scd.mk" command="make" useDefault="true"/>
+					<parser enabled="true"/>
+				</scannerInfoProvider>
+			</profile>
+			<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
+				<buildOutputProvider>
+					<openAction enabled="true" filePath=""/>
+					<parser enabled="true"/>
+				</buildOutputProvider>
+				<scannerInfoProvider id="specsFile">
+					<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
+					<parser enabled="true"/>
+				</scannerInfoProvider>
+			</profile>
+			<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
+				<buildOutputProvider>
+					<openAction enabled="true" filePath=""/>
+					<parser enabled="true"/>
+				</buildOutputProvider>
+				<scannerInfoProvider id="specsFile">
+					<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
+					<parser enabled="true"/>
+				</scannerInfoProvider>
+			</profile>
+			<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
+				<buildOutputProvider>
+					<openAction enabled="true" filePath=""/>
+					<parser enabled="true"/>
+				</buildOutputProvider>
+				<scannerInfoProvider id="specsFile">
+					<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
+					<parser enabled="true"/>
+				</scannerInfoProvider>
+			</profile>
+			<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile">
+				<buildOutputProvider>
+					<openAction enabled="true" filePath=""/>
+					<parser enabled="true"/>
+				</buildOutputProvider>
+				<scannerInfoProvider id="specsFile">
+					<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
+					<parser enabled="true"/>
+				</scannerInfoProvider>
+			</profile>
+			<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileCPP">
+				<buildOutputProvider>
+					<openAction enabled="true" filePath=""/>
+					<parser enabled="true"/>
+				</buildOutputProvider>
+				<scannerInfoProvider id="specsFile">
+					<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
+					<parser enabled="true"/>
+				</scannerInfoProvider>
+			</profile>
+			<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileC">
+				<buildOutputProvider>
+					<openAction enabled="true" filePath=""/>
+					<parser enabled="true"/>
+				</buildOutputProvider>
+				<scannerInfoProvider id="specsFile">
+					<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
+					<parser enabled="true"/>
+				</scannerInfoProvider>
+			</profile>
+		</scannerConfigBuildInfo>
+	</storageModule>
+</cproject>

rtabmap/.devcontainer/android/Dockerfile

@@ -0,0 +1,24 @@
+FROM introlab3it/rtabmap:android-noble-deps
+
+# remove ubuntu user
+RUN touch /var/mail/ubuntu && chown ubuntu /var/mail/ubuntu && userdel -r ubuntu
+
+RUN apt-get update && apt-get install -y sudo && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ARG USERNAME=vscode
+ARG USER_UID=1000
+ARG USER_GID=1000
+
+RUN set -ex && \
+    groupadd --gid ${USER_GID} ${USERNAME} && \
+    useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \
+    usermod -a -G sudo ${USERNAME} && \
+    echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \
+    chmod 0440 /etc/sudoers.d/${USERNAME}
+
+RUN chmod +x /opt/android-sdk/tools/android
+
+RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
+
+

rtabmap/.devcontainer/android/devcontainer.json

@@ -0,0 +1,21 @@
+{
+    "build": {
+      "dockerfile": "Dockerfile"
+    },
+    "customizations": {
+        "vscode": {
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "vscjava.vscode-java-pack"]
+        }
+      },
+    "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind",
+    "workspaceFolder": "/home/vscode/rtabmap",
+    "postStartCommand": "./.devcontainer/android/init.sh",
+    "settings": {
+      "terminal.integrated.defaultProfile.linux": "bash"
+    },
+    "remoteUser": "vscode",
+    "runArgs": ["--privileged", "--network=host"]
+}
+
+
+  

rtabmap/.devcontainer/android/init.sh

@@ -0,0 +1,17 @@
+#!/bin/bash
+set -e
+
+echo "Running post-start initialization..."
+
+# copy required jars
+cp /opt/android/lib/*.jar app/android/libs/.
+
+mkdir -p build_android/arm64-v8a
+
+# resource tool
+cd build_android
+cmake -DANDROID_PREBUILD=ON ..
+make
+
+echo -e "\nTo build the APK, do (adjust API number):"
+echo -e '\nexport ANDROID_API=30 && cd build_android/arm64-v8a && cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$ANDROID_API -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a -DCMAKE_FIND_ROOT_PATH="/opt/android/arm64-v8a/bin;/opt/android/arm64-v8a;/opt/android/arm64-v8a/share" -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DOpenCV_DIR=/opt/android/arm64-v8a/sdk/native/jni ../..\nmake -j6\n'

rtabmap/.devcontainer/bionic/devcontainer.json

@@ -0,0 +1,8 @@
+{
+    "image": "introlab3it/rtabmap:18.04",
+    "customizations": {
+        "vscode": {
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
+        }
+      }
+}

rtabmap/.devcontainer/focal/devcontainer.json

@@ -0,0 +1,8 @@
+{
+    "image": "introlab3it/rtabmap:20.04",
+    "customizations": {
+        "vscode": {
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
+        }
+      }
+}

rtabmap/.devcontainer/jammy/devcontainer.json

@@ -0,0 +1,8 @@
+{
+    "image": "introlab3it/rtabmap:22.04",
+    "customizations": {
+        "vscode": {
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
+        }
+      }
+}

rtabmap/.devcontainer/noble/devcontainer.json

@@ -0,0 +1,8 @@
+{
+    "image": "introlab3it/rtabmap:24.04",
+    "customizations": {
+        "vscode": {
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
+        }
+      }
+}

rtabmap/.devcontainer/rolling/Dockerfile

@@ -0,0 +1,77 @@
+
+FROM ubuntu:24.04
+
+ENV DEBIAN_FRONTEND=noninteractive
+
+# Install ROS2
+RUN apt update && \
+    apt install software-properties-common -y && \
+    add-apt-repository universe && \
+    apt update && \
+    apt install curl -y && \
+    curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \
+    echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+# Install build dependencies
+RUN apt-get update && \
+    apt upgrade -y && \
+    apt-get install -y \
+      git \
+      wget \
+      libtbb-dev \
+      libproj-dev \
+      libpcl-dev \
+      liboctomap-dev \
+      libfreenect-dev \
+      ros-rolling-ros-base \
+      ros-dev-tools \
+      ros-rolling-cv-bridge \
+      ros-rolling-image-geometry \
+      ros-rolling-laser-geometry \
+      ros-rolling-pcl-conversions \
+      ros-rolling-rviz-common \
+      ros-rolling-rviz-rendering \
+      ros-rolling-rviz-default-plugins \
+      ros-rolling-pcl-ros \
+      ros-rolling-imu-filter-madgwick \
+      ros-rolling-image-transport \
+      ros-rolling-octomap-msgs \
+      ros-rolling-libg2o \
+      ros-rolling-gtsam \
+      ros-rolling-libpointmatcher \
+      ros-rolling-qt-gui-cpp \
+      ros-rolling-diagnostic-updater && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+WORKDIR /root/
+
+RUN rm /bin/sh && ln -s /bin/bash /bin/sh
+
+RUN echo -e '#!/bin/bash\nset -e\n\n# setup ros2 environment\nsource "/opt/ros/rolling/setup.bash" --\nexec "$@"' > /ros_entrypoint.sh
+RUN chmod +x /ros_entrypoint.sh
+ENTRYPOINT [ "/ros_entrypoint.sh" ]
+
+# ros2 seems not sourcing by default its multi-arch folders
+ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/rolling/lib/x86_64-linux-gnu
+
+# For devcontainer
+# remove ubuntu user
+RUN touch /var/mail/ubuntu && chown ubuntu /var/mail/ubuntu && userdel -r ubuntu
+
+RUN apt-get update && apt-get install -y sudo && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ARG USERNAME=vscode
+ARG USER_UID=1000
+ARG USER_GID=1000
+
+RUN set -ex && \
+    groupadd --gid ${USER_GID} ${USERNAME} && \
+    useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \
+    usermod -a -G sudo ${USERNAME} && \
+    echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \
+    chmod 0440 /etc/sudoers.d/${USERNAME}
+
+RUN echo "source /usr/share/bash-completion/completions/git" >> /home/${USERNAME}/.bashrc
+

rtabmap/.devcontainer/rolling/devcontainer.json

@@ -0,0 +1,17 @@
+{
+    "build": {
+      "dockerfile": "Dockerfile"
+    },
+    "customizations": {
+        "vscode": {
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
+        }
+      },
+    "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind",
+    "workspaceFolder": "/home/vscode/rtabmap",
+    "settings": {
+      "terminal.integrated.defaultProfile.linux": "bash"
+    },
+    "remoteUser": "vscode",
+    "runArgs": ["--privileged", "--network=host"]
+}

rtabmap/.dockerignore

@@ -0,0 +1,2 @@
+build/*
+build_*

rtabmap/.github/workflows/cmake-ros.yml

@@ -0,0 +1,74 @@
+name: CMake-ROS
+
+on:
+  push:
+    branches: 
+      - master
+  pull_request:
+    branches: 
+      - '**'
+
+env:
+  # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
+  BUILD_TYPE: Release
+
+jobs:
+  build:
+    # The CMake configure and build commands are platform agnostic and should work equally
+    # well on Windows or Mac.  You can convert this to a matrix build if you need
+    # cross-platform coverage.
+    # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
+    name: Build on ros ${{ matrix.ros_distribution }} and ${{ matrix.os }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      fail-fast: false
+      matrix:
+        ros_distribution: [ humble, jazzy, kilted, rolling ]
+        include:
+        - ros_distribution: 'humble'
+          os: ubuntu-22.04
+        - ros_distribution: 'jazzy'
+          os: ubuntu-24.04
+        - ros_distribution: 'kilted'
+          os: ubuntu-24.04
+        - ros_distribution: 'rolling'
+          os: ubuntu-24.04
+
+    steps:   
+    - name: Setup ROS2
+      # https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debs.html
+      run: |
+        sudo apt install software-properties-common
+        sudo add-apt-repository universe
+        sudo apt update && sudo apt install curl -y
+        export ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest | grep -F "tag_name" | awk -F\" '{print $4}')
+        curl -L -o /tmp/ros2-apt-source.deb "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.$(. /etc/os-release && echo $VERSION_CODENAME)_all.deb"
+        sudo apt install /tmp/ros2-apt-source.deb
+        sudo apt update
+
+    - uses: ros-tooling/setup-ros@v0.7
+      with:
+        required-ros-distributions: ${{ matrix.ros_distribution }}
+    
+    - uses: actions/checkout@v4
+
+    - name: Install dependencies
+      run: |
+        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
+        rosdep update
+        rosdep install --from-paths ${{github.workspace}} -y
+
+    - name: Configure CMake
+      run: |
+        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
+        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}}
+
+    - name: Build
+      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
+
+    - name: Info
+      working-directory: ${{github.workspace}}/build/bin
+      run: |
+        source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
+        ./rtabmap-console --version
+      

rtabmap/.github/workflows/cmake.yml

@@ -0,0 +1,56 @@
+name: CMake
+
+on:
+  push:
+    branches: 
+      - master
+  pull_request:
+    branches: 
+      - '**'
+
+env:
+  BUILD_TYPE: Release
+
+jobs:
+  build:
+    name: ${{ matrix.os }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      fail-fast: false
+      matrix:
+        os: [ubuntu-24.04, ubuntu-22.04]
+        include:
+        - os: ubuntu-22.04
+          extra_deps: "libunwind-dev libceres-dev"
+          extra_cmake_def: ""
+        - os: ubuntu-24.04
+          extra_deps: "libg2o-dev libceres-dev"
+          extra_cmake_def: "-DWITH_CERES=ON"
+
+    steps:    
+    - name: Install dependencies
+      run: |
+        DEBIAN_FRONTEND=noninteractive
+        sudo apt-get update
+        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev ${{ matrix.extra_deps }}
+
+    - uses: actions/checkout@v4
+
+    - name: Configure CMake
+      run: |
+        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{ matrix.extra_cmake_def }}
+
+    - name: Build
+      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
+
+    - name: Info
+      working-directory: ${{github.workspace}}/build/bin
+      run: |
+        ./rtabmap-console --version
+
+#    - name: Test
+#      working-directory: ${{github.workspace}}/build
+#      # Execute tests defined by the CMake configuration.  
+#      # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail
+#      run: ctest -C ${{env.BUILD_TYPE}}
+      

rtabmap/.github/workflows/docker.yml

@@ -0,0 +1,210 @@
+name: docker
+
+on:
+  push:
+    branches:
+      - 'master'
+
+jobs:
+  docker_deps:
+
+    # Disabling ###-deps step from CI because it is too flaky (seg faults, arm64 build timeout...)
+    # Only way I was able to build all images is to do it from a ubuntu 20.04 computer with:
+    #    $ sudo add-apt-repository ppa:canonical-server/server-backports
+    #    $ sudo apt-get update
+    #    $ sudo apt-get upgrade qemu-user-static
+    #    $ docker run --rm --privileged multiarch/qemu-user-static --reset -p yes -c yes
+    # More info: https://github.com/introlab/rtabmap/issues/1454
+    # if: false
+
+    runs-on: ubuntu-latest
+    
+    strategy:
+      fail-fast: false
+      matrix:
+        docker_tag: [focal-deps, jammy-deps, noble-deps, noble-kilted-deps]
+        include:
+        - docker_tag: focal-deps
+          docker_tags: |
+            introlab3it/rtabmap:focal-deps
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'focal/deps'
+        - docker_tag: jammy-deps
+          docker_tags: |
+            introlab3it/rtabmap:jammy-deps
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'jammy/deps'
+        - docker_tag: noble-deps
+          docker_tags: |
+            introlab3it/rtabmap:noble-deps
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'noble/deps'
+        - docker_tag: noble-kilted-deps
+          docker_tags: |
+            introlab3it/rtabmap:noble-kilted-deps
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'noble-kilted/deps'
+    
+    steps:
+      -
+        name: Checkout
+        uses: actions/checkout@v2
+      - 
+        name: Set up QEMU
+        uses: docker/setup-qemu-action@v3
+        with:
+          platforms: all
+      -
+        name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v3
+      -
+        name: Login to DockerHub
+        uses: docker/login-action@v3
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+      -
+        name: Build and push
+        uses: docker/build-push-action@v6
+        with:
+          context: .
+          push: true
+          platforms: ${{ matrix.docker_platforms }}
+          file: ./docker/${{ matrix.docker_path }}/Dockerfile
+          tags: ${{ matrix.docker_tags }}
+          cache-from: type=registry,ref=introlab3it/rtabmap:${{ matrix.docker_tag }}
+          cache-to: type=inline
+  
+  docker:
+    needs: docker_deps
+    runs-on: ubuntu-latest
+    
+    strategy:
+      fail-fast: false
+      matrix:
+        docker_tag: [bionic, focal, jammy, noble, noble-kilted, android23, android24, android26, android30]
+        include:
+        - docker_tag: bionic
+          docker_tags: |
+            introlab3it/rtabmap:bionic
+            introlab3it/rtabmap:18.04
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'bionic'
+        - docker_tag: focal
+          docker_tags: |
+            introlab3it/rtabmap:focal
+            introlab3it/rtabmap:20.04
+            introlab3it/rtabmap:latest
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'focal'
+        - docker_tag: jammy
+          docker_tags: |
+            introlab3it/rtabmap:jammy
+            introlab3it/rtabmap:22.04
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'jammy'
+        - docker_tag: noble
+          docker_tags: |
+            introlab3it/rtabmap:noble
+            introlab3it/rtabmap:24.04
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'noble'
+        - docker_tag: noble-kilted
+          docker_tags: |
+            introlab3it/rtabmap:noble-kilted
+          docker_args: |
+            NOT_USED=0
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'noble-kilted'
+        - docker_tag: android23
+          docker_tags: |
+            introlab3it/rtabmap:android23
+            introlab3it/rtabmap:tango
+          docker_args: |
+            API_VERSION=23
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'noble/android/rtabmap_apiXX'
+        - docker_tag: android24
+          docker_tags: |
+            introlab3it/rtabmap:android24
+          docker_args: |
+            API_VERSION=24
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'noble/android/rtabmap_apiXX'
+        - docker_tag: android26
+          docker_tags: |
+            introlab3it/rtabmap:android26
+          docker_args: |
+            API_VERSION=26
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'noble/android/rtabmap_apiXX'
+        - docker_tag: android30
+          docker_tags: |
+            introlab3it/rtabmap:android30
+          docker_args: |
+            API_VERSION=30
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'noble/android/rtabmap_apiXX'
+    
+    steps:
+      -
+        name: Checkout
+        uses: actions/checkout@v2
+      - 
+        name: Set up QEMU
+        uses: docker/setup-qemu-action@v3
+        with:
+          platforms: all
+      -
+        name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v3
+      -
+        name: Login to DockerHub
+        uses: docker/login-action@v3
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+      -
+        name: Build and push
+        uses: docker/build-push-action@v6
+        with:
+          context: .
+          push: true
+          platforms: ${{ matrix.docker_platforms }}
+          file: ./docker/${{ matrix.docker_path }}/Dockerfile
+          build-args: |
+            ${{ matrix.docker_args }}
+          tags: ${{ matrix.docker_tags }}
+          cache-from: type=registry,ref=introlab3it/rtabmap:${{ matrix.docker_tag }}
+          cache-to: type=inline
+

rtabmap/.github/workflows/scheduled-stats.yml

@@ -0,0 +1,16 @@
+name: RTAB-Map Scheduled Stats Extraction From GitHub
+
+on:
+    workflow_dispatch:
+    schedule:
+        - cron: '0 5 * * *'
+jobs:
+    get_stats:
+        runs-on: ubuntu-latest
+        steps:
+            - name: Update Stats
+              uses: introlab/github-stats-action@v1
+              with:
+                  github-stats-token: ${{ secrets.STATS_TOKEN }}
+                  google-application-credentials: ${{ secrets.GOOGLE_APPLICATION_CREDENTIALS }}
+                  spreadsheet-id: ${{ secrets.SPREADSHEET_ID }}

rtabmap/.gitignore

@@ -0,0 +1,14 @@
+/lib
+.DS_Store
+.settings/language.settings.xml
+.idea/
+.vscode
+cmake-build-debug/
+app/android/.classpath
+app/android/.project
+app/android/AndroidManifest.xml
+app/android/res/raw/
+compile_flags.txt
+tags
+build_*
+*.bak

rtabmap/.project

@@ -0,0 +1,82 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>rtabmap</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
+			<triggers>clean,full,incremental,</triggers>
+			<arguments>
+				<dictionary>
+					<key>?name?</key>
+					<value></value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.append_environment</key>
+					<value>true</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.autoBuildTarget</key>
+					<value>all</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.buildArguments</key>
+					<value>-j4 -C ${ProjDirPath}/build VERBOSE=true</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.buildCommand</key>
+					<value>make</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.buildLocation</key>
+					<value>${workspace_loc:/RTAB-Map}</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.cleanBuildTarget</key>
+					<value>clean</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.contents</key>
+					<value>org.eclipse.cdt.make.core.activeConfigSettings</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.enableAutoBuild</key>
+					<value>false</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.enableCleanBuild</key>
+					<value>true</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.enableFullBuild</key>
+					<value>true</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.fullBuildTarget</key>
+					<value>all</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.stopOnError</key>
+					<value>true</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.useDefaultBuildCmd</key>
+					<value>false</value>
+				</dictionary>
+			</arguments>
+		</buildCommand>
+		<buildCommand>
+			<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.eclipse.cdt.core.cnature</nature>
+		<nature>org.eclipse.cdt.core.ccnature</nature>
+		<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
+		<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
+	</natures>
+</projectDescription>

rtabmap/.settings/org.eclipse.cdt.codan.core.prefs

@@ -0,0 +1,67 @@
+eclipse.preferences.version=1
+org.eclipse.cdt.codan.checkers.errnoreturn=Warning
+org.eclipse.cdt.codan.checkers.errnoreturn.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},implicit\=>false}
+org.eclipse.cdt.codan.checkers.errreturnvalue=Error
+org.eclipse.cdt.codan.checkers.errreturnvalue.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.checkers.noreturn=Error
+org.eclipse.cdt.codan.checkers.noreturn.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},implicit\=>false}
+org.eclipse.cdt.codan.internal.checkers.AbstractClassCreation=Error
+org.eclipse.cdt.codan.internal.checkers.AbstractClassCreation.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.AmbiguousProblem=Error
+org.eclipse.cdt.codan.internal.checkers.AmbiguousProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.AssignmentInConditionProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.AssignmentInConditionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.AssignmentToItselfProblem=Error
+org.eclipse.cdt.codan.internal.checkers.AssignmentToItselfProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.CaseBreakProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.CaseBreakProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},no_break_comment\=>"no break",last_case_param\=>true,empty_case_param\=>false}
+org.eclipse.cdt.codan.internal.checkers.CatchByReference=Warning
+org.eclipse.cdt.codan.internal.checkers.CatchByReference.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},unknown\=>false,exceptions\=>()}
+org.eclipse.cdt.codan.internal.checkers.CircularReferenceProblem=Error
+org.eclipse.cdt.codan.internal.checkers.CircularReferenceProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.ClassMembersInitialization=Warning
+org.eclipse.cdt.codan.internal.checkers.ClassMembersInitialization.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},skip\=>true}
+org.eclipse.cdt.codan.internal.checkers.FieldResolutionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.FieldResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.FunctionResolutionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.FunctionResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.InvalidArguments=Error
+org.eclipse.cdt.codan.internal.checkers.InvalidArguments.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.InvalidTemplateArgumentsProblem=Error
+org.eclipse.cdt.codan.internal.checkers.InvalidTemplateArgumentsProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.LabelStatementNotFoundProblem=Error
+org.eclipse.cdt.codan.internal.checkers.LabelStatementNotFoundProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.MemberDeclarationNotFoundProblem=Error
+org.eclipse.cdt.codan.internal.checkers.MemberDeclarationNotFoundProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.MethodResolutionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.MethodResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.NamingConventionFunctionChecker=-Info
+org.eclipse.cdt.codan.internal.checkers.NamingConventionFunctionChecker.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},pattern\=>"^[a-z]",macro\=>true,exceptions\=>()}
+org.eclipse.cdt.codan.internal.checkers.NonVirtualDestructorProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.NonVirtualDestructorProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.OverloadProblem=Error
+org.eclipse.cdt.codan.internal.checkers.OverloadProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.RedeclarationProblem=Error
+org.eclipse.cdt.codan.internal.checkers.RedeclarationProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.RedefinitionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.RedefinitionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.ReturnStyleProblem=-Warning
+org.eclipse.cdt.codan.internal.checkers.ReturnStyleProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.ScanfFormatStringSecurityProblem=-Warning
+org.eclipse.cdt.codan.internal.checkers.ScanfFormatStringSecurityProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.StatementHasNoEffectProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.StatementHasNoEffectProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},macro\=>true,exceptions\=>()}
+org.eclipse.cdt.codan.internal.checkers.SuggestedParenthesisProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.SuggestedParenthesisProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},paramNot\=>false}
+org.eclipse.cdt.codan.internal.checkers.SuspiciousSemicolonProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.SuspiciousSemicolonProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},else\=>false,afterelse\=>false}
+org.eclipse.cdt.codan.internal.checkers.TypeResolutionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.TypeResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.UnusedFunctionDeclarationProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.UnusedFunctionDeclarationProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},macro\=>true}
+org.eclipse.cdt.codan.internal.checkers.UnusedStaticFunctionProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.UnusedStaticFunctionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},macro\=>true}
+org.eclipse.cdt.codan.internal.checkers.UnusedVariableDeclarationProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.UnusedVariableDeclarationProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true},macro\=>true,exceptions\=>("@(\#)","$Id")}
+org.eclipse.cdt.codan.internal.checkers.VariableResolutionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.VariableResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>true,RUN_ON_INC_BUILD\=>true,RUN_ON_FILE_OPEN\=>false,RUN_ON_FILE_SAVE\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}

rtabmap/.settings/org.eclipse.cdt.core.prefs

@@ -0,0 +1,3 @@
+eclipse.preferences.version=1
+environment/project/0.1790260204.1906025362.1064002412/append=true
+environment/project/0.1790260204.1906025362.1064002412/appendContributed=true

rtabmap/LICENSE

@@ -0,0 +1,28 @@
+RTAB-Map - https://github.com/introlab/rtabmap
+Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved.
+Copyright (c) XXX, contributors, all rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* 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.
+
+* Neither the name of the copyright holders nor the names of the 
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS 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.

rtabmap/README.md

@@ -0,0 +1,82 @@
+rtabmap
+=======
+
+[![RTAB-Map Logo](https://raw.githubusercontent.com/introlab/rtabmap/master/guilib/src/images/RTAB-Map100.png)](http://introlab.github.io/rtabmap)
+
+[![Release][release-image]][releases]
+[![Downloads][downloads-image]][downloads]
+[![License][license-image]][license]
+
+[release-image]: https://img.shields.io/badge/release-0.21.4-green.svg?style=flat
+[releases]: https://github.com/introlab/rtabmap/releases
+
+[downloads-image]: https://img.shields.io/github/downloads/introlab/rtabmap/total?label=downloads
+[downloads]: https://github.com/introlab/rtabmap/releases
+
+[license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat
+[license]: https://github.com/introlab/rtabmap/blob/master/LICENSE
+
+RTAB-Map library and standalone application.
+
+ * For more information (e.g., papers, major updates), visit [RTAB-Map's home page](http://introlab.github.io/rtabmap).
+ * For installation instructions and examples, visit [RTAB-Map's wiki](https://github.com/introlab/rtabmap/wiki).
+
+To use RTAB-Map under ROS, visit the [rtabmap](http://wiki.ros.org/rtabmap) page on the ROS wiki.
+
+### Acknowledgements
+This project is supported by [IntRoLab - Intelligent / Interactive / Integrated / Interdisciplinary Robot Lab](https://introlab.3it.usherbrooke.ca/), Sherbrooke, Québec, Canada.
+
+<a href="https://introlab.3it.usherbrooke.ca/">
+<img src="https://github.com/introlab/16SoundsUSB/blob/master/images/IntRoLab.png" alt="IntRoLab" height="100">
+</a>
+
+#### CI Latest
+
+  <table>
+    <tbody>
+        <tr>
+           <td>Linux</td>
+           <td><a href="https://github.com/introlab/rtabmap/actions/workflows/cmake.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/docker.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg" alt="Build Status"/>
+           </td>
+        </tr>
+        <tr>
+           <td>Windows</td>
+           <td><a href="https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master"><img src="https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true" alt="Build Status"/>
+           </td>
+        </tr>
+     </tbody>
+  </table>
+ 
+ #### ROS Binaries
+ 
+ `ros-$ROS_DISTRO-rtabmap`
+ 
+ <table>
+    <tbody>
+        <tr>
+           <td rowspan="1">ROS 1</td>
+            <td>Noetic</td>
+            <td><a href="http://build.ros.org/job/Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td rowspan="3">ROS 2</td>
+            <td>Humble</td>
+            <td><a href="http://build.ros2.org/job/Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Jazzy</td>
+            <td><a href="http://build.ros2.org/job/Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+            <td>Rolling</td>
+            <td><a href="http://build.ros2.org/job/Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
+        </tr>
+        <tr>
+           <td>Docker</td>
+           <td>
+             <a href="https://hub.docker.com/r/introlab3it/rtabmap">rtabmap</a>
+           </td>
+           <td><img src="https://img.shields.io/docker/pulls/introlab3it/rtabmap" alt="Docker Pulls"/></td>
+        </tr>
+    </tbody>
+</table>

rtabmap/RTABMapConfig.cmake.in

@@ -0,0 +1,102 @@
+include(CMakeFindDependencyMacro)
+
+# Mandatory dependencies
+find_dependency(OpenCV COMPONENTS core calib3d imgproc highgui stitching photo video OPTIONAL_COMPONENTS aruco objdetect xfeatures2d nonfree gpu cudafeatures2d)
+
+if(EXISTS "${CMAKE_CURRENT_LIST_DIR}/RTABMap_guiTargets.cmake")
+    find_dependency(PCL 1.7 COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)
+    
+    if(@CONF_QT_VERSION@ EQUAL 6)
+        find_dependency(Qt6 COMPONENTS Widgets Core Gui OpenGL)
+    elseif(@CONF_QT_VERSION@ EQUAL 5)
+        find_dependency(Qt5 COMPONENTS Widgets Core Gui OpenGL)
+    else() # Qt4
+        find_dependency(Qt4 COMPONENTS QtCore QtGui)
+    endif()
+    set(RTABMap_QT_VERSION @CONF_QT_VERSION@)
+ELSE()
+    find_dependency(PCL 1.7 COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation)
+ENDIF()
+set(RTABMap_DEFINITIONS ${PCL_DEFINITIONS})
+add_definitions(${RTABMap_DEFINITIONS}) # To include -march=native if set
+
+# Optional dependencies
+IF(EXISTS "${CMAKE_CURRENT_LIST_DIR}/@CONF_MODULES_DIR@")
+    list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/@CONF_MODULES_DIR@")
+ENDIF()
+
+IF(@CONF_WITH_REALSENSE2@)
+    IF(WIN32)
+        find_dependency(RealSense2)
+    ELSE()
+        find_dependency(realsense2)
+    ENDIF()
+ENDIF()
+
+IF(@CONF_WITH_K4A@)
+    IF(WIN32)
+        find_dependency(K4A)
+    ELSE()
+        find_dependency(k4a)
+        find_dependency(k4arecord)
+    ENDIF()
+ENDIF()
+
+IF(@CONF_WITH_DEPTH_AI@)
+  find_dependency(depthai 2.24)
+ENDIF()
+
+IF(@CONF_WITH_XVSDK@)
+  find_dependency(xvsdk)
+ENDIF()
+
+IF(@CONF_WITH_OCTOMAP@)
+  find_dependency(octomap)
+ENDIF()
+
+IF(@CONF_WITH_PYTHON@)
+  find_dependency(Python3 COMPONENTS Interpreter Development NumPy)
+ENDIF()
+
+# Provide those for backward compatibilities (e.g., catkin requires them to propagate dependencies)
+set(RTABMap_INCLUDE_DIRS "")
+set(RTABMap_LIBRARIES "")
+set(RTABMap_TARGETS "")
+
+set(_RTABMap_supported_components utilite core gui)
+
+foreach(_comp ${_RTABMap_supported_components})
+  if(EXISTS "${CMAKE_CURRENT_LIST_DIR}/RTABMap_${_comp}Targets.cmake")
+    include("${CMAKE_CURRENT_LIST_DIR}/RTABMap_${_comp}Targets.cmake")
+    set(RTABMap_${_comp}_FOUND True)
+    set(RTABMap_TARGETS
+      ${RTABMap_TARGETS}
+      rtabmap::${_comp})
+    get_target_property(RTABMap_${_comp}_INCLUDE_DIRS rtabmap::${_comp} INTERFACE_INCLUDE_DIRECTORIES)
+    get_target_property(RTABMap_${_comp}_LIBRARIES rtabmap::${_comp} INTERFACE_LINK_LIBRARIES)
+    set(RTABMap_INCLUDE_DIRS 
+      ${RTABMap_INCLUDE_DIRS}
+      ${RTABMap_${_comp}_INCLUDE_DIRS})
+    set(RTABMap_LIBRARIES 
+      ${RTABMap_LIBRARIES}
+      rtabmap::${_comp})
+    if(RTABMap_${_comp}_LIBRARIES)
+      set(RTABMap_LIBRARIES 
+        ${RTABMap_LIBRARIES}
+        ${RTABMap_${_comp}_LIBRARIES})
+    endif()
+  else()
+    set(RTABMap_${_comp}_FOUND False)
+  endif()
+endforeach()
+
+include("${CMAKE_CURRENT_LIST_DIR}/RTABMapTargets.cmake")
+
+foreach(_comp ${RTABMap_FIND_COMPONENTS})
+  if (NOT RTABMap_${_comp}_FOUND)
+    if(${RTABMap_FIND_REQUIRED_${_comp}})
+      set(RTABMap_FOUND False)
+      set(RTABMap_NOT_FOUND_MESSAGE "Unsupported or not found required component: ${_comp}")
+    endif()
+  endif()
+endforeach()

rtabmap/Version.h.in

@@ -0,0 +1,106 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * 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.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 UNIVERTY DE SHERBROOKE 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 VERSION_H_
+#define VERSION_H_
+
+// This is auto-generated!
+#define RTABMAP_VERSION "@PROJECT_VERSION@"
+
+#define RTABMAP_VERSION_MAJOR @PROJECT_VERSION_MAJOR@
+#define RTABMAP_VERSION_MINOR @PROJECT_VERSION_MINOR@
+#define RTABMAP_VERSION_PATCH @PROJECT_VERSION_PATCH@
+
+#define RTABMAP_VERSION_COMPARE(major, minor, patch)  (major>=@PROJECT_VERSION_MAJOR@ || (major==@PROJECT_VERSION_MAJOR@ && minor>=@PROJECT_VERSION_MINOR@) || (major==@PROJECT_VERSION_MAJOR@ && minor==@PROJECT_VERSION_MINOR@ && patch >=@PROJECT_VERSION_PATCH@))
+
+@NONFREE@#define RTABMAP_NONFREE
+@TORO@#define RTABMAP_TORO
+@G2O@#define RTABMAP_G2O
+@G2O_CPP_CONF@#define RTABMAP_G2O_CPP11 @G2O_CPP11@
+@GTSAM@#define RTABMAP_GTSAM
+@CERES@#define RTABMAP_CERES
+@MRPT@#define RTABMAP_MRPT
+@VERTIGO@#define RTABMAP_VERTIGO
+@OPENNI@#define RTABMAP_OPENNI
+@OPENNI2@#define RTABMAP_OPENNI2
+@FREENECT@#define RTABMAP_FREENECT
+@FREENECT2@#define RTABMAP_FREENECT2
+@K4W2@#define RTABMAP_K4W2
+@K4A@#define RTABMAP_K4A
+@CVSBA@#define RTABMAP_CVSBA
+@POINTMATCHER@#define RTABMAP_POINTMATCHER
+@CCCORELIB@#define RTABMAP_CCCORELIB
+@OPEN3D@#define RTABMAP_OPEN3D
+@FASTCV@#define RTABMAP_FASTCV
+@OPENGV@#define RTABMAP_OPENGV
+@PDAL@#define RTABMAP_PDAL
+@LIBLAS@#define RTABMAP_LIBLAS
+@CUDASIFT@#define RTABMAP_CUDASIFT
+@LOAM@#define RTABMAP_LOAM
+@FLOAM@#define RTABMAP_FLOAM
+@DC1394@#define RTABMAP_DC1394
+@FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
+@ZED@#define RTABMAP_ZED
+@ZEDOC@#define RTABMAP_ZEDOC
+@REALSENSE@#define RTABMAP_REALSENSE
+@REALSENSESLAM@#define RTABMAP_REALSENSE_SLAM
+@REALSENSE2@#define RTABMAP_REALSENSE2
+@MYNTEYE@#define RTABMAP_MYNTEYE
+@DEPTHAI@#define RTABMAP_DEPTHAI
+@XVSDK@#define RTABMAP_XVSDK
+@OCTOMAP@#define RTABMAP_OCTOMAP
+@GRIDMAP@#define RTABMAP_GRIDMAP
+@CPUTSDF@#define RTABMAP_CPUTSDF
+@ALICE_VISION@#define RTABMAP_ALICE_VISION
+@OPENCHISEL@#define RTABMAP_OPENCHISEL
+@FOVIS@#define RTABMAP_FOVIS
+@VISO2@#define RTABMAP_VISO2
+@DVO@#define RTABMAP_DVO
+@OKVIS@#define RTABMAP_OKVIS
+@MSCKF_VIO@#define RTABMAP_MSCKF_VIO
+@VINS@#define RTABMAP_VINS
+@OPENVINS@#define RTABMAP_OPENVINS
+@ORB_SLAM@#define RTABMAP_ORB_SLAM @ORB_SLAM_VERSION@
+@ORB_OCTREE@#define RTABMAP_ORB_OCTREE
+@TORCH@#define RTABMAP_TORCH
+@PYTHON@#define RTABMAP_PYTHON
+@MADGWICK@#define RTABMAP_MADGWICK
+
+#include <pcl/pcl_config.h>
+
+#if PCL_VERSION_COMPARE(>, 1, 11, 1)
+#include <pcl/types.h>
+#define RTABMAP_PCL_INDEX pcl::index_t
+#elif PCL_VERSION_COMPARE(>=, 1, 10, 0)
+#define RTABMAP_PCL_INDEX std::uint32_t
+#else
+#include <pcl/pcl_macros.h>
+#define RTABMAP_PCL_INDEX pcl::uint32_t
+#endif
+
+#endif /* VERSION_H_ */
+

rtabmap/app/CMakeLists.txt

@@ -0,0 +1,6 @@
+
+IF(ANDROID)
+   ADD_SUBDIRECTORY( android )
+ELSE()
+   ADD_SUBDIRECTORY( src )
+ENDIF()

rtabmap/app/android/.gitignore

@@ -0,0 +1,2 @@
+/bin/
+/gen/

rtabmap/app/android/.settings/org.eclipse.jdt.core.prefs

@@ -0,0 +1,11 @@
+eclipse.preferences.version=1
+org.eclipse.jdt.core.compiler.codegen.inlineJsrBytecode=enabled
+org.eclipse.jdt.core.compiler.codegen.targetPlatform=1.6
+org.eclipse.jdt.core.compiler.codegen.unusedLocal=preserve
+org.eclipse.jdt.core.compiler.compliance=1.6
+org.eclipse.jdt.core.compiler.debug.lineNumber=generate
+org.eclipse.jdt.core.compiler.debug.localVariable=generate
+org.eclipse.jdt.core.compiler.debug.sourceFile=generate
+org.eclipse.jdt.core.compiler.problem.assertIdentifier=error
+org.eclipse.jdt.core.compiler.problem.enumIdentifier=error
+org.eclipse.jdt.core.compiler.source=1.6

rtabmap/app/android/AndroidManifest.xml.in

@@ -0,0 +1,88 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- BEGIN_INCLUDE(manifest) -->
+<manifest xmlns:android="http://schemas.android.com/apk/res/android"
+        package="com.introlab.rtabmap"
+        android:versionCode="72"
+        android:versionName="@RTABMAP_VERSION@">
+
+    <uses-permission android:name="android.permission.CAMERA" />
+    <uses-permission android:name="android.permission.READ_EXTERNAL_STORAGE" />
+    <uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE" />
+    <uses-permission android:name="android.permission.INTERNET" />
+    <uses-permission android:name="android.permission.ACCESS_NETWORK_STATE" />
+    <uses-permission android:name="android.permission.ACCESS_FINE_LOCATION" />
+    <uses-permission android:name="android.permission.ACCESS_WIFI_STATE" />
+    <uses-feature android:name="android.hardware.location.gps" />
+    <uses-feature android:glEsVersion="0x00020000" />
+
+    <!-- This is the platform API where depth16 support in android was introduced. -->
+    <uses-sdk android:minSdkVersion="@ANDROID_NATIVE_API_LEVEL@" />
+    
+    <queries>
+        <package android:name="com.google.ar.core" />
+        <package android:name="com.huawei.ar.engine" /> 
+    </queries>
+
+    <!-- This .apk has no Java code itself, so set hasCode to false. -->
+    <application 
+        android:label="@string/app_name" 
+        android:icon="@drawable/ic_launcher"
+        android:debuggable="@ANDROID_DEBUGGABLE@">
+        
+        <uses-library android:name="com.projecttango.libtango_device2" android:required="false" />
+        <meta-data android:name="com.google.ar.core" android:value="optional" />
+        <meta-data android:name="com.huawei.ar.engine" android:value="optional" />
+
+        <!-- Our activity is the built-in NativeActivity framework class.
+             This will take care of integrating with our NDK code. -->
+        <activity android:name="RTABMapActivity"
+                android:label="@string/app_name"
+                android:launchMode="singleTask"
+		android:screenOrientation="fullSensor"
+		android:configChanges="orientation|screenSize|keyboardHidden"
+		android:theme="@style/ThemeApp">
+            <!-- Tell NativeActivity the name of our .so -->
+            <meta-data android:name="android.app.lib_name"
+                    android:value="NativeRTABMap" />
+            <intent-filter>
+                <action android:name="android.intent.action.MAIN" />
+                <category android:name="android.intent.category.LAUNCHER" />
+		    </intent-filter>
+		    <intent-filter>
+		        <action android:name="android.intent.action.SEND" />
+		        <action android:name="android.intent.action.SEND_MULTIPLE" />
+		        <action android:name="android.intent.action.OPEN_DOCUMENT" />
+		        <category android:name="android.intent.category.DEFAULT" />
+		        <data android:mimeType="application/octet-stream" />
+		        <data android:pathPattern=".*\.db" />
+		    </intent-filter>
+		    
+        </activity>
+
+        <activity android:name="SettingsActivity" android:label="@string/settings" android:screenOrientation="fullSensor"/>
+        <activity android:name="SketchfabActivity" android:label="@string/sketchfab" android:screenOrientation="fullSensor"/>
+    
+        <meta-data
+            android:name="com.google.ar.core.min_apk_version"
+            android:value="191106000" /> <!-- This activity is critical for installing ARCore when it is not already present. -->
+        <activity
+            android:name="com.google.ar.core.InstallActivity"
+            android:configChanges="keyboardHidden|orientation|screenSize"
+            android:excludeFromRecents="true"
+            android:exported="false"
+            android:launchMode="singleTop"
+            android:theme="@android:style/Theme.Material.Light.Dialog.Alert" />
+            
+        <provider
+            android:name="android.support.v4.content.FileProvider"
+            android:authorities="com.introlab.rtabmap.provider"
+            android:exported="false"
+            android:grantUriPermissions="true">
+            <meta-data
+                android:name="android.support.FILE_PROVIDER_PATHS"
+                android:resource="@xml/provider_paths"/>
+        </provider>
+    </application>
+
+</manifest>
+<!-- END_INCLUDE(manifest) -->

rtabmap/app/android/CMakeLists.txt

@@ -0,0 +1,191 @@
+
+option(WITH_TANGO         "Include Tango support"                ON)
+option(WITH_ARCORE        "Include ARCore support"               ON)
+option(WITH_ARENGINE      "Include AREngine support"             ON)
+option(DISABLE_LOG        "Disable Android logging (should be true in release)"    ON)
+option(DEPTH_TEST         "Enable depth test on ARCore"    OFF)
+
+# Google Tango needs access to system shared 
+# libraries (e.g. libbinder.so) that are not accessible 
+# with android >=24
+IF(WITH_TANGO AND ${ANDROID_NATIVE_API_LEVEL} LESS 24)
+    FIND_PACKAGE(Tango QUIET)
+    IF(Tango_FOUND)
+       MESSAGE(STATUS "Found Tango: ${Tango_INCLUDE_DIRS}")
+    ENDIF(Tango_FOUND)
+ENDIF(WITH_TANGO AND ${ANDROID_NATIVE_API_LEVEL} LESS 24)
+
+IF(WITH_ARCORE AND ${ANDROID_NATIVE_API_LEVEL} GREATER 22)
+    FIND_PACKAGE(ARCore QUIET)
+    IF(ARCore_FOUND)
+       MESSAGE(STATUS "Found ARCore: ${ARCore_INCLUDE_DIRS}")
+    ENDIF(ARCore_FOUND)
+ENDIF(WITH_ARCORE AND ${ANDROID_NATIVE_API_LEVEL} GREATER 22)
+
+IF(WITH_ARENGINE AND ${ANDROID_NATIVE_API_LEVEL} GREATER 23)
+    FIND_PACKAGE(AREngine QUIET)
+    IF(AREngine_FOUND)
+       MESSAGE(STATUS "Found AREngine: ${AREngine_INCLUDE_DIRS}")
+    ENDIF(AREngine_FOUND)
+ENDIF(WITH_ARENGINE AND ${ANDROID_NATIVE_API_LEVEL} GREATER 23)
+
+IF(NOT Tango_FOUND)
+   SET(TANGO "//")
+ENDIF(NOT Tango_FOUND)
+IF(NOT ARCore_FOUND)
+   SET(ARCORE "//")
+ENDIF(NOT ARCore_FOUND)
+IF(NOT AREngine_FOUND)
+   SET(ARENGINE "//")
+ENDIF(NOT AREngine_FOUND)
+
+CONFIGURE_FILE(CameraAvailability.h.in ${CMAKE_CURRENT_SOURCE_DIR}/jni/CameraAvailability.h)
+
+
+IF(DISABLE_LOG)
+  ADD_DEFINITIONS(-DDISABLE_LOG)
+ENDIF(DISABLE_LOG)
+IF(DEPTH_TEST)
+  ADD_DEFINITIONS(-DDEPTH_TEST)
+ENDIF(DEPTH_TEST)
+
+MESSAGE(STATUS "--------------------------------------------")
+MESSAGE(STATUS "Android build info:")
+MESSAGE(STATUS "  DISABLE_LOG    = ${DISABLE_LOG}")
+MESSAGE(STATUS "  DEPTH_TEST     = ${DEPTH_TEST}")
+IF(Tango_FOUND)
+  MESSAGE(STATUS "  With Tango     = YES")
+ELSEIF(NOT WITH_TANGO)
+  MESSAGE(STATUS "  With Tango     = NO (WITH_TANGO=OFF)")
+ELSE()
+  IF(${ANDROID_NATIVE_API_LEVEL} GREATER 23)
+    MESSAGE(STATUS "  With Tango     = NO (ANDROID_NATIVE_API_LEVEL should be <= 23)")
+  ELSE()
+    MESSAGE(STATUS "  With Tango     = NO (tango not found)")
+  ENDIF()
+ENDIF()
+IF(ARCore_FOUND)
+  MESSAGE(STATUS "  With ARCore    = YES")
+ELSEIF(NOT WITH_ARCORE)
+  MESSAGE(STATUS "  With ARCore    = NO (WITH_ARCORE=OFF)")
+ELSE()
+  IF(${ANDROID_NATIVE_API_LEVEL} LESS 23)
+    MESSAGE(STATUS "  With ARCore    = NO (ANDROID_NATIVE_API_LEVEL should be >= 23)")
+  ELSE()
+    MESSAGE(STATUS "  With ARCore    = NO (ARCore not found)")
+  ENDIF()
+ENDIF()
+IF(AREngine_FOUND)
+  MESSAGE(STATUS "  With AREngine  = YES")
+ELSEIF(NOT WITH_ARENGINE)
+  MESSAGE(STATUS "  With AREngine  = NO (WITH_ARENGINE=OFF)")
+ELSE()
+  IF(${ANDROID_NATIVE_API_LEVEL} LESS 24)
+    MESSAGE(STATUS "  With AREngine  = NO (ANDROID_NATIVE_API_LEVEL should be >= 24)")
+  ELSE()
+    MESSAGE(STATUS "  With AREngine  = NO (AREngine not found)")
+  ENDIF()
+ENDIF()
+MESSAGE(STATUS "  ANDROID_NATIVE_API_LEVEL       = ${ANDROID_NATIVE_API_LEVEL}")
+MESSAGE(STATUS "  ANDROID_COMPILER_FLAGS_RELEASE = ${ANDROID_COMPILER_FLAGS_RELEASE}")
+MESSAGE(STATUS "  ANDROID_TOOLCHAIN_PREFIX       = ${ANDROID_TOOLCHAIN_PREFIX}")
+
+
+IF(DISABLE_LOG)
+   SET(ANDROID_DEBUGGABLE false)
+ELSE()
+   SET(ANDROID_DEBUGGABLE true)
+ENDIF()
+
+add_subdirectory(jni)
+
+
+######
+# Packaging
+######
+
+# find android
+find_host_program(ANDROID_EXECUTABLE
+  NAMES android
+  DOC   "The android command-line tool")
+if(NOT ANDROID_EXECUTABLE)
+  message(FATAL_ERROR "Can not find android command line tool: android")
+endif()
+
+configure_file(
+  "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml.in"
+  "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml"
+  @ONLY)
+  
+configure_file(
+  "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml"
+  "${CMAKE_CURRENT_BINARY_DIR}/AndroidManifest.xml"
+  COPYONLY)
+  
+configure_file(
+  "${CMAKE_CURRENT_SOURCE_DIR}/info.txt.in"
+  "${CMAKE_CURRENT_SOURCE_DIR}/res/raw/info.txt")
+  
+
+configure_file(
+  "${CMAKE_CURRENT_SOURCE_DIR}/ant.properties.in"
+  "${CMAKE_CURRENT_BINARY_DIR}/ant.properties"
+  @ONLY)
+  
+configure_file(
+  "${CMAKE_CURRENT_SOURCE_DIR}/custom_rules.xml"
+  "${CMAKE_CURRENT_BINARY_DIR}/custom_rules.xml"
+  COPYONLY)
+
+add_custom_target(NativeRTABMap-ant-configure ALL
+  COMMAND "${ANDROID_EXECUTABLE}"
+          update project
+          --name RTABMap
+          --path "${CMAKE_CURRENT_SOURCE_DIR}"
+          --target "android-${ANDROID_NATIVE_API_LEVEL}"
+  COMMAND "${CMAKE_COMMAND}" -E copy_if_different
+          "${CMAKE_CURRENT_SOURCE_DIR}/build.xml"
+          "${CMAKE_CURRENT_BINARY_DIR}/build.xml"
+  COMMAND "${CMAKE_COMMAND}" -E copy_if_different
+          "${CMAKE_CURRENT_SOURCE_DIR}/local.properties"
+          "${CMAKE_CURRENT_BINARY_DIR}/local.properties"
+  COMMAND "${CMAKE_COMMAND}" -E copy_if_different
+          "${CMAKE_CURRENT_SOURCE_DIR}/project.properties"
+          "${CMAKE_CURRENT_BINARY_DIR}/project.properties"
+  COMMAND "${CMAKE_COMMAND}" -E copy_if_different
+          "${CMAKE_CURRENT_SOURCE_DIR}/proguard-project.txt"
+          "${CMAKE_CURRENT_BINARY_DIR}/proguard-project.txt"
+  COMMAND "${CMAKE_COMMAND}" -E remove
+          "${CMAKE_CURRENT_SOURCE_DIR}/build.xml"
+          "${CMAKE_CURRENT_SOURCE_DIR}/local.properties"
+          "${CMAKE_CURRENT_SOURCE_DIR}/project.properties"
+          "${CMAKE_CURRENT_SOURCE_DIR}/proguard-project.txt"
+  WORKING_DIRECTORY
+          "${CMAKE_CURRENT_BINARY_DIR}")
+
+add_dependencies(NativeRTABMap-ant-configure NativeRTABMap)
+
+#find ant
+find_host_program(ANT_EXECUTABLE
+  NAMES ant
+  DOC   "The ant build tool")
+if(NOT ANT_EXECUTABLE)
+  message(FATAL_ERROR "Can not find ant build tool: ant")
+endif()
+
+add_custom_target(NativeRTABMap-apk-release ALL
+  COMMAND ${ANT_EXECUTABLE}
+          -file "${CMAKE_CURRENT_BINARY_DIR}/build.xml"
+          release)
+add_dependencies(NativeRTABMap-apk-release
+  NativeRTABMap-ant-configure
+  NativeRTABMap)
+
+add_custom_target(NativeRTABMap-apk-debug ALL
+  COMMAND ${ANT_EXECUTABLE}
+          -file "${CMAKE_CURRENT_BINARY_DIR}/build.xml"
+          debug)
+add_dependencies(NativeRTABMap-apk-debug
+#  NativeRTABMap-apk-release
+  NativeRTABMap-ant-configure
+  NativeRTABMap)

rtabmap/app/android/CameraAvailability.h.in

@@ -0,0 +1,39 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * 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.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 UNIVERTY DE SHERBROOKE 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 CAMERAAVAILABILITY_H_
+#define CAMERAAVAILABILITY_H_
+
+// This is auto-generated!
+
+@TANGO@#define RTABMAP_TANGO
+@ARCORE@#define RTABMAP_ARCORE
+@ARENGINE@#define RTABMAP_ARENGINE
+
+
+#endif /* CAMERAAVAILABILITY_H_ */
+

rtabmap/app/android/ant.properties.in

@@ -0,0 +1,13 @@
+builddir=@CMAKE_CURRENT_BINARY_DIR@
+srcdir=@CMAKE_CURRENT_SOURCE_DIR@
+android.abi=@ANDROID_ABI@
+
+source.dir=${srcdir}/src
+gen.dir=${builddir}/gen
+out.dir=${builddir}/bin
+asset.dir=${builddir}/assets
+resource.absolute.dir=${srcdir}/res
+
+jar.libs.dir=${builddir}/libs
+external.libs.dir=${builddir}/libs
+native.libs.dir=${builddir}/libs

rtabmap/app/android/custom_rules.xml

@@ -0,0 +1,13 @@
+<project>
+    <target name="-pre-build">
+		<copy todir="${jar.libs.dir}">
+			<fileset dir="${srcdir}/libs" includes="**/*.jar" excludes="**/*sources.jar, **/*javadoc.jar" />
+		</copy>
+		<copy todir="${native.libs.dir}/${android.abi}">
+			<fileset dir="${srcdir}/jni/third-party/lib" includes="*.so"/>
+		</copy>
+		<copy todir="${native.libs.dir}">
+			<fileset dir="${srcdir}/jni/third-party/lib" includes="${android.abi}/*.so"/>
+		</copy>
+	</target>
+</project>

rtabmap/app/android/info.txt.in

@@ -0,0 +1,7 @@
+<h3>Real-Time Appearance-Based Mapping</h3>
+Version @RTABMAP_VERSION@<br>
+Author: Mathieu Labb&eacute;<br>
+Copyright 2016-2020<br>
+IntRoLab - Universit&eacute; de Sherbrooke<br>
+<b>http://introlab.github.io/rtabmap</b><br><br>
+

rtabmap/app/android/jni/.gitignore

@@ -0,0 +1 @@
+CameraAvailability.h

rtabmap/app/android/jni/CMakeLists.txt

@@ -0,0 +1,171 @@
+
+SET(INCLUDE_DIRS
+    ${CMAKE_CURRENT_SOURCE_DIR}
+    ${CMAKE_CURRENT_SOURCE_DIR}/tango-gl/include
+    ${CMAKE_CURRENT_SOURCE_DIR}/third-party/include
+	  ${PROJECT_BINARY_DIR}/corelib/include
+	  ${PROJECT_SOURCE_DIR}/corelib/include
+	  ${PROJECT_SOURCE_DIR}/utilite/include
+    ${CMAKE_CURRENT_BINARY_DIR}
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+    "${ANDROID_NDK}/platforms/android-${ANDROID_NATIVE_API_LEVEL}/arch-${ANDROID_ARCH_NAME}/usr/include"
+)
+
+SET(LIBRARIES
+	${OpenCV_LIBRARIES} 
+	${PCL_LIBRARIES}
+)
+
+set(sources
+  jni_interface.cpp
+  CameraMobile.cpp
+  RTABMapApp.cpp
+  scene.cpp
+  point_cloud_drawable.cpp
+  graph_drawable.cpp
+  background_renderer.cc
+  text_drawable.cpp
+  quad_color.cpp
+  tango-gl/bounding_box.cpp 
+  tango-gl/axis.cpp 
+  tango-gl/camera.cpp 
+  tango-gl/circle.cpp 
+  tango-gl/conversions.cpp 
+  tango-gl/drawable_object.cpp 
+  tango-gl/frustum.cpp 
+  tango-gl/gesture_camera.cpp 
+  tango-gl/grid.cpp 
+  tango-gl/line.cpp 
+  tango-gl/mesh.cpp 
+  tango-gl/shaders.cpp 
+  tango-gl/trace.cpp 
+  tango-gl/transform.cpp 
+  tango-gl/util.cpp
+)
+
+IF(OPENMP_FOUND)
+file(COPY ${OpenMP_CXX_LIBRARIES}
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+ENDIF(OPENMP_FOUND)
+
+IF(Tango_FOUND)
+
+  SET(sources
+    ${sources}
+    CameraTango.cpp
+  )
+  SET(INCLUDE_DIRS
+    ${INCLUDE_DIRS}
+    ${Tango_INCLUDE_DIRS}
+  )
+  SET(LIBRARIES
+    ${LIBRARIES}
+    ${Tango_LIBRARIES}
+  )
+  
+  file(COPY ${Tango_support_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME}) 
+ENDIF(Tango_FOUND)
+
+IF(ARCore_FOUND)
+
+  SET(sources
+    ${sources}
+    CameraARCore.cpp
+  )
+  SET(INCLUDE_DIRS
+    ${INCLUDE_DIRS}
+    ${ARCore_INCLUDE_DIRS}
+  )
+  SET(LIBRARIES
+    ${LIBRARIES}
+    ${ARCore_LIBRARIES}
+  )
+ 
+  file(COPY ${ARCore_c_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+  file(COPY ${ARCore_jni_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+    
+ENDIF(ARCore_FOUND)
+
+IF(AREngine_FOUND)
+
+  SET(sources
+    ${sources}
+    CameraAREngine.cpp
+  )
+  SET(INCLUDE_DIRS
+    ${INCLUDE_DIRS}
+    ${AREngine_INCLUDE_DIRS}
+  )
+  SET(LIBRARIES
+    ${LIBRARIES}
+    ${AREngine_LIBRARIES}
+    camera2ndk
+    mediandk
+  )
+ 
+  file(COPY ${AREngine_impl_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+  file(COPY ${AREngine_jni_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+  file(COPY ${AREngine_ndk_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+    
+ENDIF(AREngine_FOUND)
+
+add_definitions(${PCL_DEFINITIONS})
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+####################################
+# Generate resources files
+####################################
+SET(RESOURCES
+    ${CMAKE_CURRENT_SOURCE_DIR}/resources/text_atlas.png
+)
+
+foreach(arg ${RESOURCES})
+   get_filename_component(filename ${arg} NAME)
+   string(REPLACE "." "_" output ${filename})
+   set(RESOURCES_HEADERS "${RESOURCES_HEADERS}" "${CMAKE_CURRENT_BINARY_DIR}/${output}.h")
+endforeach(arg ${RESOURCES})
+
+find_host_program(RTABMAP_RES_TOOL rtabmap-res_tool PATHS ${CMAKE_RUNTIME_OUTPUT_DIRECTORY})
+IF(NOT RTABMAP_RES_TOOL)
+  MESSAGE( FATAL_ERROR "RTABMAP_RES_TOOL is not defined (it is the path to \"rtabmap-res_tool\" application created by a non-Android build)." )
+ENDIF(NOT RTABMAP_RES_TOOL)
+
+ADD_CUSTOM_COMMAND(
+      OUTPUT ${RESOURCES_HEADERS}
+      COMMAND ${RTABMAP_RES_TOOL} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES}
+      COMMENT "[Creating resources]"
+      DEPENDS ${RESOURCES}
+)
+####################################
+# Generate resources files END
+####################################
+
+add_library(NativeRTABMap SHARED ${sources} ${RESOURCES_HEADERS})        
+target_link_libraries(NativeRTABMap ${LIBRARIES}
+  android
+  log
+  GLESv2
+  rtabmap_core
+  rtabmap_utilite
+)
+
+# see ant.properties.in
+set_target_properties(NativeRTABMap PROPERTIES
+         LIBRARY_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME}"
+         LIBRARY_OUTPUT_DIRECTORY_DEBUG "${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME}"
+         LIBRARY_OUTPUT_DIRECTORY_RELEASE "${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME}")
+         
+IF(ANDROID_NATIVE_API_LEVEL GREATER 22)
+add_custom_command(TARGET NativeRTABMap POST_BUILD
+    COMMAND "${ANDROID_TOOLCHAIN_PREFIX}strip" -g -S -d --strip-debug --verbose
+    "${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME}/libNativeRTABMap.so"
+    COMMENT "Strip debug symbols done on final binary.")
+ENDIF(ANDROID_NATIVE_API_LEVEL GREATER 22)

rtabmap/app/android/jni/CameraARCore.cpp

@@ -0,0 +1,523 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * 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.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+*/
+
+#include "CameraARCore.h"
+#include "util.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/util2d.h"
+
+namespace rtabmap {
+
+//////////////////////////////
+// CameraARCore
+//////////////////////////////
+CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, float upstreamRelocalizationAccThr):
+	CameraMobile(upstreamRelocalizationAccThr),
+	env_(env),
+	context_(context),
+	activity_(activity),
+	arInstallRequested_(false),
+	depthFromMotion_(depthFromMotion)
+{
+}
+
+CameraARCore::~CameraARCore() {
+	// Disconnect ARCore service
+	close();
+}
+
+
+struct CameraConfig {
+  int32_t width = 0;
+  int32_t height = 0;
+  std::string config_label;
+  ArCameraConfig* config = nullptr;
+};
+
+void getCameraConfigLowestAndHighestResolutions(
+		std::vector<CameraConfig> & camera_configs,
+    CameraConfig** lowest_resolution_config,
+    CameraConfig** highest_resolution_config) {
+  if (camera_configs.empty()) {
+    return;
+  }
+
+  int low_resolution_config_idx = 0;
+  int high_resolution_config_idx = 0;
+  int32_t smallest_height = camera_configs[0].height;
+  int32_t largest_height = camera_configs[0].height;
+
+  for (int i = 1; i < camera_configs.size(); ++i) {
+    int32_t image_height = camera_configs[i].height;
+    if (image_height < smallest_height) {
+      smallest_height = image_height;
+      low_resolution_config_idx = i;
+    } else if (image_height > largest_height) {
+      largest_height = image_height;
+      high_resolution_config_idx = i;
+    }
+  }
+
+  if (low_resolution_config_idx == high_resolution_config_idx) {
+    *lowest_resolution_config = &camera_configs[low_resolution_config_idx];
+  } else {
+    *lowest_resolution_config = &camera_configs[low_resolution_config_idx];
+    *highest_resolution_config = &camera_configs[high_resolution_config_idx];
+  }
+}
+
+void copyCameraConfig(
+    const ArSession* ar_session, const ArCameraConfigList* all_configs,
+    int index, int num_configs, CameraConfig* camera_config) {
+  if (camera_config != nullptr && index >= 0 && index < num_configs) {
+    ArCameraConfig_create(ar_session, &camera_config->config);
+    ArCameraConfigList_getItem(ar_session, all_configs, index,
+                               camera_config->config);
+    ArCameraConfig_getImageDimensions(ar_session, camera_config->config,
+                                      &camera_config->width,
+                                      &camera_config->height);
+    camera_config->config_label = "(" + std::to_string(camera_config->width) +
+                                  "x" + std::to_string(camera_config->height) +
+                                  ")";
+  }
+}
+
+void destroyCameraConfigs(std::vector<CameraConfig> & camera_configs) {
+  for (int i = 0; i < camera_configs.size(); ++i) {
+    if (camera_configs[i].config != nullptr) {
+      ArCameraConfig_destroy(camera_configs[i].config);
+    }
+  }
+}
+
+std::string CameraARCore::getSerial() const
+{
+	return "ARCore";
+}
+
+bool CameraARCore::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	close();
+
+	CameraMobile::init(calibrationFolder, cameraName);
+
+	UScopeMutex lock(arSessionMutex_);
+
+	ArInstallStatus install_status;
+	// If install was not yet requested, that means that we are resuming the
+	// activity first time because of explicit user interaction (such as
+	// launching the application)
+	bool user_requested_install = !arInstallRequested_;
+
+	// === ATTENTION!  ATTENTION!  ATTENTION! ===
+	// This method can and will fail in user-facing situations.  Your
+	// application must handle these cases at least somewhat gracefully.  See
+	// HelloAR Java sample code for reasonable behavior.
+	ArCoreApk_requestInstall(env_, activity_, user_requested_install, &install_status);
+
+	switch (install_status)
+	{
+		case AR_INSTALL_STATUS_INSTALLED:
+			break;
+		case AR_INSTALL_STATUS_INSTALL_REQUESTED:
+			arInstallRequested_ = true;
+			return false;
+	}
+
+	// === ATTENTION!  ATTENTION!  ATTENTION! ===
+	// This method can and will fail in user-facing situations.  Your
+	// application must handle these cases at least somewhat gracefully.  See
+	// HelloAR Java sample code for reasonable behavior.
+	UASSERT(ArSession_create(env_, context_, &arSession_) == AR_SUCCESS);
+	UASSERT(arSession_);
+
+	int32_t is_depth_supported = 0;
+	ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
+
+	ArConfig_create(arSession_, &arConfig_);
+	UASSERT(arConfig_);
+
+	if (is_depth_supported!=0) {
+	  ArConfig_setDepthMode(arSession_, arConfig_, AR_DEPTH_MODE_AUTOMATIC);
+	} else {
+	  ArConfig_setDepthMode(arSession_, arConfig_, AR_DEPTH_MODE_DISABLED);
+	}
+
+	ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_FIXED);
+	UASSERT(ArSession_configure(arSession_, arConfig_) == AR_SUCCESS);
+
+	ArFrame_create(arSession_, &arFrame_);
+	UASSERT(arFrame_);
+
+	ArCameraIntrinsics_create(arSession_, &arCameraIntrinsics_);
+	UASSERT(arCameraIntrinsics_);
+
+	ArPose_create(arSession_, nullptr, &arPose_);
+	UASSERT(arPose_);
+
+	ArCameraConfigList* all_camera_configs = nullptr;
+	int32_t num_configs = 0;
+	ArCameraConfigList_create(arSession_, &all_camera_configs);
+	// Create filter first to get both 30 and 60 fps.
+	ArCameraConfigFilter* camera_config_filter = nullptr;
+	ArCameraConfigFilter_create(arSession_, &camera_config_filter);
+	ArCameraConfigFilter_setTargetFps(arSession_, camera_config_filter, AR_CAMERA_CONFIG_TARGET_FPS_30 | AR_CAMERA_CONFIG_TARGET_FPS_60);
+	ArSession_getSupportedCameraConfigsWithFilter(arSession_, camera_config_filter, all_camera_configs);
+	ArCameraConfigList_getSize(arSession_, all_camera_configs, &num_configs);
+
+	if (num_configs < 1) {
+		UERROR("No camera config found");
+		close();
+		return false;
+	}
+
+	std::vector<CameraConfig> camera_configs;
+	CameraConfig* cpu_low_resolution_camera_config_ptr = nullptr;
+	CameraConfig* cpu_high_resolution_camera_config_ptr = nullptr;
+	camera_configs.resize(num_configs);
+	for (int i = 0; i < num_configs; ++i) {
+		copyCameraConfig(arSession_, all_camera_configs, i, num_configs,
+						 &camera_configs[i]);
+	}
+	// Determine the highest and lowest CPU resolutions.
+	cpu_low_resolution_camera_config_ptr = nullptr;
+	cpu_high_resolution_camera_config_ptr = nullptr;
+	getCameraConfigLowestAndHighestResolutions(
+			camera_configs,
+			&cpu_low_resolution_camera_config_ptr,
+			&cpu_high_resolution_camera_config_ptr);
+
+	// Cleanup the list obtained as it is safe to destroy the list as camera
+	// config instances were explicitly created and copied. Refer to the
+	// previous comment.
+	ArCameraConfigList_destroy(all_camera_configs);
+	ArSession_setCameraConfig(arSession_, cpu_low_resolution_camera_config_ptr->config);
+
+	/// Sets the behavior of @ref ArSession_update(). See
+	/// ::ArUpdateMode for available options.
+	ArConfig_setUpdateMode(arSession_, arConfig_, AR_UPDATE_MODE_BLOCKING);
+
+	deviceTColorCamera_ = opticalRotation;
+
+	if (ArSession_resume(arSession_) != ArStatus::AR_SUCCESS)
+	{
+		UERROR("Cannot resume camera!");
+		// In a rare case (such as another camera app launching) the camera may be
+		// given to a different app and so may not be available to this app. Handle
+		// this properly and recreate the session at the next iteration.
+		close();
+		return false;
+	}
+
+	return true;
+}
+
+void CameraARCore::close()
+{
+	UScopeMutex lock(arSessionMutex_);
+	if(arSession_!= nullptr)
+	{
+		ArSession_destroy(arSession_);
+	}
+	arSession_ = nullptr;
+
+	if(arConfig_!= nullptr)
+	{
+		ArConfig_destroy(arConfig_);
+	}
+	arConfig_ = nullptr;
+
+	if (arFrame_ != nullptr)
+	{
+		ArFrame_destroy(arFrame_);
+	}
+	arFrame_ = nullptr;
+
+	if (arCameraIntrinsics_ != nullptr)
+	{
+		ArCameraIntrinsics_destroy(arCameraIntrinsics_);
+	}
+	arCameraIntrinsics_ = nullptr;
+
+	if (arPose_ != nullptr)
+	{
+		ArPose_destroy(arPose_);
+	}
+	arPose_ = nullptr;
+
+	CameraMobile::close();
+}
+
+void CameraARCore::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height)
+{
+	CameraMobile::setScreenRotationAndSize(colorCameraToDisplayRotation, width, height);
+	if(arSession_)
+	{
+		int ret = static_cast<int>(colorCameraToDisplayRotation) + 1; // remove 90deg camera rotation
+		  if (ret > 3) {
+		    ret -= 4;
+		  }
+
+		ArSession_setDisplayGeometry(arSession_, ret, width, height);
+	}
+}
+
+SensorData CameraARCore::updateDataOnRender(Transform & pose)
+{
+	UScopeMutex lock(arSessionMutex_);
+	//LOGI("Capturing image...");
+
+	pose.setNull();
+	SensorData data;
+	if(!arSession_)
+	{
+		return data;
+	}
+
+	if(textureId_ == 0)
+	{
+		glGenTextures(1, &textureId_);
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	}
+	if(textureId_!=0)
+		ArSession_setCameraTextureName(arSession_, textureId_);
+
+	// Update session to get current frame and render camera background.
+	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
+		LOGE("CameraARCore::captureImage() ArSession_update error");
+		return data;
+	}
+
+	// If display rotation changed (also includes view size change), we need to
+	// re-query the uv coordinates for the on-screen portion of the camera image.
+	int32_t geometry_changed = 0;
+	ArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
+	if (geometry_changed != 0 || !uvs_initialized_) {
+		ArFrame_transformCoordinates2d(
+				arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
+				BackgroundRenderer::kNumVertices, BackgroundRenderer_kVerticesDevice, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
+				transformed_uvs_);
+		UASSERT(transformed_uvs_);
+		uvs_initialized_ = true;
+	}
+
+	ArCamera* ar_camera;
+	ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
+
+	ArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
+	ArCamera_getProjectionMatrix(arSession_, ar_camera,
+							   /*near=*/0.1f, /*far=*/100.f,
+							   glm::value_ptr(projectionMatrix_));
+
+	ArTrackingState camera_tracking_state;
+	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
+
+	CameraModel model;
+	if(camera_tracking_state == AR_TRACKING_STATE_TRACKING)
+	{
+		// pose in OpenGL coordinates
+		float pose_raw[7];
+		ArCamera_getPose(arSession_, ar_camera, arPose_);
+		ArPose_getPoseRaw(arSession_, arPose_, pose_raw);
+		Transform poseArCore = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
+		pose = rtabmap::rtabmap_world_T_opengl_world * poseArCore * rtabmap::opengl_world_T_rtabmap_world;
+
+		if(pose.isNull())
+		{
+			LOGE("CameraARCore: Pose is null");
+			return data;
+		}
+
+		// Get calibration parameters
+		float fx,fy, cx, cy;
+		int32_t width, height;
+		ArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_);
+		ArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
+		ArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
+		ArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &width, &height);
+#ifndef DISABLE_LOG
+		LOGI("%f %f %f %f %d %d", fx, fy, cx, cy, width, height);
+#endif
+
+		if(fx > 0 && fy > 0 && width > 0 && height > 0 && cx > 0 && cy > 0)
+		{
+			model = CameraModel(fx, fy, cx, cy, deviceTColorCamera_, 0, cv::Size(width, height));
+
+			ArPointCloud * pointCloud = nullptr;
+			ArFrame_acquirePointCloud(arSession_, arFrame_, &pointCloud);
+
+			int32_t is_depth_supported = 0;
+			ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
+
+			ArImage * image = nullptr;
+			ArStatus status = ArFrame_acquireCameraImage(arSession_, arFrame_, &image);
+			if(status == AR_SUCCESS)
+			{
+				if(is_depth_supported)
+				{
+					LOGD("Acquire depth image!");
+					ArImage * depthImage = nullptr;
+					ArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
+
+					ArImageFormat format;
+					ArImage_getFormat(arSession_, depthImage, &format);
+					if(format == AR_IMAGE_FORMAT_DEPTH16)
+					{
+						LOGD("Depth format detected!");
+						int planeCount;
+						ArImage_getNumberOfPlanes(arSession_, depthImage, &planeCount);
+						LOGD("planeCount=%d", planeCount);
+						UASSERT_MSG(planeCount == 1, uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
+						const uint8_t *data = nullptr;
+						int len = 0;
+						int stride;
+						int depth_width;
+						int depth_height;
+						ArImage_getWidth(arSession_, depthImage, &depth_width);
+						ArImage_getHeight(arSession_, depthImage, &depth_height);
+						ArImage_getPlaneRowStride(arSession_, depthImage, 0, &stride);
+						ArImage_getPlaneData(arSession_, depthImage, 0, &data, &len);
+
+						LOGD("width=%d, height=%d, bytes=%d stride=%d", depth_width, depth_height, len, stride);
+
+						cv::Mat occlusionImage = cv::Mat(depth_height, depth_width, CV_16UC1, (void*)data).clone();
+
+						float scaleX = (float)depth_width / (float)width;
+						float scaleY = (float)depth_height / (float)height;
+						CameraModel occlusionModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(depth_width, depth_height));
+						this->setOcclusionImage(occlusionImage, occlusionModel);
+					}
+					ArImage_release(depthImage);
+				}
+
+				int64_t timestamp_ns;
+				ArImageFormat format;
+				ArImage_getTimestamp(arSession_, image, &timestamp_ns);
+				ArImage_getFormat(arSession_, image, &format);
+				if(format == AR_IMAGE_FORMAT_YUV_420_888)
+				{
+#ifndef DISABLE_LOG
+					int32_t num_planes;
+					ArImage_getNumberOfPlanes(arSession_, image, &num_planes);
+					for(int i=0;i<num_planes; ++i)
+					{
+						int32_t pixel_stride;
+						int32_t row_stride;
+						ArImage_getPlanePixelStride(arSession_, image, i, &pixel_stride);
+						ArImage_getPlaneRowStride(arSession_, image, i, &row_stride);
+						LOGI("Plane %d/%d: pixel stride=%d, row stride=%d", i+1, num_planes, pixel_stride, row_stride);
+					}
+#endif
+					const uint8_t * plane_data;
+					const uint8_t * plane_uv_data;
+					int32_t data_length;
+					ArImage_getPlaneData(arSession_, image, 0, &plane_data, &data_length);
+					int32_t uv_data_length;
+					ArImage_getPlaneData(arSession_, image, 2, &plane_uv_data, &uv_data_length);
+
+					if(plane_data != nullptr && data_length == height*width)
+					{
+						double stamp = double(timestamp_ns)/10e8;
+#ifndef DISABLE_LOG
+						LOGI("data_length=%d stamp=%f", data_length, stamp);
+#endif
+						cv::Mat rgb;
+						if((long)plane_uv_data-(long)plane_data != data_length)
+						{
+							// The uv-plane is not concatenated to y plane in memory, so concatenate them
+							cv::Mat yuv(height+height/2, width, CV_8UC1);
+							memcpy(yuv.data, plane_data, data_length);
+							memcpy(yuv.data+data_length, plane_uv_data, height/2*width);
+							cv::cvtColor(yuv, rgb, cv::COLOR_YUV2BGR_NV21);
+						}
+						else
+						{
+							cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)plane_data), rgb, cv::COLOR_YUV2BGR_NV21);
+						}
+
+						std::vector<cv::KeyPoint> kpts;
+						std::vector<cv::Point3f> kpts3;
+						LaserScan scan;
+						if(pointCloud)
+						{
+							int32_t points = 0;
+							ArPointCloud_getNumberOfPoints(arSession_, pointCloud, &points);
+							const float * pointCloudData = 0;
+							ArPointCloud_getData(arSession_, pointCloud, &pointCloudData);
+#ifndef DISABLE_LOG
+							LOGI("pointCloudData=%d size=%d", pointCloudData?1:0, points);
+#endif
+							if(pointCloudData && points>0)
+							{
+								cv::Mat pointCloudDataMat(1, points, CV_32FC4, (void *)pointCloudData);
+								scan = scanFromPointCloudData(pointCloudDataMat, pose, model, rgb, &kpts, &kpts3);
+#ifndef DISABLE_LOG
+								LOGI("valid scan points = %d", scan.size());
+#endif
+							}
+						}
+						else
+						{
+							LOGI("pointCloud empty");
+						}
+
+						data = SensorData(scan, rgb, depthFromMotion_?getOcclusionImage():cv::Mat(), model, 0, stamp);
+						data.setFeatures(kpts,  kpts3, cv::Mat());
+
+						if(!pose.isNull())
+						{
+							this->poseReceived(pose, stamp);
+						}
+					}
+				}
+				else
+				{
+					LOGE("CameraARCore: cannot convert image format %d", format);
+				}
+			}
+			else
+			{
+				LOGE("CameraARCore: failed to get rgb image (status=%d)", (int)status);
+			}
+
+			ArImage_release(image);
+			ArPointCloud_release(pointCloud);
+		}
+	}
+
+	ArCamera_release(ar_camera);
+
+	return data;
+}
+
+} /* namespace rtabmap */

rtabmap/app/android/jni/CameraARCore.h

@@ -0,0 +1,84 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * 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.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS 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 CAMERAARCORE_H_
+#define CAMERAARCORE_H_
+
+#include "CameraMobile.h"
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/GeodeticCoords.h>
+#include <rtabmap/utilite/UMutex.h>
+#include <rtabmap/utilite/USemaphore.h>
+#include <rtabmap/utilite/UEventsSender.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <boost/thread/mutex.hpp>
+#include <background_renderer.h>
+
+#include <arcore_c_api.h>
+#include <camera/NdkCameraDevice.h>
+#include <camera/NdkCameraManager.h>
+#include <media/NdkImageReader.h>
+#include <android/native_window.h>
+
+namespace rtabmap {
+
+class CameraARCore : public CameraMobile {
+public:
+	CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, float upstreamRelocalizationAccThr = 0.0f);
+	virtual ~CameraARCore();
+
+	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual void close(); // close ARCore connection
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData updateDataOnRender(Transform & pose); // should be called in opengl thread
+
+private:
+	rtabmap::Transform getPoseAtTimestamp(double timestamp);
+
+private:
+	void * env_;
+	void * context_;
+	void * activity_;
+	ArSession* arSession_ = nullptr;
+	ArConfig* arConfig_ = nullptr;
+	ArFrame* arFrame_ = nullptr;
+	ArCameraIntrinsics *arCameraIntrinsics_ = nullptr;
+	ArPose * arPose_ = nullptr;
+	bool arInstallRequested_;
+	UMutex arSessionMutex_;
+
+	bool depthFromMotion_;
+};
+
+} /* namespace rtabmap */
+#endif /* CAMERAARCORE_H_ */

rtabmap/app/android/jni/CameraAREngine.cpp

@@ -0,0 +1,355 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * 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.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+*/
+
+#include "CameraAREngine.h"
+#include "util.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/util2d.h"
+
+#include <media/NdkImage.h>
+
+namespace rtabmap {
+
+
+//////////////////////////////
+// CameraAREngine
+//////////////////////////////
+CameraAREngine::CameraAREngine(void* env, void* context, void* activity, float upstreamRelocalizationAccThr):
+	CameraMobile(upstreamRelocalizationAccThr),
+	env_(env),
+	context_(context),
+	activity_(activity),
+	arInstallRequested_(false)
+{
+	glGenTextures(1, &textureId_);
+}
+
+CameraAREngine::~CameraAREngine() {
+	// Disconnect ARCore service
+	close();
+
+	glDeleteTextures(1, &textureId_);
+}
+
+std::string CameraAREngine::getSerial() const
+{
+	return "AREngine";
+}
+
+bool CameraAREngine::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	close();
+
+	CameraMobile::init(calibrationFolder, cameraName);
+
+	UScopeMutex lock(arSessionMutex_);
+
+	HwArInstallStatus install_status;
+	// If install was not yet requested, that means that we are resuming the
+	// activity first time because of explicit user interaction (such as
+	// launching the application)
+	bool user_requested_install = !arInstallRequested_;
+
+	// === ATTENTION!  ATTENTION!  ATTENTION! ===
+	// This method can and will fail in user-facing situations.  Your
+	// application must handle these cases at least somewhat gracefully.  See
+	// HelloAR Java sample code for reasonable behavior.
+	HwArEnginesApk_requestInstall(env_, activity_, user_requested_install, &install_status);
+
+	switch (install_status)
+	{
+		case HWAR_INSTALL_STATUS_INSTALLED:
+			break;
+		case HWAR_INSTALL_STATUS_INSTALL_REQUESTED:
+			arInstallRequested_ = true;
+			return false;
+	}
+
+	// === ATTENTION!  ATTENTION!  ATTENTION! ===
+	// This method can and will fail in user-facing situations.  Your
+	// application must handle these cases at least somewhat gracefully.  See
+	// HelloAR Java sample code for reasonable behavior.
+	UASSERT(HwArSession_create(env_, context_, &arSession_) == HWAR_SUCCESS);
+	UASSERT(arSession_);
+
+	HwArConfig_create(arSession_, &arConfig_);
+	UASSERT(arConfig_);
+
+	HwArConfig_setFocusMode(arSession_, arConfig_, HWAR_FOCUS_MODE_FIXED);
+	UASSERT(HwArSession_configure(arSession_, arConfig_) == HWAR_SUCCESS);
+
+	HwArFrame_create(arSession_, &arFrame_);
+	UASSERT(arFrame_);
+
+	HwArCameraIntrinsics_create(arSession_, &arCameraIntrinsics_); // May fail?!
+	//UASSERT(arCameraIntrinsics_);
+
+	HwArPose_create(arSession_, nullptr, &arPose_);
+	UASSERT(arPose_);
+
+	/// Sets the behavior of @ref ArSession_update(). See
+	/// ::ArUpdateMode for available options.
+	HwArConfig_setUpdateMode(arSession_, arConfig_, HWAR_UPDATE_MODE_BLOCKING);
+
+	deviceTColorCamera_ = opticalRotation;
+
+	if (HwArSession_resume(arSession_) != HWAR_SUCCESS)
+	{
+		UERROR("Cannot resume camera!");
+		// In a rare case (such as another camera app launching) the camera may be
+		// given to a different app and so may not be available to this app. Handle
+		// this properly and recreate the session at the next iteration.
+		close();
+		return false;
+	}
+
+	return true;
+}
+
+void CameraAREngine::close()
+{
+	UScopeMutex lock(arSessionMutex_);
+	if (arCameraIntrinsics_ != nullptr)
+	{
+		HwArCameraIntrinsics_destroy(arSession_, arCameraIntrinsics_);
+	}
+	arCameraIntrinsics_ = nullptr;
+
+	if(arSession_!= nullptr)
+	{
+		HwArSession_destroy(arSession_);
+	}
+	arSession_ = nullptr;
+
+	if(arConfig_!= nullptr)
+	{
+		HwArConfig_destroy(arConfig_);
+	}
+	arConfig_ = nullptr;
+
+	if (arFrame_ != nullptr)
+	{
+		HwArFrame_destroy(arFrame_);
+	}
+	arFrame_ = nullptr;
+
+	if (arPose_ != nullptr)
+	{
+		HwArPose_destroy(arPose_);
+	}
+	arPose_ = nullptr;
+
+	CameraMobile::close();
+}
+
+void CameraAREngine::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height)
+{
+	CameraMobile::setScreenRotationAndSize(colorCameraToDisplayRotation, width, height);
+	if(arSession_)
+	{
+		int ret = static_cast<int>(colorCameraToDisplayRotation) + 1; // remove 90deg camera rotation
+		  if (ret > 3) {
+		    ret -= 4;
+		  }
+
+		HwArSession_setDisplayGeometry(arSession_, ret, width, height);
+	}
+}
+
+SensorData CameraAREngine::updateDataOnRender(Transform & pose)
+{
+	UScopeMutex lock(arSessionMutex_);
+	//LOGI("Capturing image...");
+
+	pose.setNull();
+	SensorData data;
+	if(!arSession_)
+	{
+		return data;
+	}
+
+	if(textureId_ == 0)
+	{
+		glGenTextures(1, &textureId_);
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	}
+	if(textureId_!=0)
+		HwArSession_setCameraTextureName(arSession_, textureId_);
+
+	// Update session to get current frame and render camera background.
+	if (HwArSession_update(arSession_, arFrame_) != HWAR_SUCCESS) {
+		LOGE("CameraAREngine::captureImage() ArSession_update error");
+		return data;
+	}
+
+	// If display rotation changed (also includes view size change), we need to
+	// re-query the uv coordinates for the on-screen portion of the camera image.
+	int32_t geometry_changed = 0;
+	HwArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
+	if (geometry_changed != 0 || !uvs_initialized_) {
+		HwArFrame_transformDisplayUvCoords(
+				arSession_, arFrame_,
+				BackgroundRenderer::kNumVertices*2, BackgroundRenderer_kVerticesView,
+				transformed_uvs_);
+		UERROR("uv: (%f,%f) (%f,%f) (%f,%f) (%f,%f)", 
+			transformed_uvs_[0], transformed_uvs_[1],
+			transformed_uvs_[2], transformed_uvs_[3],
+			transformed_uvs_[4], transformed_uvs_[5],
+			transformed_uvs_[6], transformed_uvs_[7]);
+		UASSERT(transformed_uvs_);
+		uvs_initialized_ = true;
+	}
+
+	HwArCamera* ar_camera;
+	HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
+
+	HwArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
+	HwArCamera_getProjectionMatrix(arSession_, ar_camera,
+							   /*near=*/0.1f, /*far=*/100.f,
+							   glm::value_ptr(projectionMatrix_));
+
+	HwArTrackingState camera_tracking_state;
+	HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
+
+	if(camera_tracking_state == HWAR_TRACKING_STATE_TRACKING)
+	{
+		// Get calibration parameters
+		// FIXME: Hard-coded as getting intrinsics with the api fails
+		float fx=492.689667,fy=492.606201, cx=323.594849, cy=234.659744;
+		int32_t camWidth=640, camHeight=480;
+		//HwArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_);
+		//HwArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
+		//HwArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
+		//HwArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &camWidth, &camHeight);
+		LOGI("%f %f %f %f %d %d", fx, fy, cx, cy, camWidth, camHeight);
+
+		if(fx > 0 && fy > 0 && camWidth > 0 && camHeight > 0 && cx > 0 && cy > 0)
+		{
+			//ArPointCloud * point_cloud;
+			//ArFrame_acquirePointCloud(ar_session_, ar_frame_, &point_cloud);
+
+			HwArImage * image = nullptr;
+			HwArImage * depthImage = nullptr;
+			HwArStatus statusRgb = HwArFrame_acquireCameraImage(arSession_, arFrame_, &image);
+			HwArStatus statusDepth = HwArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
+			if(statusRgb == HWAR_SUCCESS && statusDepth == HWAR_SUCCESS)
+			{
+				int64_t timestamp_ns;
+				HwArFrame_getTimestamp(arSession_, arFrame_, &timestamp_ns);
+
+				int planeCount;
+				uint8_t *imageData = nullptr;
+				int len = 0;
+				int stride;
+				int width;
+				int height;
+				const AImage* ndkImageRGB;
+				HwArImage_getNdkImage(image, &ndkImageRGB);
+
+				AImage_getNumberOfPlanes(ndkImageRGB, &planeCount);
+				AImage_getWidth(ndkImageRGB, &width);
+				AImage_getHeight(ndkImageRGB, &height);
+				AImage_getPlaneRowStride(ndkImageRGB, 0, &stride);
+				AImage_getPlaneData(ndkImageRGB, 0, &imageData, &len);
+				LOGI("RGB: width=%d, height=%d, bytes=%d stride=%d planeCount=%d", width, height, len, stride, planeCount);
+
+				cv::Mat outputRGB;
+				if(imageData != nullptr && len>0)
+				{
+					cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)imageData), outputRGB, cv::COLOR_YUV2BGR_NV21);
+				}
+
+				//Depth
+				const AImage* ndkImageDepth;
+				HwArImage_getNdkImage(depthImage, &ndkImageDepth);
+				AImage_getNumberOfPlanes(ndkImageDepth, &planeCount);
+				AImage_getWidth(ndkImageDepth, &width);
+				AImage_getHeight(ndkImageDepth, &height);
+				AImage_getPlaneRowStride(ndkImageDepth, 0, &stride);
+				AImage_getPlaneData(ndkImageDepth, 0, &imageData, &len);
+				LOGI("Depth: width=%d, height=%d, bytes=%d stride=%d planeCount=%d", width, height, len, stride, planeCount);
+
+				cv::Mat outputDepth(height, width, CV_16UC1);
+				uint16_t *dataShort = (uint16_t *)imageData;
+				for (int y = 0; y < outputDepth.rows; ++y)
+				{
+					for (int x = 0; x < outputDepth.cols; ++x)
+					{
+						uint16_t depthSample = dataShort[y*outputDepth.cols + x];
+						uint16_t depthRange = (depthSample & 0x1FFF); // first 3 bits are confidence
+						outputDepth.at<uint16_t>(y,x) = depthRange;
+					}
+				}
+
+				if(!outputRGB.empty() && !outputDepth.empty())
+				{
+					double stamp = double(timestamp_ns)/10e8;
+					CameraModel model = CameraModel(fx, fy, cx, cy, deviceTColorCamera_, 0, cv::Size(camWidth, camHeight));
+					data = SensorData(outputRGB, outputDepth, model, 0, stamp);
+
+					// pose in OpenGL coordinates
+					float pose_raw[7];
+					HwArCamera_getPose(arSession_, ar_camera, arPose_);
+					HwArPose_getPoseRaw(arSession_, arPose_, pose_raw);
+					pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
+					if(pose.isNull())
+					{
+						LOGE("CameraAREngine: Pose is null");
+					}
+					else
+					{
+						pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
+						this->poseReceived(pose, stamp);
+					}
+				}
+			}
+			else
+			{
+				LOGE("CameraAREngine: failed to get rgb image (status=%d %d)", (int)statusRgb, (int)statusDepth);
+			}
+
+			HwArImage_release(image);
+			HwArImage_release(depthImage);
+		}
+		else
+		{
+			LOGE("Invalid intrinsics!");
+		}
+	}
+
+	HwArCamera_release(ar_camera);
+	return data;
+
+}
+
+} /* namespace rtabmap */

rtabmap/app/android/jni/CameraAREngine.h

@@ -0,0 +1,79 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * 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.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS 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 CAMERAARENGINE_H_
+#define CAMERAARENGINE_H_
+
+#include "CameraMobile.h"
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/GeodeticCoords.h>
+#include <rtabmap/utilite/UMutex.h>
+#include <rtabmap/utilite/USemaphore.h>
+#include <rtabmap/utilite/UEventsSender.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <boost/thread/mutex.hpp>
+#include <background_renderer.h>
+
+#include <huawei_arengine_interface.h>
+
+namespace rtabmap {
+
+class CameraAREngine : public CameraMobile {
+public:
+	CameraAREngine(void* env, void* context, void* activity, float upstreamRelocalizationAccThr = 0.0f);
+	virtual ~CameraAREngine();
+
+	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual void close(); // close AREngine connection
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData updateDataOnRender(Transform & pose);
+
+private:
+	rtabmap::Transform getPoseAtTimestamp(double timestamp);
+
+private:
+	void * env_;
+	void * context_;
+	void * activity_;
+	HwArSession* arSession_ = nullptr;
+	HwArConfig* arConfig_ = nullptr;
+	HwArFrame* arFrame_ = nullptr;
+	HwArCameraIntrinsics *arCameraIntrinsics_ = nullptr;
+	HwArPose * arPose_ = nullptr;
+	bool arInstallRequested_;
+	UMutex arSessionMutex_;
+
+};
+
+} /* namespace rtabmap */
+#endif /* CAMERAARENGINE_H_ */

rtabmap/app/android/jni/CameraMobile.cpp

@@ -0,0 +1,636 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * 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.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+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.
+*/
+
+#include "CameraMobile.h"
+#include "util.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/util2d.h"
+#include <glm/gtx/transform.hpp>
+
+namespace rtabmap {
+
+#define nullptr 0
+
+//////////////////////////////
+// CameraMobile
+//////////////////////////////
+const rtabmap::Transform CameraMobile::opticalRotation = Transform(
+		0.0f,  0.0f,  1.0f, 0.0f,
+	   -1.0f,  0.0f,  0.0f, 0.0f,
+		0.0f, -1.0f,  0.0f, 0.0f);
+const rtabmap::Transform CameraMobile::opticalRotationInv = Transform(
+		0.0f, -1.0f,  0.0f, 0.0f,
+	    0.0f,  0.0f, -1.0f, 0.0f,
+		1.0f,  0.0f,  0.0f, 0.0f);
+
+CameraMobile::CameraMobile(float upstreamRelocalizationAccThr) :
+		Camera(10),
+		deviceTColorCamera_(Transform::getIdentity()),
+		textureId_(0),
+		uvs_initialized_(false),
+		stampEpochOffset_(0.0),
+		colorCameraToDisplayRotation_(ROTATION_0),
+		originUpdate_(true),
+		upstreamRelocalizationAccThr_(upstreamRelocalizationAccThr),
+		previousAnchorStamp_(0.0),
+		dataGoodTracking_(true)
+{
+}
+
+CameraMobile::~CameraMobile() {
+	// Disconnect camera service
+	close();
+}
+
+bool CameraMobile::init(const std::string &, const std::string &)
+{
+	deviceTColorCamera_ = opticalRotation;
+	// clear semaphore
+	if(dataReady_.value() > 0) {
+		dataReady_.acquire(dataReady_.value());
+	}
+	return true;
+}
+
+void CameraMobile::close()
+{
+	UScopeMutex lock(dataMutex_);
+
+	firstFrame_ = true;
+	lastKnownGPS_ = GPS();
+	lastEnvSensors_.clear();
+	originOffset_ = Transform();
+	originUpdate_ = true;
+	dataPose_ = Transform();
+	data_ = SensorData();
+    dataGoodTracking_ = true;
+    previousAnchorPose_.setNull();
+    previousAnchorLinearVelocity_.clear();
+    previousAnchorStamp_ = 0.0;
+
+    if(textureId_ != 0)
+    {
+        glDeleteTextures(1, &textureId_);
+        textureId_ = 0;
+    }
+    // in case someone is waiting on captureImage()
+    dataReady_.release();
+}
+
+void CameraMobile::resetOrigin(const rtabmap::Transform & offset)
+{
+    manualOriginOffset_ = offset;
+	originUpdate_ = true;
+}
+
+bool CameraMobile::getPose(double epochStamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
+{
+	pose.setNull();
+
+	int maxWaitTimeMs = maxWaitTime * 1000;
+
+	// Interpolate pose
+	if(!poseBuffer_.empty())
+	{
+		poseMutex_.lock();
+		int waitTry = 0;
+		while(maxWaitTimeMs>0 && poseBuffer_.rbegin()->first < epochStamp && waitTry < maxWaitTimeMs)
+		{
+			poseMutex_.unlock();
+			++waitTry;
+			uSleep(1);
+			poseMutex_.lock();
+		}
+		if(poseBuffer_.rbegin()->first < epochStamp)
+		{
+			if(maxWaitTimeMs > 0)
+			{
+				UWARN("Could not find poses to interpolate at time %f after waiting %d ms (latest is %f)...", epochStamp, maxWaitTimeMs, poseBuffer_.rbegin()->first);
+			}
+			else
+			{
+				UWARN("Could not find poses to interpolate at time %f (latest is %f)...", epochStamp, poseBuffer_.rbegin()->first);
+			}
+		}
+		else
+		{
+			std::map<double, Transform>::const_iterator iterB = poseBuffer_.lower_bound(epochStamp);
+			std::map<double, Transform>::const_iterator iterA = iterB;
+			if(iterA != poseBuffer_.begin())
+			{
+				iterA = --iterA;
+			}
+			if(iterB == poseBuffer_.end())
+			{
+				iterB = --iterB;
+			}
+			if(iterA == iterB && epochStamp == iterA->first)
+			{
+				pose = iterA->second;
+			}
+			else if(epochStamp >= iterA->first && epochStamp <= iterB->first)
+			{
+				pose = iterA->second.interpolate((epochStamp-iterA->first) / (iterB->first-iterA->first), iterB->second);
+			}
+			else // stamp < iterA->first
+			{
+				UWARN("Could not find pose data to interpolate at time %f (earliest is %f). Are sensors synchronized?", epochStamp, iterA->first);
+			}
+		}
+		poseMutex_.unlock();
+	}
+	return !pose.isNull();
+}
+
+void CameraMobile::poseReceived(const Transform & pose, double deviceStamp)
+{
+	// Pose reveived is the pose of the device in rtabmap coordinate
+	if(!pose.isNull())
+	{
+		Transform p = pose;
+		
+		if(stampEpochOffset_ == 0.0)
+		{
+			stampEpochOffset_ = UTimer::now() - deviceStamp;
+		}
+
+		if(originUpdate_)
+		{
+			firstFrame_ = true;
+			lastKnownGPS_ = GPS();
+			lastEnvSensors_.clear();
+			dataGoodTracking_ = true;
+			previousAnchorPose_.setNull();
+			previousAnchorLinearVelocity_.clear();
+			previousAnchorStamp_ = 0.0;
+            originOffset_ = manualOriginOffset_.isNull() ? pose.translation().inverse() : manualOriginOffset_;
+			originUpdate_ = false;
+		}
+        
+		double epochStamp = stampEpochOffset_ + deviceStamp;
+		if(!originOffset_.isNull())
+		{
+			// Filter re-localizations from poses received
+			rtabmap::Transform rawPose = originOffset_ * pose.translation(); // remove rotation to keep position in fixed frame
+			// Remove upstream localization corrections by integrating pose from previous frame anchor
+            bool showLog = false;
+            if(upstreamRelocalizationAccThr_>0.0f && !previousAnchorPose_.isNull())
+			{
+				float dt = epochStamp - previousAnchorStamp_;
+                std::vector<float> currentLinearVelocity(3);
+                float dx = rawPose.x()-previousAnchorPose_.x();
+                float dy = rawPose.y()-previousAnchorPose_.y();
+                float dz = rawPose.z()-previousAnchorPose_.z();
+                currentLinearVelocity[0] = dx / dt;
+                currentLinearVelocity[1] = dy / dt;
+                currentLinearVelocity[2] = dz / dt;
+				if(!previousAnchorLinearVelocity_.empty() && uNorm(dx, dy, dz)>0.02)
+				{
+                    float ax = (currentLinearVelocity[0] - previousAnchorLinearVelocity_[0]) / dt;
+                    float ay = (currentLinearVelocity[1] - previousAnchorLinearVelocity_[1]) / dt;
+                    float az = (currentLinearVelocity[2] - previousAnchorLinearVelocity_[2]) / dt;
+					float acceleration = sqrt(ax*ax + ay*ay + az*az);
+					if(acceleration>=upstreamRelocalizationAccThr_)
+					{
+						// Only correct the translation to not lose rotation aligned
+						// with gravity.
+                        
+                        // Use constant motion model to update current pose.
+						rtabmap::Transform offset(previousAnchorLinearVelocity_[0] * dt,
+                                                  previousAnchorLinearVelocity_[1] * dt,
+                                                  previousAnchorLinearVelocity_[2] * dt,
+                                                  0, 0, 0, 1);
+						rtabmap::Transform newRawPose = offset * previousAnchorPose_;
+						currentLinearVelocity = previousAnchorLinearVelocity_;
+						originOffset_.x() += newRawPose.x() - rawPose.x();
+                        originOffset_.y() += newRawPose.y() - rawPose.y();
+                        originOffset_.z() += newRawPose.z() - rawPose.z();
+                        UERROR("Upstream re-localization has been suppressed because of "
+						     "high acceleration detected (%f m/s^2) causing a jump!",
+								acceleration);
+						dataGoodTracking_ = false;
+                        post(new CameraInfoEvent(0, "UpstreamRelocationFiltered", uFormat("%.1f m/s^2", acceleration).c_str()));
+                        showLog = true;
+					}
+				}
+				previousAnchorLinearVelocity_ = currentLinearVelocity;
+			}
+
+            p = originOffset_*pose;
+            previousAnchorPose_ = p;
+            previousAnchorStamp_ = epochStamp;
+            
+            if(upstreamRelocalizationAccThr_>0.0f) {
+                relocalizationDebugBuffer_.insert(std::make_pair(epochStamp, std::make_pair(pose, p)));
+                if(relocalizationDebugBuffer_.size() > 60)
+                {
+                    relocalizationDebugBuffer_.erase(relocalizationDebugBuffer_.begin());
+                }
+                if(showLog) {
+                    std::stringstream stream;
+                    for(auto iter=relocalizationDebugBuffer_.begin(); iter!=relocalizationDebugBuffer_.end(); ++iter)
+                    {
+                        stream << iter->first - relocalizationDebugBuffer_.begin()->first
+                        << " " << iter->second.first.x()
+                        << " " << iter->second.first.y()
+                        << " " << iter->second.first.z()
+                        << " " << iter->second.second.x()
+                        << " " << iter->second.second.y()
+                        << " " << iter->second.second.z() << std::endl;
+                    }
+                    UERROR("timestamp original_xyz corrected_xyz:\n%s", stream.str().c_str());
+                }
+            }
+		}
+
+		{
+			UScopeMutex lock(poseMutex_);
+			poseBuffer_.insert(poseBuffer_.end(), std::make_pair(epochStamp, p));
+			if(poseBuffer_.size() > 1000)
+			{
+				poseBuffer_.erase(poseBuffer_.begin());
+			}
+		}
+
+		// send pose of the camera (with optical rotation)
+		this->post(new PoseEvent(p * deviceTColorCamera_));
+	}
+}
+
+bool CameraMobile::isCalibrated() const
+{
+	return model_.isValidForProjection();
+}
+
+void CameraMobile::setGPS(const GPS & gps)
+{
+	lastKnownGPS_ = gps;
+}
+
+void CameraMobile::addEnvSensor(int type, float value)
+{
+	lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value)));
+}
+
+void CameraMobile::update(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord)
+{
+	UScopeMutex lock(dataMutex_);
+
+    LOGD("CameraMobile::update pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
+
+	bool notify = !data_.isValid();
+    
+	data_ = data;
+    dataPose_ = pose;
+
+    viewMatrix_ = viewMatrix;
+    projectionMatrix_ = projectionMatrix;
+        
+    if(textureId_ == 0)
+    {
+    	glGenTextures(1, &textureId_);
+    }
+
+    if(texCoord)
+    {
+        memcpy(transformed_uvs_, texCoord, 8*sizeof(float));
+        uvs_initialized_ = true;
+    }
+    
+    LOGD("CameraMobile::update textureId_=%d", (int)textureId_);
+
+    if(textureId_ != 0 && texCoord != 0)
+    {
+        cv::Mat rgbImage;
+        cv::cvtColor(data.imageRaw(), rgbImage, cv::COLOR_BGR2RGBA);
+        
+        glBindTexture(GL_TEXTURE_2D, textureId_);
+        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+        
+        glPixelStorei(GL_UNPACK_ALIGNMENT, 4);
+        //glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
+        //glPixelStorei(GL_UNPACK_SKIP_PIXELS, 0);
+        //glPixelStorei(GL_UNPACK_SKIP_ROWS, 0);
+        glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, rgbImage.cols, rgbImage.rows, 0, GL_RGBA, GL_UNSIGNED_BYTE, rgbImage.data);
+
+        GLint error = glGetError();
+        if(error != GL_NO_ERROR)
+        {
+            LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
+            textureId_ = 0;
+            return;
+        }
+    }
+
+    if(data_.isValid())
+    {
+        postUpdate();
+
+        if(notify)
+        {
+            dataReady_.release();
+        }
+    }
+}
+
+void CameraMobile::updateOnRender()
+{
+	UScopeMutex lock(dataMutex_);
+	bool notify = !data_.isValid();
+
+	data_ = updateDataOnRender(dataPose_);
+	if(data_.isValid())
+	{
+		postUpdate();
+
+		if(notify)
+		{
+			dataReady_.release();
+		}
+	}
+}
+
+SensorData CameraMobile::updateDataOnRender(Transform & pose)
+{
+	LOGE("To use CameraMobile::updateOnRender(), CameraMobile::updateDataOnRender() "
+	     "should be overridden by inherited classes. Returning empty data!\n");
+	return SensorData();
+}
+
+void CameraMobile::postUpdate()
+{
+	if(data_.isValid())
+    {
+        // adjust origin
+        if(!originOffset_.isNull())
+        {
+            dataPose_ = originOffset_ * dataPose_;
+            viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
+            occlusionModel_.setLocalTransform(originOffset_ * occlusionModel_.localTransform());
+        }
+        
+        if(lastKnownGPS_.stamp() > 0.0 && data_.stamp()-lastKnownGPS_.stamp()<1.0)
+        {
+            data_.setGPS(lastKnownGPS_);
+        }
+        else if(lastKnownGPS_.stamp()>0.0)
+        {
+            LOGD("GPS too old (current time=%f, gps time = %f)", data_.stamp(), lastKnownGPS_.stamp());
+        }
+        
+        if(lastEnvSensors_.size())
+        {
+            data_.setEnvSensors(lastEnvSensors_);
+            lastEnvSensors_.clear();
+        }
+        
+        
+        // Rotate image depending on the camera orientation
+        if(colorCameraToDisplayRotation_ == ROTATION_90)
+        {
+            UDEBUG("ROTATION_90");
+            cv::Mat rgb, depth, confidence;
+            cv::Mat rgbt;
+            cv::flip(data_.imageRaw(),rgb,1);
+            cv::transpose(rgb,rgbt);
+            rgb = rgbt;
+            cv::Mat deptht;
+            cv::flip(data_.depthRaw(),depth,1);
+            cv::transpose(depth,deptht);
+            depth = deptht;
+            if(!data_.depthConfidenceRaw().empty()) {
+                cv::Mat conft;
+                cv::flip(data_.depthConfidenceRaw(),confidence,1);
+                cv::transpose(confidence,conft);
+                confidence = conft;
+            }
+            CameraModel model = data_.cameraModels()[0];
+            cv::Size sizet(model.imageHeight(), model.imageWidth());
+            model = CameraModel(
+                                model.fy(),
+                                model.fx(),
+                                model.cy(),
+                                model.cx()>0?model.imageWidth()-model.cx():0,
+                                model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
+            model.setImageSize(sizet);
+            data_.setRGBDImage(rgb, depth, confidence, model);
+            
+            std::vector<cv::KeyPoint> keypoints = data_.keypoints();
+            for(size_t i=0; i<keypoints.size(); ++i)
+            {
+                keypoints[i].pt.x = data_.keypoints()[i].pt.y;
+                keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.x;
+            }
+            data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
+        }
+        else if(colorCameraToDisplayRotation_ == ROTATION_180)
+        {
+            UDEBUG("ROTATION_180");
+            cv::Mat rgb, depth, confidence;
+            cv::flip(data_.imageRaw(),rgb,1);
+            cv::flip(rgb,rgb,0);
+            cv::flip(data_.depthOrRightRaw(),depth,1);
+            cv::flip(depth,depth,0);
+            if(!data_.depthConfidenceRaw().empty()) {
+                cv::flip(data_.depthConfidenceRaw(),confidence,1);
+                cv::flip(confidence,confidence,0);
+            }
+            CameraModel model = data_.cameraModels()[0];
+            cv::Size sizet(model.imageWidth(), model.imageHeight());
+            model = CameraModel(
+                                model.fx(),
+                                model.fy(),
+                                model.cx()>0?model.imageWidth()-model.cx():0,
+                                model.cy()>0?model.imageHeight()-model.cy():0,
+                                model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
+            model.setImageSize(sizet);
+            data_.setRGBDImage(rgb, depth, confidence, model);
+            
+            std::vector<cv::KeyPoint> keypoints = data_.keypoints();
+            for(size_t i=0; i<keypoints.size(); ++i)
+            {
+                keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.x;
+                keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.y;
+            }
+            data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
+        }
+        else if(colorCameraToDisplayRotation_ == ROTATION_270)
+        {
+            UDEBUG("ROTATION_270");
+            cv::Mat rgb, depth, confidence;
+            cv::transpose(data_.imageRaw(),rgb);
+            cv::flip(rgb,rgb,1);
+            cv::transpose(data_.depthOrRightRaw(),depth);
+            cv::flip(depth,depth,1);
+            if(!data_.depthConfidenceRaw().empty()) {
+                cv::transpose(data_.depthConfidenceRaw(),confidence);
+                cv::flip(confidence,confidence,1);
+            }
+            CameraModel model = data_.cameraModels()[0];
+            cv::Size sizet(model.imageHeight(), model.imageWidth());
+            model = CameraModel(
+                                model.fy(),
+                                model.fx(),
+                                model.cy()>0?model.imageHeight()-model.cy():0,
+                                model.cx(),
+                                model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
+            model.setImageSize(sizet);
+            data_.setRGBDImage(rgb, depth, confidence, model);
+            
+            std::vector<cv::KeyPoint> keypoints = data_.keypoints();
+            for(size_t i=0; i<keypoints.size(); ++i)
+            {
+                keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.y;
+                keypoints[i].pt.y = data_.keypoints()[i].pt.x;
+            }
+            data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
+        }
+        else
+        {
+            UDEBUG("ROTATION_0");
+        }
+	}
+}
+
+SensorData CameraMobile::captureImage(SensorCaptureInfo * info)
+{
+	SensorData data;
+    bool firstFrame = true;
+    bool dataGoodTracking = true;
+    rtabmap::Transform dataPose;
+	if(dataReady_.acquire(1, 15000))
+	{
+		UScopeMutex lock(dataMutex_);
+		data = data_;
+        dataPose = dataPose_;
+        firstFrame = firstFrame_;
+        dataGoodTracking = dataGoodTracking_;
+        firstFrame_ = false;
+        dataGoodTracking_ = true;
+		data_ = SensorData();
+        dataPose_.setNull();
+	}
+	if(data.isValid())
+	{
+		data.setGroundTruth(Transform());
+		data.setStamp(stampEpochOffset_ + data.stamp());
+
+		if(info)
+		{
+			// linear cov = 0.0001
+			info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.00001);
+			if(!firstFrame)
+			{
+				// angular cov = 0.000001
+                // roll/pitch should be fairly accurate with VIO input
+				info->odomCovariance.at<double>(3,3) *= 0.01; // roll
+				info->odomCovariance.at<double>(4,4) *= 0.01; // pitch
+                if(!dataGoodTracking)
+                {
+                    UERROR("not good tracking!");
+                    // add slightly more error on translation
+                    // 0.001
+                    info->odomCovariance.at<double>(0,0) *= 10; // x
+                    info->odomCovariance.at<double>(1,1) *= 10; // y
+                    info->odomCovariance.at<double>(2,2) *= 10; // z
+                    info->odomCovariance.at<double>(5,5) *= 10; // yaw
+                }
+			}
+			info->odomPose = dataPose;
+		}
+	}
+	else
+	{
+		UWARN("CameraMobile::captureImage() invalid data!");
+	}
+	return data;
+}
+
+LaserScan CameraMobile::scanFromPointCloudData(
+        const cv::Mat & pointCloudData,
+        const Transform & pose,
+        const CameraModel & model,
+        const cv::Mat & rgb,
+        std::vector<cv::KeyPoint> * kpts,
+        std::vector<cv::Point3f> * kpts3D,
+        int kptsSize)
+{
+    if(!pointCloudData.empty())
+    {
+        cv::Mat scanData(1, pointCloudData.cols, CV_32FC4);
+        float * ptr = scanData.ptr<float>();
+        const float * inPtr = pointCloudData.ptr<float>();
+        int ic = pointCloudData.channels();
+        UASSERT(pointCloudData.depth() == CV_32F && ic >= 3);
+        
+        int oi = 0;
+        for(unsigned int i=0;i<pointCloudData.cols; ++i)
+        {
+            cv::Point3f pt(inPtr[i*ic], inPtr[i*ic + 1], inPtr[i*ic + 2]);
+            pt = util3d::transformPoint(pt, pose.inverse()*rtabmap_world_T_opengl_world);
+            ptr[oi*4] = pt.x;
+            ptr[oi*4 + 1] = pt.y;
+            ptr[oi*4 + 2] = pt.z;
+
+            //get color from rgb image
+            cv::Point3f org= pt;
+            pt = util3d::transformPoint(pt, opticalRotationInv);
+            if(pt.z > 0)
+            {
+                int u,v;
+                model.reproject(pt.x, pt.y, pt.z, u, v);
+                unsigned char r=255,g=255,b=255;
+                if(model.inFrame(u, v))
+                {
+                    b=rgb.at<cv::Vec3b>(v,u).val[0];
+                    g=rgb.at<cv::Vec3b>(v,u).val[1];
+                    r=rgb.at<cv::Vec3b>(v,u).val[2];
+                    if(kpts)
+                        kpts->push_back(cv::KeyPoint(u,v,kptsSize));
+                    if(kpts3D)
+                        kpts3D->push_back(org);
+                    
+                    *(int*)&ptr[oi*4 + 3] = int(b) | (int(g) << 8) | (int(r) << 16);
+                    ++oi;
+                }
+            }
+            //confidence
+            //*(int*)&ptr[i*4 + 3] = (int(pointCloudData[i*4 + 3] * 255.0f) << 8) | (int(255) << 16);
+
+        }
+        return LaserScan::backwardCompatibility(scanData.colRange(0, oi), 0, 10, rtabmap::Transform::getIdentity());
+    }
+    return LaserScan();
+}
+
+} /* namespace rtabmap */

rtabmap/app/android/jni/CameraMobile.h

@@ -0,0 +1,169 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * 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.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS 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 CAMERAMOBILE_H_
+#define CAMERAMOBILE_H_
+
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/GeodeticCoords.h>
+#include <rtabmap/utilite/UMutex.h>
+#include <rtabmap/utilite/USemaphore.h>
+#include <rtabmap/utilite/UEventsSender.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <boost/thread/mutex.hpp>
+#include "util.h"
+
+namespace rtabmap {
+
+class CameraInfoEvent: public UEvent
+{
+public:
+	CameraInfoEvent(int type, const std::string & key, const std::string & value) : type_(type), key_(key), value_(value) {}
+	virtual std::string getClassName() const {return "CameraInfoEvent";}
+	int type() const {return type_;}
+	const std::string & key() const {return key_;}
+	const std::string & value() const {return value_;}
+
+private:
+	int type_;
+	std::string key_;
+	std::string value_;
+
+};
+
+class PoseEvent: public UEvent
+{
+public:
+	PoseEvent(const Transform & pose) : pose_(pose) {}
+	virtual std::string getClassName() const {return "PoseEvent";}
+	const Transform & pose() const {return pose_;}
+
+private:
+	Transform pose_;
+};
+
+class CameraMobile : public Camera, public UEventsSender {
+public:
+	static const rtabmap::Transform opticalRotation;
+	static const rtabmap::Transform opticalRotationInv;
+    
+public:
+    static LaserScan scanFromPointCloudData(
+            const cv::Mat & pointCloudData,
+            const Transform & pose,
+            const CameraModel & model,
+            const cv::Mat & rgb,
+            std::vector<cv::KeyPoint> * kpts = 0,
+            std::vector<cv::Point3f> * kpts3D = 0,
+            int kptsSize = 3);
+
+public:
+	CameraMobile(float upstreamRelocalizationAccThr = 0.0f);
+	virtual ~CameraMobile();
+
+	// abstract functions
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual void close(); // inherited classes should call its parent at the end of their close().
+	virtual std::string getSerial() const {return "CameraMobile";}
+
+	// original pose of device in rtabmap frame (without origin offset), stamp of the device (may be not epoch), viewMatrix in opengl frame (without origin offset)
+	void update(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord);
+	void updateOnRender();
+
+	void resetOrigin(const rtabmap::Transform & offset = rtabmap::Transform());
+	virtual bool isCalibrated() const;
+
+	virtual bool odomProvided() const { return true; }
+	virtual bool getPose(double epochStamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06); // Return pose of device in rtabmap frame (with origin offset), stamp should be epoch time
+	// original pose of device in rtabmap frame (without origin offset), stamp of the device (may be not epoch)
+	void poseReceived(const Transform & pose, double deviceStamp);
+	double getStampEpochOffset() const {return stampEpochOffset_;}
+
+	const CameraModel & getCameraModel() const {return model_;}
+	const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;}
+	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
+	void setGPS(const GPS & gps);
+	void addEnvSensor(int type, float value);
+
+    GLuint getTextureId() {return textureId_;}
+    bool uvsInitialized() const {return uvs_initialized_;}
+    const float* uvsTransformed() const {return transformed_uvs_;}
+    void getVPMatrices(glm::mat4 & view, glm::mat4 & projection) const {view=viewMatrix_; projection=projectionMatrix_;}
+    ScreenRotation getScreenRotation() const {return colorCameraToDisplayRotation_;}
+    
+    void setOcclusionImage(const cv::Mat & image, const CameraModel & model) {occlusionModel_ = model; occlusionImage_ = image;}
+    const cv::Mat & getOcclusionImage(CameraModel * model=0) const {if(model)*model=occlusionModel_; return occlusionImage_; }
+
+protected:
+	virtual SensorData updateDataOnRender(Transform & pose);
+
+private:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+	void postUpdate(); // Should be called while being protected by dataMutex_
+
+protected:
+	CameraModel model_; // local transform is the device to camera optical rotation in rtabmap frame
+	Transform deviceTColorCamera_; // device to camera optical rotation in rtabmap frame
+    
+    GLuint textureId_;
+    glm::mat4 viewMatrix_;
+    glm::mat4 projectionMatrix_;
+    float transformed_uvs_[8];
+    bool uvs_initialized_ = false;
+
+private:
+	bool firstFrame_;
+	double stampEpochOffset_;
+	ScreenRotation colorCameraToDisplayRotation_;
+	GPS lastKnownGPS_;
+	EnvSensors lastEnvSensors_;
+	Transform originOffset_;
+	bool originUpdate_;
+    rtabmap::Transform manualOriginOffset_;
+	float upstreamRelocalizationAccThr_;
+	rtabmap::Transform previousAnchorPose_;
+	std::vector<float> previousAnchorLinearVelocity_;
+	double previousAnchorStamp_;
+    std::map<double, std::pair<rtabmap::Transform, rtabmap::Transform> > relocalizationDebugBuffer_;
+
+	USemaphore dataReady_;
+	UMutex dataMutex_;
+	SensorData data_;
+	Transform dataPose_;
+    bool dataGoodTracking_;
+
+	UMutex poseMutex_;
+	std::map<double, Transform> poseBuffer_; // <stamp, Pose>
+    
+    cv::Mat occlusionImage_;
+    CameraModel occlusionModel_;
+};
+
+} /* namespace rtabmap */
+#endif /* CAMERATANGO_H_ */

rtabmap/app/android/jni/CameraTango.cpp

@@ -0,0 +1,880 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * 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.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+*/
+
+#include "CameraTango.h"
+#include "util.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/util2d.h"
+#include <tango_client_api.h>
+#include <tango_support_api.h>
+#include "tango-gl/camera.h"
+
+namespace rtabmap {
+
+#define nullptr 0
+const int kVersionStringLength = 128;
+const int holeSize = 5;
+const float maxDepthError = 0.10;
+const int scanDownsampling = 1;
+
+//android phone
+//11 10 01 00 // portrait
+//01 11 00 10 // left
+//10 00 11 01 // right
+//00 01 10 11 // down
+
+const float kTextureCoords0[] = {1.0, 1.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0};
+const float kTextureCoords90[] = {0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 1.0, 0.0};
+const float kTextureCoords180[] = {0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 1.0};
+const float kTextureCoords270[] = {1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0};
+
+// Callbacks
+void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_cloud)
+{
+	CameraTango* app = static_cast<CameraTango*>(context);
+	if(point_cloud->num_points>0)
+	{
+		app->cloudReceived(cv::Mat(1, point_cloud->num_points, CV_32FC4, point_cloud->points[0]), point_cloud->timestamp);
+	}
+}
+
+void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuffer* color)
+{
+	CameraTango* app = static_cast<CameraTango*>(context);
+
+	cv::Mat tangoImage;
+	if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
+	{
+		tangoImage = cv::Mat(color->height, color->width, CV_8UC4, color->data);
+	}
+	else if(color->format == TANGO_HAL_PIXEL_FORMAT_YV12)
+	{
+		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+	}
+	else if(color->format == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
+	{
+		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+	}
+	else if(color->format == 35)
+	{
+		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+	}
+	else
+	{
+		LOGE("Not supported color format : %d.", color->format);
+	}
+
+	if(!tangoImage.empty())
+	{
+		app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp);
+	}
+}
+
+void onPoseAvailableRouter(void* context, const TangoPoseData* pose)
+{
+	if(pose->status_code == TANGO_POSE_VALID)
+	{
+		CameraTango* app = static_cast<CameraTango*>(context);
+		app->poseReceived(rtabmap_world_T_tango_world * app->tangoPoseToTransform(pose) * tango_device_T_rtabmap_world, pose->timestamp);
+	}
+}
+
+void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
+{
+	CameraTango* app = static_cast<CameraTango*>(context);
+	app->tangoEventReceived(event->type, event->event_key, event->event_value);
+}
+
+//////////////////////////////
+// CameraTango
+//////////////////////////////
+CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan) :
+		tango_config_(0),
+		colorCamera_(colorCamera),
+		decimation_(decimation),
+		rawScanPublished_(publishRawScan),
+		tangoColorType_(0),
+		tangoColorStamp_(0)
+{
+	UASSERT(decimation >= 1);
+}
+
+CameraTango::~CameraTango() {
+	// Disconnect Tango service
+	close();
+}
+
+// Compute fisheye distorted coordinates from undistorted coordinates.
+// The distortion model used by the Tango fisheye camera is called FOV and is
+// described in 'Straight lines have to be straight' by Frederic Devernay and
+// Olivier Faugeras. See https://hal.inria.fr/inria-00267247/document.
+// Tango ROS Streamer: https://github.com/Intermodalics/tango_ros/blob/master/tango_ros_common/tango_ros_native/src/tango_ros_node.cpp
+void applyFovModel(
+    double xu, double yu, double w, double w_inverse, double two_tan_w_div_two,
+    double* xd, double* yd) {
+  double ru = sqrt(xu * xu + yu * yu);
+  constexpr double epsilon = 1e-7;
+  if (w < epsilon || ru < epsilon) {
+    *xd = xu;
+    *yd = yu ;
+  } else {
+    double rd_div_ru = std::atan(ru * two_tan_w_div_two) * w_inverse / ru;
+    *xd = xu * rd_div_ru;
+    *yd = yu * rd_div_ru;
+  }
+}
+// Compute the warp maps to undistort the Tango fisheye image using the FOV
+// model. See OpenCV documentation for more information on warp maps:
+// http://docs.opencv.org/2.4/modules/imgproc/doc/geometric_transformations.html
+// Tango ROS Streamer: https://github.com/Intermodalics/tango_ros/blob/master/tango_ros_common/tango_ros_native/src/tango_ros_node.cpp
+// @param fisheyeModel the fisheye camera intrinsics.
+// @param mapX the output map for the x direction.
+// @param mapY the output map for the y direction.
+void initFisheyeRectificationMap(
+    const CameraModel& fisheyeModel,
+    cv::Mat & mapX, cv::Mat & mapY) {
+  const double & fx = fisheyeModel.K().at<double>(0,0);
+  const double & fy = fisheyeModel.K().at<double>(1,1);
+  const double & cx = fisheyeModel.K().at<double>(0,2);
+  const double & cy = fisheyeModel.K().at<double>(1,2);
+  const double & w = fisheyeModel.D().at<double>(0,0);
+  mapX.create(fisheyeModel.imageSize(), CV_32FC1);
+  mapY.create(fisheyeModel.imageSize(), CV_32FC1);
+  LOGD("initFisheyeRectificationMap: fx=%f fy=%f, cx=%f, cy=%f, w=%f", fx, fy, cx, cy, w);
+  // Pre-computed variables for more efficiency.
+  const double fy_inverse = 1.0 / fy;
+  const double fx_inverse = 1.0 / fx;
+  const double w_inverse = 1 / w;
+  const double two_tan_w_div_two = 2.0 * std::tan(w * 0.5);
+  // Compute warp maps in x and y directions.
+  // OpenCV expects maps from dest to src, i.e. from undistorted to distorted
+  // pixel coordinates.
+  for(int iu = 0; iu < fisheyeModel.imageHeight(); ++iu) {
+    for (int ju = 0; ju < fisheyeModel.imageWidth(); ++ju) {
+      double xu = (ju - cx) * fx_inverse;
+      double yu = (iu - cy) * fy_inverse;
+      double xd, yd;
+      applyFovModel(xu, yu, w, w_inverse, two_tan_w_div_two, &xd, &yd);
+      double jd = cx + xd * fx;
+      double id = cy + yd * fy;
+      mapX.at<float>(iu, ju) = jd;
+      mapY.at<float>(iu, ju) = id;
+    }
+  }
+}
+
+bool CameraTango::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	close();
+
+	CameraMobile::init(calibrationFolder, cameraName);
+
+	TangoSupport_initialize(TangoService_getPoseAtTime, TangoService_getCameraIntrinsics);
+
+	// Connect to Tango
+	LOGI("NativeRTABMap: Setup tango config");
+	tango_config_ = TangoService_getConfig(TANGO_CONFIG_DEFAULT);
+	if (tango_config_ == nullptr)
+	{
+		LOGE("NativeRTABMap: Failed to get default config form");
+		return false;
+	}
+
+	// Set auto-recovery for motion tracking as requested by the user.
+	bool is_atuo_recovery = true;
+	int ret = TangoConfig_setBool(tango_config_, "config_enable_auto_recovery", is_atuo_recovery);
+	if (ret != TANGO_SUCCESS)
+	{
+		LOGE("NativeRTABMap: config_enable_auto_recovery() failed with error code: %d", ret);
+		return false;
+	}
+
+	if(colorCamera_)
+	{
+		// Enable color.
+		ret = TangoConfig_setBool(tango_config_, "config_enable_color_camera", true);
+		if (ret != TANGO_SUCCESS)
+		{
+			LOGE("NativeRTABMap: config_enable_color_camera() failed with error code: %d", ret);
+			return false;
+		}
+	}
+
+	// Enable depth.
+	ret = TangoConfig_setBool(tango_config_, "config_enable_depth", true);
+	if (ret != TANGO_SUCCESS)
+	{
+		LOGE("NativeRTABMap: config_enable_depth() failed with error code: %d", ret);
+		return false;
+	}
+
+	// Need to specify the depth_mode as XYZC.
+	ret = TangoConfig_setInt32(tango_config_, "config_depth_mode", TANGO_POINTCLOUD_XYZC);
+	if (ret != TANGO_SUCCESS)
+	{
+		LOGE("Failed to set 'depth_mode' configuration flag with error code: %d", ret);
+		return false;
+	}
+
+	// Note that it's super important for AR applications that we enable low
+	// latency imu integration so that we have pose information available as
+	// quickly as possible. Without setting this flag, you'll often receive
+	// invalid poses when calling GetPoseAtTime for an image.
+	ret = TangoConfig_setBool(tango_config_, "config_enable_low_latency_imu_integration", true);
+	if (ret != TANGO_SUCCESS)
+	{
+		LOGE("NativeRTABMap: Failed to enable low latency imu integration.");
+		return false;
+	}
+
+	// Drift correction allows motion tracking to recover after it loses tracking.
+	//
+	// The drift corrected pose is is available through the frame pair with
+	// base frame AREA_DESCRIPTION and target frame DEVICE.
+	/*ret = TangoConfig_setBool(tango_config_, "config_enable_drift_correction", true);
+	if (ret != TANGO_SUCCESS) {
+		LOGE(
+			"NativeRTABMap: enabling config_enable_drift_correction "
+			"failed with error code: %d",
+			ret);
+		return false;
+	}*/
+
+	// Get TangoCore version string from service.
+	char tango_core_version[kVersionStringLength];
+	ret = TangoConfig_getString(tango_config_, "tango_service_library_version", tango_core_version, kVersionStringLength);
+	if (ret != TANGO_SUCCESS)
+	{
+		LOGE("NativeRTABMap: get tango core version failed with error code: %d", ret);
+		return false;
+	}
+	LOGI("NativeRTABMap: Tango version : %s", tango_core_version);
+
+
+	// Callbacks
+	LOGI("NativeRTABMap: Setup callbacks");
+	// Attach the OnXYZijAvailable callback.
+	// The callback will be called after the service is connected.
+	ret = TangoService_connectOnPointCloudAvailable(onPointCloudAvailableRouter);
+	if (ret != TANGO_SUCCESS)
+	{
+		LOGE("NativeRTABMap: Failed to connect to point cloud callback with error code: %d", ret);
+		return false;
+	}
+
+	ret = TangoService_connectOnFrameAvailable(colorCamera_?TANGO_CAMERA_COLOR:TANGO_CAMERA_FISHEYE, this, onFrameAvailableRouter);
+	if (ret != TANGO_SUCCESS)
+	{
+		LOGE("NativeRTABMap: Failed to connect to color callback with error code: %d", ret);
+		return false;
+	}
+
+	// Attach the onPoseAvailable callback.
+	// The callback will be called after the service is connected.
+	TangoCoordinateFramePair pair;
+	//pair.base = TANGO_COORDINATE_FRAME_AREA_DESCRIPTION; // drift correction is enabled
+	pair.base = TANGO_COORDINATE_FRAME_START_OF_SERVICE;
+	pair.target = TANGO_COORDINATE_FRAME_DEVICE;
+	ret = TangoService_connectOnPoseAvailable(1, &pair, onPoseAvailableRouter);
+	if (ret != TANGO_SUCCESS)
+	{
+		LOGE("NativeRTABMap: Failed to connect to pose callback with error code: %d", ret);
+		return false;
+	}
+
+	// Attach the onEventAvailable callback.
+	// The callback will be called after the service is connected.
+	ret = TangoService_connectOnTangoEvent(onTangoEventAvailableRouter);
+	if (ret != TANGO_SUCCESS)
+	{
+		LOGE("PointCloudApp: Failed to connect to event callback with error code: %d", ret);
+		return false;
+	}
+
+	// Now connect service so the callbacks above will be called
+	LOGI("NativeRTABMap: Connect to tango service");
+	ret = TangoService_connect(this, tango_config_);
+	if (ret != TANGO_SUCCESS)
+	{
+		LOGE("NativeRTABMap: Failed to connect to the Tango service with error code: %d", ret);
+		return false;
+	}
+
+	// update extrinsics
+	LOGI("NativeRTABMap: Update extrinsics");
+	TangoPoseData pose_data;
+	TangoCoordinateFramePair frame_pair;
+
+	// TangoService_getPoseAtTime function is used for query device extrinsics
+	// as well. We use timestamp 0.0 and the target frame pair to get the
+	// extrinsics from the sensors.
+	//
+	// Get color camera with respect to device transformation matrix.
+	frame_pair.base = TANGO_COORDINATE_FRAME_DEVICE;
+	frame_pair.target = colorCamera_?TANGO_COORDINATE_FRAME_CAMERA_COLOR:TANGO_COORDINATE_FRAME_CAMERA_FISHEYE;
+	ret = TangoService_getPoseAtTime(0.0, frame_pair, &pose_data);
+	if (ret != TANGO_SUCCESS)
+	{
+		LOGE("NativeRTABMap: Failed to get transform between the color camera frame and device frames");
+		return false;
+	}
+	deviceTColorCamera_ = rtabmap::Transform(
+			pose_data.translation[0],
+			pose_data.translation[1],
+			pose_data.translation[2],
+			pose_data.orientation[0],
+			pose_data.orientation[1],
+			pose_data.orientation[2],
+			pose_data.orientation[3]);
+	deviceTColorCamera_ = rtabmap_world_T_opengl_world * deviceTColorCamera_;
+
+	// camera intrinsic
+	TangoCameraIntrinsics color_camera_intrinsics;
+	ret = TangoService_getCameraIntrinsics(colorCamera_?TANGO_CAMERA_COLOR:TANGO_CAMERA_FISHEYE, &color_camera_intrinsics);
+	if (ret != TANGO_SUCCESS)
+	{
+		LOGE("NativeRTABMap: Failed to get the intrinsics for the color camera with error code: %d.", ret);
+		return false;
+	}
+
+	LOGD("Calibration: fx=%f fy=%f cx=%f cy=%f width=%d height=%d",
+			color_camera_intrinsics.fx,
+			color_camera_intrinsics.fy,
+			color_camera_intrinsics.cx,
+			color_camera_intrinsics.cy,
+			color_camera_intrinsics.width,
+			color_camera_intrinsics.height);
+
+	cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
+	K.at<double>(0,0) = color_camera_intrinsics.fx;
+	K.at<double>(1,1) = color_camera_intrinsics.fy;
+	K.at<double>(0,2) = color_camera_intrinsics.cx;
+	K.at<double>(1,2) = color_camera_intrinsics.cy;
+	cv::Mat D = cv::Mat::zeros(1, 5, CV_64FC1);
+	LOGD("Calibration type = %d", color_camera_intrinsics.calibration_type);
+	if(color_camera_intrinsics.calibration_type == TANGO_CALIBRATION_POLYNOMIAL_5_PARAMETERS ||
+			color_camera_intrinsics.calibration_type == TANGO_CALIBRATION_EQUIDISTANT)
+	{
+		D.at<double>(0,0) = color_camera_intrinsics.distortion[0];
+		D.at<double>(0,1) = color_camera_intrinsics.distortion[1];
+		D.at<double>(0,2) = color_camera_intrinsics.distortion[2];
+		D.at<double>(0,3) = color_camera_intrinsics.distortion[3];
+		D.at<double>(0,4) = color_camera_intrinsics.distortion[4];
+	}
+	else if(color_camera_intrinsics.calibration_type == TANGO_CALIBRATION_POLYNOMIAL_3_PARAMETERS)
+	{
+		D.at<double>(0,0) = color_camera_intrinsics.distortion[0];
+		D.at<double>(0,1) = color_camera_intrinsics.distortion[1];
+		D.at<double>(0,2) = 0.;
+		D.at<double>(0,3) = 0.;
+		D.at<double>(0,4) = color_camera_intrinsics.distortion[2];
+	}
+	else if(color_camera_intrinsics.calibration_type == TANGO_CALIBRATION_POLYNOMIAL_2_PARAMETERS)
+	{
+		D.at<double>(0,0) = color_camera_intrinsics.distortion[0];
+		D.at<double>(0,1) = color_camera_intrinsics.distortion[1];
+		D.at<double>(0,2) = 0.;
+		D.at<double>(0,3) = 0.;
+		D.at<double>(0,4) = 0.;
+	}
+
+	cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+	cv::Mat P;
+
+	LOGD("Distortion params: %f, %f, %f, %f, %f", D.at<double>(0,0), D.at<double>(0,1), D.at<double>(0,2), D.at<double>(0,3), D.at<double>(0,4));
+	model_ = CameraModel(colorCamera_?"color":"fisheye",
+			cv::Size(color_camera_intrinsics.width, color_camera_intrinsics.height),
+			K, D, R, P,
+			deviceTColorCamera_);
+
+	if(!colorCamera_)
+	{
+		initFisheyeRectificationMap(model_, fisheyeRectifyMapX_, fisheyeRectifyMapY_);
+	}
+
+	LOGI("deviceTColorCameraRtabmap  =%s", deviceTColorCamera_.prettyPrint().c_str());
+	return true;
+}
+
+void CameraTango::close()
+{
+	if(tango_config_)
+	{
+		TangoConfig_free(tango_config_);
+		tango_config_ = nullptr;
+		LOGI("TangoService_disconnect()");
+		TangoService_disconnect();
+		LOGI("TangoService_disconnect() done.");
+	}
+	fisheyeRectifyMapX_ = cv::Mat();
+	fisheyeRectifyMapY_ = cv::Mat();
+
+	CameraMobile::close();
+}
+
+void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
+{
+	if(!cloud.empty())
+	{
+		//LOGD("Depth received! %fs (%d points)", timestamp, cloud.cols);
+
+		UASSERT(cloud.type() == CV_32FC4);
+		boost::mutex::scoped_lock  lock(tangoDataMutex_);
+
+		// From post: http://stackoverflow.com/questions/29236110/timing-issues-with-tango-image-frames
+		// "In the current version of Project Tango Tablet RGB IR camera
+		//  is used for both depth and color images and it can only do one
+		//  or the other for each frame. So in the stream we get 4 RGB frames
+		//  followed by 1 Depth frame resulting in the pattern you observed. This
+		//  is more of a hardware limitation."
+		//
+		//  So, synchronize with the last RGB frame before the Depth is acquired
+		if(!tangoColor_.empty())
+		{
+			UTimer timer;
+			double dt = fabs(timestamp - tangoColorStamp_);
+
+			//LOGD("Depth: %f vs %f = %f", tangoColorStamp_, timestamp, dt);
+
+			if(dt >= 0.0 && dt < 0.5)
+			{
+				bool notify = !tangoData_.isValid();
+
+				cv::Mat tangoImage = tangoColor_;
+				cv::Mat rgb;
+				double cloudStamp = timestamp;
+				double rgbStamp = tangoColorStamp_;
+				int tangoColorType = tangoColorType_;
+
+				tangoColor_ = cv::Mat();
+				tangoColorStamp_ = 0.0;
+				tangoColorType_ = 0;
+
+				LOGD("tangoColorType=%d", tangoColorType);
+				if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
+				{
+					cv::cvtColor(tangoImage, rgb, cv::COLOR_RGBA2BGR);
+				}
+				else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YV12)
+				{
+					cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV2BGR_YV12);
+				}
+				else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
+				{
+					cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV2BGR_NV21);
+				}
+				else if(tangoColorType == 35)
+				{
+					cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV420sp2GRAY);
+				}
+				else
+				{
+					LOGE("Not supported color format : %d.", tangoColorType);
+					tangoData_ = SensorData();
+					return;
+				}
+
+				//for(int i=0; i<rgb.cols; ++i)
+				//{
+				//	UERROR("%d,%d,%d", (int)rgb.at<cv::Vec3b>(i)[0], (int)rgb.at<cv::Vec3b>(i)[1], (int)rgb.at<cv::Vec3b>(i)[2]);
+				//}
+
+				CameraModel model = model_;
+
+				if(colorCamera_)
+				{
+					if(decimation_ > 1)
+					{
+						rgb = util2d::decimate(rgb, decimation_);
+						model = model.scaled(1.0/double(decimation_));
+					}
+				}
+				else
+				{
+					//UTimer t;
+					cv::Mat rgbRect;
+					cv::remap(rgb, rgbRect, fisheyeRectifyMapX_, fisheyeRectifyMapY_, cv::INTER_LINEAR, cv::BORDER_CONSTANT, 0);
+					rgb = rgbRect;
+					//LOGD("Rectification time=%fs", t.ticks());
+				}
+
+				// Querying the depth image's frame transformation based on the depth image's
+				// timestamp.
+				cv::Mat depth;
+
+				// Calculate the relative pose from color camera frame at timestamp
+				// color_timestamp t1 and depth
+				// camera frame at depth_timestamp t0.
+				Transform colorToDepth;
+				TangoPoseData pose_color_image_t1_T_depth_image_t0;
+				if (TangoSupport_calculateRelativePose(
+						rgbStamp, colorCamera_?TANGO_COORDINATE_FRAME_CAMERA_COLOR:TANGO_COORDINATE_FRAME_CAMERA_FISHEYE, cloudStamp,
+					  TANGO_COORDINATE_FRAME_CAMERA_DEPTH,
+					  &pose_color_image_t1_T_depth_image_t0) == TANGO_SUCCESS)
+				{
+					colorToDepth = tangoPoseToTransform(&pose_color_image_t1_T_depth_image_t0);
+				}
+				else
+				{
+					LOGE(
+						"SynchronizationApplication: Could not find a valid relative pose at "
+						"time for color and "
+						" depth cameras.");
+				}
+
+				if(colorToDepth.getNormSquared() > 100000)
+				{
+					LOGE("Very large color to depth error detected (%s)! Ignoring this frame!", colorToDepth.prettyPrint().c_str());
+					colorToDepth.setNull();
+				}
+				cv::Mat scan;
+				if(!colorToDepth.isNull())
+				{
+					// The Color Camera frame at timestamp t0 with respect to Depth
+					// Camera frame at timestamp t1.
+					//LOGD("colorToDepth=%s", colorToDepth.prettyPrint().c_str());
+					LOGD("rgb=%dx%d cloud size=%d", rgb.cols, rgb.rows, (int)cloud.total());
+
+					int pixelsSet = 0;
+					int depthSizeDec = colorCamera_?8:1;
+					depth = cv::Mat::zeros(model_.imageHeight()/depthSizeDec, model_.imageWidth()/depthSizeDec, CV_16UC1); // mm
+					CameraModel depthModel = model_.scaled(1.0f/float(depthSizeDec));
+					std::vector<cv::Point3f> scanData(rawScanPublished_?cloud.total():0);
+					int oi=0;
+					int closePoints = 0;
+					float closeROI[4];
+					closeROI[0] = depth.cols/4;
+					closeROI[1] = 3*(depth.cols/4);
+					closeROI[2] = depth.rows/4;
+					closeROI[3] = 3*(depth.rows/4);
+					unsigned short minDepthValue=10000;
+					for(unsigned int i=0; i<cloud.total(); ++i)
+					{
+						const float * p = cloud.ptr<float>(0,i);
+						cv::Point3f pt = util3d::transformPoint(cv::Point3f(p[0], p[1], p[2]), colorToDepth);
+
+						if(pt.z > 0.0f && i%scanDownsampling == 0 && rawScanPublished_)
+						{
+							scanData.at(oi++) = pt;
+						}
+
+						int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
+						// get the coordinate on image plane.
+						pixel_x_l = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx());
+						pixel_y_l = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy());
+						pixel_x_h = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx() + 0.5f);
+						pixel_y_h = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy() + 0.5f);
+						unsigned short depth_value(pt.z * 1000.0f);
+
+						if(pixel_x_l>=closeROI[0] && pixel_x_l<closeROI[1] &&
+						   pixel_y_l>closeROI[2] && pixel_y_l<closeROI[3] &&
+						   depth_value < 600)
+						{
+							++closePoints;
+							if(depth_value < minDepthValue)
+							{
+								minDepthValue = depth_value;
+							}
+						}
+
+						bool pixelSet = false;
+						if(pixel_x_l>=0 && pixel_x_l<depth.cols &&
+						   pixel_y_l>0 && pixel_y_l<depth.rows && // ignore first line
+						   depth_value)
+						{
+							unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_l, pixel_x_l);
+							if(depthPixel == 0 || depthPixel > depth_value)
+							{
+								depthPixel = depth_value;
+								pixelSet = true;
+							}
+						}
+						if(pixel_x_h>=0 && pixel_x_h<depth.cols &&
+						   pixel_y_h>0 && pixel_y_h<depth.rows && // ignore first line
+						   depth_value)
+						{
+							unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_h, pixel_x_h);
+							if(depthPixel == 0 || depthPixel > depth_value)
+							{
+								depthPixel = depth_value;
+								pixelSet = true;
+							}
+						}
+						if(pixelSet)
+						{
+							pixelsSet += 1;
+						}
+					}
+
+					if(closePoints > 100)
+					{
+						this->post(new CameraInfoEvent(0, "TooClose", ""));
+					}
+
+					if(oi)
+					{
+						scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone();
+					}
+					//LOGD("pixels depth set= %d", pixelsSet);
+				}
+				else
+				{
+					LOGE("color to depth pose is null?!? (rgb stamp=%f) (depth stamp=%f)", rgbStamp, cloudStamp);
+				}
+
+				if(!rgb.empty() && !depth.empty())
+				{
+					depth = rtabmap::util2d::fillDepthHoles(depth, holeSize, maxDepthError);
+
+					Transform odom = getPoseAtTimestamp(rgbStamp);
+
+					//LOGD("Local    = %s", model.localTransform().prettyPrint().c_str());
+					//LOGD("tango    = %s", poseDevice.prettyPrint().c_str());
+					//LOGD("opengl(t)= %s", (opengl_world_T_tango_world * poseDevice).prettyPrint().c_str());
+
+					// occlusion depth
+					if(!depth.empty())
+					{
+						rtabmap::CameraModel depthModel = model.scaled(float(depth.cols) / float(model.imageWidth()));
+						depthModel.setLocalTransform(odom*model.localTransform());
+						this->setOcclusionImage(depth, depthModel);
+					}
+
+					//LOGD("rtabmap  = %s", odom.prettyPrint().c_str());
+					//LOGD("opengl(r)= %s", (opengl_world_T_rtabmap_world * odom * rtabmap_device_T_opengl_device).prettyPrint().c_str());
+
+					Transform scanLocalTransform = model.localTransform();
+
+					if(rawScanPublished_)
+					{
+						tangoData_ = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
+					}
+					else
+					{
+						tangoData_ = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
+					}
+					tangoData_.setGroundTruth(odom);
+				}
+				else
+				{
+					LOGE("Could not get depth and rgb images!?!");
+					tangoData_ = SensorData();
+					return;
+				}
+
+				if(notify)
+				{
+					tangoDataReady_.release();
+				}
+				LOGD("process cloud received %fs", timer.ticks());
+			}
+		}
+	}
+}
+
+void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double timestamp)
+{
+	if(!tangoImage.empty())
+	{
+		//LOGD("RGB received! %fs", timestamp);
+
+		boost::mutex::scoped_lock  lock(tangoDataMutex_);
+
+		tangoColor_ = tangoImage.clone();
+		tangoColorStamp_ = timestamp;
+		tangoColorType_ = type;
+	}
+}
+
+void CameraTango::tangoEventReceived(int type, const char * key, const char * value)
+{
+	this->post(new CameraInfoEvent(type, key, value));
+}
+
+std::string CameraTango::getSerial() const
+{
+	return "Tango";
+}
+
+rtabmap::Transform CameraTango::tangoPoseToTransform(const TangoPoseData * tangoPose) const
+{
+	UASSERT(tangoPose);
+	rtabmap::Transform pose;
+
+	pose = rtabmap::Transform(
+			tangoPose->translation[0],
+			tangoPose->translation[1],
+			tangoPose->translation[2],
+			tangoPose->orientation[0],
+			tangoPose->orientation[1],
+			tangoPose->orientation[2],
+			tangoPose->orientation[3]);
+
+	return pose;
+}
+
+rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
+{
+	rtabmap::Transform pose;
+
+	TangoPoseData pose_start_service_T_device;
+	TangoCoordinateFramePair frame_pair;
+	frame_pair.base = TANGO_COORDINATE_FRAME_START_OF_SERVICE;
+	frame_pair.target = TANGO_COORDINATE_FRAME_DEVICE;
+	TangoErrorType status = TangoService_getPoseAtTime(timestamp, frame_pair, &pose_start_service_T_device);
+	if (status != TANGO_SUCCESS)
+	{
+		LOGE(
+			"PoseData: Failed to get transform between the Start of service and "
+			"device frames at timestamp %lf",
+			timestamp);
+	}
+	if (pose_start_service_T_device.status_code != TANGO_POSE_VALID)
+	{
+		LOGW(
+			"PoseData: Failed to get transform between the Start of service and "
+			"device frames at timestamp %lf",
+			timestamp);
+	}
+	else
+	{
+		pose = rtabmap_world_T_tango_world * tangoPoseToTransform(&pose_start_service_T_device) * tango_device_T_rtabmap_world;
+	}
+
+	return pose;
+}
+
+SensorData CameraTango::updateDataOnRender(Transform & pose)
+{
+	//LOGI("Capturing image...");
+
+	pose.setNull();
+	if(textureId_ == 0)
+	{
+		glGenTextures(1, &textureId_);
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	}
+
+	// Update Texture (optional, just for first-view rendering)
+	if(colorCamera_ && textureId_)
+	{
+		double video_overlay_timestamp;
+		TangoErrorType status = TangoService_updateTextureExternalOes(TANGO_CAMERA_COLOR, textureId_, &video_overlay_timestamp);
+
+		if (status == TANGO_SUCCESS)
+		{
+			pose = getPoseAtTimestamp(video_overlay_timestamp);
+
+			int rotation = static_cast<int>(getScreenRotation()) + 1; // remove 90deg camera rotation
+			  if (rotation > 3) {
+				  rotation -= 4;
+			  }
+
+			TangoDoubleMatrixTransformData matrix_transform;
+			status = TangoSupport_getDoubleMatrixTransformAtTime(
+					video_overlay_timestamp,
+					TANGO_COORDINATE_FRAME_CAMERA_COLOR,
+					TANGO_COORDINATE_FRAME_START_OF_SERVICE,
+					TANGO_SUPPORT_ENGINE_OPENGL,
+					TANGO_SUPPORT_ENGINE_OPENGL,
+					static_cast<TangoSupportRotation>(rotation),
+					&matrix_transform);
+			if (matrix_transform.status_code == TANGO_POSE_VALID)
+			{
+				// Get projection matrix
+				TangoCameraIntrinsics color_camera_intrinsics;
+				int ret = TangoSupport_getCameraIntrinsicsBasedOnDisplayRotation(
+						TANGO_CAMERA_COLOR,
+						static_cast<TangoSupportRotation>(rotation),
+						&color_camera_intrinsics);
+
+				if (ret == TANGO_SUCCESS) {
+					float image_width = static_cast<float>(color_camera_intrinsics.width);
+					float image_height = static_cast<float>(color_camera_intrinsics.height);
+					float fx = static_cast<float>(color_camera_intrinsics.fx);
+					float fy = static_cast<float>(color_camera_intrinsics.fy);
+					float cx = static_cast<float>(color_camera_intrinsics.cx);
+					float cy = static_cast<float>(color_camera_intrinsics.cy);
+
+					viewMatrix_ = glm::make_mat4(matrix_transform.matrix);
+
+					projectionMatrix_ = tango_gl::Camera::ProjectionMatrixForCameraIntrinsics(
+							image_width, image_height, fx, fy, cx, cy, 0.3, 50);
+
+					switch(rotation)
+					{
+					case ROTATION_90:
+						memcpy(transformed_uvs_, kTextureCoords90, 8*sizeof(float));
+						break;
+					case ROTATION_180:
+						memcpy(transformed_uvs_, kTextureCoords180, 8*sizeof(float));
+						break;
+					case ROTATION_270:
+						memcpy(transformed_uvs_, kTextureCoords270, 8*sizeof(float));
+						break;
+					case ROTATION_0:
+					default:
+						memcpy(transformed_uvs_, kTextureCoords0, 8*sizeof(float));
+					}
+					uvs_initialized_ = true;
+				}
+				else
+				{
+					UERROR("TangoSupport_getCameraIntrinsicsBasedOnDisplayRotation failed!");
+				}
+			}
+			else
+			{
+				UERROR("TangoSupport_getDoubleMatrixTransformAtTime failed!");
+			}
+		}
+		else
+		{
+			UERROR("TangoService_updateTextureExternalOes failed!");
+		}
+	}
+
+	SensorData data;
+	if(tangoDataReady_.acquireTry(1))
+	{
+		boost::mutex::scoped_lock  lock(tangoDataMutex_);
+		data = tangoData_;
+		tangoData_ = SensorData();
+		pose = data.groundTruth();
+		data.setGroundTruth(Transform());
+	}
+	return data;
+
+}
+
+} /* namespace rtabmap */

rtabmap/app/android/jni/CameraTango.h

@@ -0,0 +1,84 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * 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.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS 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 CAMERATANGO_H_
+#define CAMERATANGO_H_
+
+#include "CameraMobile.h"
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/GeodeticCoords.h>
+#include <rtabmap/utilite/UMutex.h>
+#include <rtabmap/utilite/USemaphore.h>
+#include <rtabmap/utilite/UEventsSender.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <boost/thread/mutex.hpp>
+#include <tango_client_api.h>
+#include <tango_support_api.h>
+
+namespace rtabmap {
+
+class CameraTango : public CameraMobile {
+public:
+	CameraTango(bool colorCamera, int decimation, bool publishRawScan);
+	virtual ~CameraTango();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual void close(); // close Tango connection
+	virtual std::string getSerial() const;
+	rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const;
+	void setDecimation(int value) {decimation_ = value;}
+	void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}
+
+	void cloudReceived(const cv::Mat & cloud, double timestamp);
+	void rgbReceived(const cv::Mat & tangoImage, int type, double timestamp);
+	void tangoEventReceived(int type, const char * key, const char * value);
+
+protected:
+	virtual SensorData updateDataOnRender(Transform & pose);
+
+private:
+	rtabmap::Transform getPoseAtTimestamp(double timestamp);
+
+private:
+	void * tango_config_;
+	bool colorCamera_;
+	int decimation_;
+	bool rawScanPublished_;
+	SensorData tangoData_;
+	cv::Mat tangoColor_;
+	int tangoColorType_;
+	double tangoColorStamp_;
+	boost::mutex tangoDataMutex_;
+	USemaphore tangoDataReady_;
+	cv::Mat fisheyeRectifyMapX_;
+	cv::Mat fisheyeRectifyMapY_;
+};
+
+} /* namespace rtabmap */
+#endif /* CAMERATANGO_H_ */

rtabmap/app/android/jni/Measure.h

@@ -0,0 +1,65 @@
+/*
+Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * 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.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS 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 MEASURE_H_
+#define MEASURE_H_
+
+#include <opencv2/opencv.hpp>
+
+class Measure
+{
+public:
+	Measure(
+			const cv::Point3f & pt1,
+			const cv::Point3f & pt2,
+			const cv::Vec3f & n1,
+			const cv::Vec3f & n2) :
+				pt1_(pt1),
+				pt2_(pt2),
+				n1_(n1),
+				n2_(n2)
+	{
+		length_ = cv::norm(pt2_ - pt1_);
+	}
+	virtual ~Measure() {}
+
+	float length() const {return length_;}
+	const cv::Point3f & pt1() const {return pt1_;}
+	const cv::Point3f & pt2() const {return pt2_;}
+	const cv::Vec3f & n1() const {return n1_;}
+	const cv::Vec3f & n2() const {return n2_;}
+
+private:
+	cv::Point3f pt1_;
+	cv::Point3f pt2_;
+	cv::Vec3f n1_;
+	cv::Vec3f n2_;
+	float length_;
+};
+
+
+#endif /* MEASURE_H_ */

rtabmap/app/android/jni/ProgressionStatus.h

@@ -0,0 +1,155 @@
+/*
+ * ProgressionStatus.h
+ *
+ *  Created on: Feb 28, 2017
+ *      Author: mathieu
+ */
+
+#ifndef APP_ANDROID_JNI_PROGRESSIONSTATUS_H_
+#define APP_ANDROID_JNI_PROGRESSIONSTATUS_H_
+
+#include <rtabmap/core/ProgressState.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UEventsManager.h>
+#ifdef __ANDROID__
+#include <jni.h>
+#endif
+
+namespace rtabmap {
+
+class ProgressEvent : public UEvent
+{
+public:
+	ProgressEvent(int count = 1) : count_(count){}
+	virtual std::string getClassName() const {return "ProgressEvent";}
+
+	int count_;
+};
+
+class ProgressionStatus: public ProgressState, public UEventsHandler
+{
+public:
+	ProgressionStatus() : count_(0), max_(100)
+#ifdef __ANDROID__
+    , jvm_(0), rtabmap_(0)
+#else
+    , swiftClassPtr_(0)
+#endif
+	{
+		registerToEventsManager();
+	}
+
+#ifdef __ANDROID__
+	void setJavaObjects(JavaVM * jvm, jobject rtabmap)
+	{
+		jvm_ = jvm;
+		rtabmap_ = rtabmap;
+	}
+#else
+    void setSwiftCallback(void * classPtr, void(*callback)(void *, int, int))
+    {
+        swiftClassPtr_ = classPtr;
+        swiftCallback = callback;
+    }
+#endif
+
+	void reset(int max)
+	{
+		count_=-1;
+		max_ = max;
+		setCanceled(false);
+
+		increment();
+	}
+
+	void setMax(int max)
+	{
+		max_ = max;
+	}
+	int getMax() const {return max_;}
+
+	void increment(int count = 1) const
+	{
+		UEventsManager::post(new ProgressEvent(count));
+	}
+
+	void finish()
+	{
+
+	}
+
+	virtual bool callback(const std::string & msg) const
+	{
+		if(!isCanceled())
+		{
+			increment();
+		}
+
+		return ProgressState::callback(msg);
+	}
+	virtual ~ProgressionStatus(){}
+
+protected:
+	virtual bool handleEvent(UEvent * event)
+	{
+		if(event->getClassName().compare("ProgressEvent") == 0)
+		{
+			count_ += ((ProgressEvent*)event)->count_;
+			// Call JAVA callback
+			bool success = false;
+#ifdef __ANDROID__
+			if(jvm_ && rtabmap_)
+			{
+				JNIEnv *env = 0;
+				jint rs = jvm_->AttachCurrentThread(&env, NULL);
+				if(rs == JNI_OK && env)
+				{
+					jclass clazz = env->GetObjectClass(rtabmap_);
+					if(clazz)
+					{
+						jmethodID methodID = env->GetMethodID(clazz, "updateProgressionCallback", "(II)V" );
+						if(methodID)
+						{
+							env->CallVoidMethod(rtabmap_, methodID,
+									count_,
+									max_);
+							success = true;
+						}
+					}
+				}
+				jvm_->DetachCurrentThread();
+			}
+#else // APPLE
+            if(swiftClassPtr_)
+            {
+                std::function<void()> actualCallback = [&](){
+                    swiftCallback(swiftClassPtr_, count_, max_);
+                };
+                actualCallback();
+                success = true;
+            }
+#endif
+			if(!success)
+			{
+				UERROR("Failed to call rtabmap::updateProgressionCallback");
+			}
+		}
+		return false;
+	}
+
+private:
+	int count_;
+	int max_;
+#ifdef __ANDROID__
+	JavaVM *jvm_;
+	jobject rtabmap_;
+#else
+    void * swiftClassPtr_;
+    void(*swiftCallback)(void *, int, int);
+#endif
+};
+
+}
+
+
+#endif /* APP_ANDROID_JNI_PROGRESSIONSTATUS_H_ */

rtabmap/app/android/jni/RTABMapApp.h

@@ -0,0 +1,348 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * 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.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS 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 RTABMAP_APP_H_
+#define RTABMAP_APP_H_
+
+#ifdef __ANDROID__
+#include <jni.h>
+#endif
+#include <memory>
+
+#include <tango-gl/util.h>
+
+#include "scene.h"
+#include "CameraMobile.h"
+#include "util.h"
+#include "ProgressionStatus.h"
+
+#include <rtabmap/core/SensorCaptureThread.h>
+#include <rtabmap/core/RtabmapThread.h>
+#include <rtabmap/core/SensorEvent.h>
+#include <rtabmap/utilite/UEventsHandler.h>
+#include <boost/thread/mutex.hpp>
+#include <pcl/pcl_base.h>
+#include <pcl/TextureMesh.h>
+
+#include "Measure.h"
+
+// RTABMapApp handles the application lifecycle and resources.
+class RTABMapApp : public UEventsHandler {
+ public:
+  // Constructor and deconstructor.
+#ifdef __ANDROID__
+  RTABMapApp(JNIEnv* env, jobject caller_activity);
+#else // __APPLE__
+  RTABMapApp();
+  void setupSwiftCallbacks(void * classPtr,
+                           void(*progressCallback)(void *, int, int),
+                           void(*initCallback)(void *, int, const char*),
+                           void(*statsUpdatedCallback)(void *,
+                                                    int, int, int, int,
+                                                    float,
+                                                    int, int, int, int, int ,int,
+                                                    float,
+                                                    int,
+                                                    float,
+                                                    int,
+                                                    float, float, float, float,
+                                                    int, int,
+                                                    float, float, float, float, float, float),
+                           void(*cameraInfoCallback)(void *, int, const char*, const char*));
+    
+#endif
+  ~RTABMapApp();
+
+  void setScreenRotation(int displayRotation, int cameraRotation);
+
+  int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, bool clearDatabase);
+
+  bool isBuiltWith(int cameraDriver) const;
+#ifdef __ANDROID__
+  bool startCamera(JNIEnv* env, jobject iBinder, jobject context, jobject activity, int driver);
+#else // __APPLE__
+  bool startCamera();
+#endif
+  // Allocate OpenGL resources for rendering, mainly for initializing the Scene.
+  void InitializeGLContent();
+
+  // Setup the view port width and height.
+  void SetViewPort(int width, int height);
+
+  // Main render loop.
+  int Render();
+
+  void stopCamera();
+
+  // Set render camera's viewing angle, first person, third person or top down.
+  //
+  // @param: camera_type, camera type includes first person, third person and
+  //         top down
+  void SetCameraType(tango_gl::GestureCamera::CameraType camera_type);
+
+  // Touch event passed from android activity. This function only supports two
+  // touches.
+  //
+  // @param: touch_count, total count for touches.
+  // @param: event, touch event of current touch.
+  // @param: x0, normalized touch location for touch 0 on x axis.
+  // @param: y0, normalized touch location for touch 0 on y axis.
+  // @param: x1, normalized touch location for touch 1 on x axis.
+  // @param: y1, normalized touch location for touch 1 on y axis.
+  void OnTouchEvent(int touch_count, tango_gl::GestureCamera::TouchEvent event,
+                    float x0, float y0, float x1, float y1);
+
+  void setPausedMapping(bool paused);
+  void setOnlineBlending(bool enabled);
+  void setMapCloudShown(bool shown);
+  void setOdomCloudShown(bool shown);
+  void setMeshRendering(bool enabled, bool withTexture);
+  void setPointSize(float value);
+  void setFOV(float angle);
+  void setOrthoCropFactor(float value);
+  void setGridRotation(float value);
+  void setLighting(bool enabled);
+  void setBackfaceCulling(bool enabled);
+  void setWireframe(bool enabled);
+  void setTextureColorSeamsHidden(bool hidden);
+  void setLocalizationMode(bool enabled);
+  void setTrajectoryMode(bool enabled);
+  void setGraphOptimization(bool enabled);
+  void setNodesFiltering(bool enabled);
+  void setGraphVisible(bool visible);
+  void setGridVisible(bool visible);
+  void setRawScanSaved(bool enabled);
+  void setCameraColor(bool enabled);
+  void setFullResolution(bool enabled);
+  void setSmoothing(bool enabled);
+  void setDepthBleedingError(float value);
+  void setDepthFromMotion(bool enabled);
+  void setAppendMode(bool enabled);
+  void setUpstreamRelocalizationAccThr(float value);
+  void setDataRecorderMode(bool enabled);
+  void setMaxCloudDepth(float value);
+  void setMinCloudDepth(float value);
+  void setCloudDensityLevel(int value);
+  void setMeshAngleTolerance(float value);
+  void setMeshDecimationFactor(float value);
+  void setMeshTriangleSize(int value);
+  void setClusterRatio(float value);
+  void setMaxGainRadius(float value);
+  void setRenderingTextureDecimation(int value);
+  void setBackgroundColor(float gray);
+  void setDepthConfidence(int value);
+  void setExportPointCloudFormat(const std::string & format);
+  int setMappingParameter(const std::string & key, const std::string & value);
+  void setGPS(const rtabmap::GPS & gps);
+  void addEnvSensor(int type, float value);
+
+  void save(const std::string & databasePath);
+  bool recover(const std::string & from, const std::string & to);
+  void cancelProcessing();
+  bool exportMesh(
+		  float cloudVoxelSize,
+		  bool regenerateCloud,
+		  bool meshing,
+		  int textureSize,
+		  int textureCount,
+		  int normalK,
+		  bool optimized,
+		  float optimizedVoxelSize,
+		  int optimizedDepth,
+		  int optimizedMaxPolygons,
+		  float optimizedColorRadius,
+		  bool optimizedCleanWhitePolygons,
+		  int optimizedMinClusterSize,
+		  float optimizedMaxTextureDistance,
+		  int optimizedMinTextureClusterSize,
+      int textureVertexColorPolicy,
+		  bool blockRendering);
+  bool postExportation(bool visualize);
+  bool writeExportedMesh(const std::string & directory, const std::string & name);
+  int postProcessing(int approach);
+  void clearMeasures();
+  void showMeasures(bool x, bool y, bool z, bool custom);
+  void setMeasuringMode(int mode);
+  void addMeasureButtonClicked();
+  void teleportButtonClicked();
+  void removeMeasure();
+  void setMetricSystem(bool enabled);
+  void setMeasuringTextSize(float size);
+
+  void postOdometryEvent(
+		rtabmap::Transform pose,
+		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
+		float depth_fx, float depth_fy, float depth_cx, float depth_cy,
+		const rtabmap::Transform & rgbFrame,
+		const rtabmap::Transform & depthFrame,
+		double stamp,
+		double depthStamp,
+        const void * yPlane, const void * uPlane, const void * vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+        const void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+        const void * conf, int confLen, int confWidth, int confHeight, int confFormat,
+        const float * points, int pointsLen, int pointsChannels,
+		rtabmap::Transform viewMatrix, //view matrix
+        float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix
+        float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7); // tex coord
+
+ protected:
+  virtual bool handleEvent(UEvent * event);
+
+ private:
+  int updateMeshDecimation(int width, int height);
+  rtabmap::ParametersMap getRtabmapParameters();
+  void updateMeasuringState();
+  bool smoothMesh(int id, rtabmap::Mesh & mesh);
+  void gainCompensation(bool full = false);
+  std::vector<pcl::Vertices> filterOrganizedPolygons(const std::vector<pcl::Vertices> & polygons, int cloudSize) const;
+  std::vector<pcl::Vertices> filterPolygons(const std::vector<pcl::Vertices> & polygons, int cloudSize) const;
+
+ private:
+  int cameraDriver_;
+  rtabmap::CameraMobile * camera_;
+  rtabmap::SensorCaptureThread * sensorCaptureThread_;
+  rtabmap::RtabmapThread * rtabmapThread_;
+  rtabmap::Rtabmap * rtabmap_;
+  rtabmap::LogHandler * logHandler_;
+
+  bool odomCloudShown_;
+  bool graphOptimization_;
+  bool nodesFiltering_;
+  bool localizationMode_;
+  bool trajectoryMode_;
+  bool rawScanSaved_;
+  bool smoothing_;
+  float depthBleedingError_;
+  bool depthFromMotion_;
+  bool cameraColor_;
+  bool fullResolution_;
+  bool appendMode_;
+  bool useExternalLidar_;
+  float maxCloudDepth_;
+  float minCloudDepth_;
+  int cloudDensityLevel_;
+  int meshTrianglePix_;
+  float meshAngleToleranceDeg_;
+  float meshDecimationFactor_;
+  float clusterRatio_;
+  float maxGainRadius_;
+  int renderingTextureDecimation_;
+  float backgroundColor_;
+  unsigned char depthConfidence_;
+  float upstreamRelocalizationMaxAcc_;
+  std::string exportPointCloudFormat_;
+
+  rtabmap::ParametersMap mappingParameters_;
+
+  bool dataRecorderMode_;
+  bool clearSceneOnNextRender_;
+  bool openingDatabase_;
+  bool exporting_;
+  bool postProcessing_;
+  bool filterPolygonsOnNextRender_;
+  int gainCompensationOnNextRender_;
+  bool bilateralFilteringOnNextRender_;
+  bool takeScreenshotOnNextRender_;
+  bool cameraJustInitialized_;
+  int totalPoints_;
+  int totalPolygons_;
+  int lastDrawnCloudsCount_;
+  float renderingTime_;
+  double lastPostRenderEventTime_;
+  double lastPoseEventTime_;
+  std::map<std::string, float> bufferedStatsData_;
+
+  bool visualizingMesh_;
+  bool exportedMeshUpdated_;
+  pcl::TextureMesh::Ptr optTextureMesh_;
+  cv::Mat optTexture_;
+  rtabmap::Mesh optMesh_;
+  int optRefId_;
+  rtabmap::Transform * optRefPose_; // App crashes when loading native library if not dynamic
+  std::list<Measure> measures_; // In opengl frame
+  bool measuresUpdated_;
+  bool metricSystem_;
+  float measuringTextSize_;
+  float snapAxisThr_;
+  std::vector<cv::Vec3f> snapAxes_;
+  int measuringMode_;
+  bool addMeasureClicked_;
+  bool teleportClicked_;
+  bool removeMeasureClicked_;
+  std::vector<cv::Point3f> measuringTmpPts_; // In opengl frame
+  std::vector<cv::Point3f> measuringTmpNormals_; // In opengl frame
+  pcl::PointCloud<pcl::PointXYZRGB>::Ptr targetPoint_;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr quadSample_;
+  std::vector<pcl::Vertices> quadSamplePolygons_;
+  // main_scene_ includes all drawable object for visualizing Tango device's
+  // movement and point cloud.
+  Scene main_scene_;
+    
+    UTimer fpsTime_;
+
+	std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
+	std::list<rtabmap::SensorEvent> sensorEvents_;
+	std::list<rtabmap::Transform> poseEvents_;
+
+	rtabmap::Transform mapToOdom_;
+
+	boost::mutex cameraMutex_;
+	boost::mutex rtabmapMutex_;
+	boost::mutex meshesMutex_;
+	boost::mutex sensorMutex_;
+	boost::mutex poseMutex_;
+	boost::mutex renderingMutex_;
+
+	USemaphore screenshotReady_;
+
+	std::map<int, rtabmap::Mesh> createdMeshes_;
+	std::map<int, rtabmap::Transform> rawPoses_;
+
+	std::pair<rtabmap::RtabmapEventInit::Status, std::string> status_;
+
+	rtabmap::ProgressionStatus progressionStatus_;
+    
+#ifndef __ANDROID__
+    void * swiftClassPtr_;
+    void(*swiftInitCallback)(void *, int, const char *);
+    void(*swiftStatsUpdatedCallback)(void *,
+                             int, int, int, int,
+                             float,
+                             int, int, int, int, int ,int,
+                             float,
+                             int,
+                             float,
+                             int,
+                             float, float, float, float,
+                             int, int,
+                             float, float, float, float, float, float);
+    void(*swiftCameraInfoEventCallback)(void *, int, const char *, const char *);
+    
+#endif
+};
+
+#endif  // TANGO_POINT_CLOUD_POINT_CLOUD_APP_H_

rtabmap/app/android/jni/background_renderer.cc

@@ -0,0 +1,210 @@
+/*
+ * Copyright 2018 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+// This modules handles drawing the passthrough camera image into the OpenGL
+// scene.
+
+#include "background_renderer.h"
+
+#include <type_traits>
+
+namespace {
+
+const std::string kVertexShader =
+		"attribute vec4 a_Position;\n"
+		"attribute vec2 a_TexCoord;\n"
+
+		"varying vec2 v_TexCoord;\n"
+
+		"void main() {\n"
+		"   gl_Position = a_Position;\n"
+		"   v_TexCoord = a_TexCoord;\n"
+		"}\n";
+
+const std::string kFragmentShaderOES =
+	"#extension GL_OES_EGL_image_external : require\n"
+	"precision mediump float;\n"
+	"varying vec2 v_TexCoord;\n"
+	"uniform samplerExternalOES sTexture;\n"
+    "uniform bool uRedUnknown;\n"
+	"void main() {\n"
+	"    vec4 sample = texture2D(sTexture, v_TexCoord);\n"
+    "    float grey = 0.21 * sample.r + 0.71 * sample.g + 0.07 * sample.b;\n"
+    "    gl_FragColor = vec4(grey, uRedUnknown?0.0:grey, uRedUnknown?0.0:grey, 0.5);\n"
+	"}\n";
+
+const std::string kFragmentShaderBlendingOES =
+    "#extension GL_OES_EGL_image_external : require\n"
+    "precision mediump float;\n"
+    "varying vec2 v_TexCoord;\n"
+    "uniform samplerExternalOES sTexture;\n"
+	"uniform sampler2D uDepthTexture;\n"
+	"uniform vec2 uScreenScale;\n"
+    "uniform bool uRedUnknown;\n"
+    "void main() {\n"
+    "    vec4 sample = texture2D(sTexture, v_TexCoord);\n"
+    "    vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
+    "    vec4 depthPacked = texture2D(uDepthTexture, coord);\n"
+    "    float depth = dot(depthPacked, 1./vec4(1.,255.,65025.,16581375.));\n"
+    "    if(depth > 0.0)\n"
+    "        gl_FragColor = vec4(sample.r, sample.g, sample.b, 0.5);\n"
+    "    else {\n"
+    "        float grey = 0.21 * sample.r + 0.71 * sample.g + 0.07 * sample.b;\n"
+    "        gl_FragColor = vec4(grey, uRedUnknown?0.0:grey, uRedUnknown?0.0:grey, 0.5);\n"
+    "    }\n"
+    "}\n";
+
+const std::string kFragmentShader =
+	"precision mediump float;\n"
+	"varying vec2 v_TexCoord;\n"
+    "uniform sampler2D sTexture;\n"
+    "uniform bool uRedUnknown;\n"
+	"void main() {\n"
+	"    vec4 sample = texture2D(sTexture, v_TexCoord);\n"
+	"    float grey = 0.21 * sample.r + 0.71 * sample.g + 0.07 * sample.b;\n"
+	"    gl_FragColor = vec4(grey, uRedUnknown?0.0:grey, uRedUnknown?0.0:grey, 0.5);\n"
+	"}\n";
+
+const std::string kFragmentShaderBlending =
+    "precision mediump float;\n"
+    "varying vec2 v_TexCoord;\n"
+    "uniform sampler2D sTexture;\n"
+    "uniform sampler2D uDepthTexture;\n"
+    "uniform vec2 uScreenScale;\n"
+    "uniform bool uRedUnknown;\n"
+    "void main() {\n"
+    "    vec4 sample = texture2D(sTexture, v_TexCoord);\n"
+    "    vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
+    "    vec4 depthPacked = texture2D(uDepthTexture, coord);\n"
+    "    float depth = dot(depthPacked, 1./vec4(1.,255.,65025.,16581375.));\n"
+    "    if(depth > 0.0)\n"
+    "        gl_FragColor = vec4(sample.r, sample.g, sample.b, 0.5);\n"
+    "    else {\n"
+    "        float grey = 0.21 * sample.r + 0.71 * sample.g + 0.07 * sample.b;\n"
+    "        gl_FragColor = vec4(grey, uRedUnknown?0.0:grey, uRedUnknown?0.0:grey, 0.5);\n"
+    "    }\n"
+    "}\n";
+
+/* To debug depth texture
+ const std::string kFragmentShader =
+     "precision mediump float;\n"
+     "varying vec2 v_TexCoord;\n"
+     "uniform sampler2D sTexture;\n"
+
+     "void main() {\n"
+     "  float uNearZ = 0.2;\n"
+     "  float uFarZ = 1000.0;\n"
+     "  float depth = texture2D(sTexture, v_TexCoord).r;\n"
+     "  float num =  (2.0 * uNearZ * uFarZ);\n"
+     "  float diff = (uFarZ - uNearZ);\n"
+     "  float add = (uFarZ + uNearZ);\n"
+     "  float ndcDepth = depth * 2.0 - 1.0;\n" // Back to NDC
+     "  float linearDepth = num / (add - ndcDepth * diff);\n" // inverse projection matrix
+     "  float grey = linearDepth/3.0;\n"
+     "  gl_FragColor = vec4(grey, grey, grey, 0.5);\n"
+     "}\n";
+ */
+
+}  // namespace
+
+std::vector<GLuint> BackgroundRenderer::shaderPrograms_;
+
+BackgroundRenderer::~BackgroundRenderer()
+{
+	for(unsigned int i=0; i<shaderPrograms_.size(); ++i)
+	{
+		glDeleteShader(shaderPrograms_[i]);
+	}
+	shaderPrograms_.clear();
+}
+
+void BackgroundRenderer::InitializeGlContent(GLuint textureId, bool oes)
+{
+	LOGI("textureId=%d", textureId);
+
+  texture_id_ = textureId;
+#ifdef __ANDROID__
+  oes_ = oes;
+#endif
+
+    if(shaderPrograms_.empty())
+    {
+        shaderPrograms_.resize(2,0);
+        shaderPrograms_[0] = tango_gl::util::CreateProgram(
+              kVertexShader.c_str(),
+              oes_?kFragmentShaderOES.c_str():kFragmentShader.c_str());
+        UASSERT(shaderPrograms_[0]!=0);
+        shaderPrograms_[1] = tango_gl::util::CreateProgram(
+              kVertexShader.c_str(),
+              oes_?kFragmentShaderBlendingOES.c_str():kFragmentShaderBlending.c_str());
+        UASSERT(shaderPrograms_[1]!=0);
+    }
+}
+
+void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & depthTexture, int screenWidth, int screenHeight, bool redUnknown) {
+  static_assert(std::extent<decltype(BackgroundRenderer_kVerticesDevice)>::value == kNumVertices * 2, "Incorrect kVertices length");
+
+  GLuint program = shaderPrograms_[depthTexture>0?1:0];
+    
+  glUseProgram(program);
+  glDepthMask(GL_FALSE);
+  glEnable (GL_BLEND);
+
+  glActiveTexture(GL_TEXTURE0);
+#ifdef __ANDROID__
+  if(oes_)
+    glBindTexture(GL_TEXTURE_EXTERNAL_OES, texture_id_);
+  else
+#endif
+    glBindTexture(GL_TEXTURE_2D, texture_id_);
+
+  if(depthTexture>0)
+  {
+    // Texture activate unit 1
+    glActiveTexture(GL_TEXTURE1);
+    // Bind the texture to this unit.
+    glBindTexture(GL_TEXTURE_2D, depthTexture);
+    // Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 1.
+    GLuint depth_texture_handle = glGetUniformLocation(program, "uDepthTexture");
+    glUniform1i(depth_texture_handle, 1);
+      
+    GLuint screenScale_handle = glGetUniformLocation(program, "uScreenScale");
+    glUniform2f(screenScale_handle, 1.0f/(float)screenWidth, 1.0f/(float)screenHeight);
+  }
+    
+  GLuint screenScale_handle = glGetUniformLocation(program, "uRedUnknown");
+  glUniform1i(screenScale_handle, redUnknown);
+
+  GLuint attributeVertices = glGetAttribLocation(program, "a_Position");
+  GLuint attributeUvs = glGetAttribLocation(program, "a_TexCoord");
+
+  glVertexAttribPointer(attributeVertices, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVerticesDevice);
+  glVertexAttribPointer(attributeUvs, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs?transformed_uvs:BackgroundRenderer_kTexCoord);
+
+  glEnableVertexAttribArray(attributeVertices);
+  glEnableVertexAttribArray(attributeUvs);
+
+  glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
+
+  glDisableVertexAttribArray(attributeVertices);
+  glDisableVertexAttribArray(attributeUvs);
+
+  glUseProgram(0);
+  glDepthMask(GL_TRUE);
+  glDisable (GL_BLEND);
+  tango_gl::util::CheckGlError("BackgroundRenderer::Draw() error");
+}
+

rtabmap/app/android/jni/background_renderer.h

@@ -0,0 +1,76 @@
+/*
+ * Copyright 2018 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
+#define C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
+
+#ifdef __ANDROID__
+#include <GLES2/gl2.h>
+#include <GLES2/gl2ext.h>
+#else // __APPLE__
+#include <OpenGLES/ES2/gl.h>
+#include <OpenGLES/ES2/glext.h>
+#endif
+#include <cstdlib>
+
+#include "util.h"
+
+static const GLfloat BackgroundRenderer_kVerticesDevice[] = {
+	-1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f,
+};
+//static const GLfloat BackgroundRenderer_kVerticesView[] = {
+//	0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f,
+//};
+static const GLfloat BackgroundRenderer_kVerticesView[] = {
+	0.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f,
+};
+static const GLfloat BackgroundRenderer_kTexCoord[] = {
+   1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
+};
+
+//android phone
+//11 10 01 00 // portrait
+//01 11 00 10 // left
+//10 00 11 01 // right
+//00 01 10 11 // down
+
+
+// This class renders the passthrough camera image into the OpenGL frame.
+class BackgroundRenderer {
+public:
+	// Positions of the quad vertices in clip space (X, Y).
+
+	static constexpr int kNumVertices = 4;
+
+ public:
+  BackgroundRenderer() = default;
+  ~BackgroundRenderer();
+
+  // Sets up OpenGL state.  Must be called on the OpenGL thread and before any
+  // other methods below.
+  void InitializeGlContent(GLuint textureId, bool oes);
+
+  // Draws the background image.  This methods must be called for every ArFrame
+  // returned by ArSession_update() to catch display geometry change events.
+  void Draw(const float * transformed_uvs, const GLuint & depthTexture, int screenWidth, int screenHeight, bool redUnknown);
+
+private:
+  static std::vector<GLuint> shaderPrograms_;
+  GLuint texture_id_;
+  bool oes_ = false;
+};
+
+#endif  // C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_

rtabmap/app/android/jni/bounding_box_drawable.h

@@ -0,0 +1,131 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+ * 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.
+ * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS 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 BOUNDING_BOX_DRAWABLE_H_
+#define BOUNDING_BOX_DRAWABLE_H_
+
+#include "tango-gl/line.h"
+
+class BoundingBoxDrawable : public tango_gl::Line
+{
+public:
+	BoundingBoxDrawable() :
+		Line(3.0f, GL_LINES)
+	{
+		vec_vertices_.resize(24);
+	}
+
+	void updateVertices(const pcl::PointXYZ & min, const pcl::PointXYZ & max)
+	{
+		int index = 0;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+	}
+
+};
+#endif  // TANGO_GL_LINE_H_

rtabmap/app/android/jni/graph_drawable.cpp

@@ -0,0 +1,164 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * 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.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+*/
+
+#include <sstream>
+
+#include "graph_drawable.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "util.h"
+
+#ifdef __ANDROID__
+#include <GLES2/gl2.h>
+#else //__APPLE__
+#include <OpenGLES/ES2/gl.h>
+#endif
+
+GraphDrawable::GraphDrawable(
+		GLuint shaderProgram,
+		const std::map<int, rtabmap::Transform> & poses,
+		const std::multimap<int, rtabmap::Link> & links) :
+		vertex_buffers_(0),
+		pose_(1.0f),
+		visible_(true),
+		lineWidth_(3.0f),
+		shader_program_(shaderProgram)
+{
+	UASSERT(!poses.empty());
+
+	glGenBuffers(1, &vertex_buffers_);
+
+	if(vertex_buffers_)
+	{
+		LOGI("Creating vertex buffer %d", vertex_buffers_);
+		std::vector<float> vertices = std::vector<float>(poses.size()*3);
+		int i=0;
+		std::map<int, int> idsToIndices;
+		for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+		{
+			vertices[i*3] = iter->second.x();
+			vertices[i*3+1] = iter->second.y();
+			vertices[i*3+2] = iter->second.z();
+			idsToIndices.insert(std::make_pair(iter->first, i));
+			++i;
+		}
+
+		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+		glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
+		glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+		GLint error = glGetError();
+		if(error != GL_NO_ERROR)
+		{
+			LOGI("OpenGL: Could not allocate point cloud (0x%x)\n", error);
+			vertex_buffers_ = 0;
+		}
+		else if(links.size())
+		{
+			neighborIndices_.resize(links.size() * 2);
+			loopClosureIndices_.resize(links.size() * 2);
+			int oiNeighbors = 0;
+			int oiLoopClosures = 0;
+			for(std::multimap<int, rtabmap::Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+			{
+				std::map<int, int>::const_iterator jterFrom = idsToIndices.find(iter->second.from());
+				std::map<int, int>::const_iterator jterTo = idsToIndices.find(iter->second.to());
+				if(jterFrom != idsToIndices.end() && jterTo != idsToIndices.end())
+				{
+					if(iter->second.type() == rtabmap::Link::kNeighbor)
+					{
+						neighborIndices_[oiNeighbors++] = (unsigned short)jterFrom->second;
+						neighborIndices_[oiNeighbors++] = (unsigned short)jterTo->second;
+					}
+					else
+					{
+						loopClosureIndices_[oiLoopClosures++] = (unsigned short)jterFrom->second;
+						loopClosureIndices_[oiLoopClosures++] = (unsigned short)jterTo->second;
+					}
+				}
+			}
+			neighborIndices_.resize(oiNeighbors);
+			loopClosureIndices_.resize(oiLoopClosures);
+		}
+	}
+}
+
+GraphDrawable::~GraphDrawable()
+{
+	LOGI("Freeing cloud buffer %d", vertex_buffers_);
+	if (vertex_buffers_)
+	{
+		glDeleteBuffers(1, &vertex_buffers_);
+		tango_gl::util::CheckGlError("GraphDrawable::~GraphDrawable()");
+		vertex_buffers_ = 0;
+	}
+}
+
+void GraphDrawable::setPose(const rtabmap::Transform & pose)
+{
+	UASSERT(!pose.isNull());
+
+	pose_ = glmFromTransform(pose);
+}
+
+void GraphDrawable::Render(const glm::mat4 & projectionMatrix, const glm::mat4 & viewMatrix) {
+
+	if(vertex_buffers_ && (neighborIndices_.size() || loopClosureIndices_.size()) && visible_)
+	{
+		glUseProgram(shader_program_);
+		glLineWidth(lineWidth_);
+
+		GLuint mvp_handle_ = glGetUniformLocation(shader_program_, "mvp");
+		glm::mat4 mvp_mat = projectionMatrix * viewMatrix * pose_;
+		glUniformMatrix4fv(mvp_handle_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
+
+		GLuint color_handle = glGetUniformLocation(shader_program_, "color");
+
+		GLint attribute_vertex = glGetAttribLocation(shader_program_, "vertex");
+
+		glEnableVertexAttribArray(attribute_vertex);
+		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+		glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 3*sizeof(GLfloat), 0);
+
+		if(neighborIndices_.size())
+		{
+			glUniform3f(color_handle, 1.0f, 0.0f, 0.0f); // blue for neighbors
+			glDrawElements(GL_LINES, neighborIndices_.size(), GL_UNSIGNED_SHORT, neighborIndices_.data());
+		}
+		if(loopClosureIndices_.size())
+		{
+			glUniform3f(color_handle, 0.0f, 0.0f, 1.0f); // red for loop closures
+			glDrawElements(GL_LINES, loopClosureIndices_.size(), GL_UNSIGNED_SHORT, loopClosureIndices_.data());
+		}
+
+		glDisableVertexAttribArray(0);
+		glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+		glUseProgram(0);
+		tango_gl::util::CheckGlError("GraphDrawable::Render()");
+	}
+}
+

rtabmap/app/android/jni/graph_drawable.h

@@ -0,0 +1,68 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * 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.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS 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 GRAPH_DRAWABLE_H_
+#define GRAPH_DRAWABLE_H_
+
+#ifdef __ANDROID__
+#include <jni.h>
+#endif
+#include <tango-gl/util.h>
+#include <vector>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Link.h>
+#include <pcl/Vertices.h>
+#include "util.h"
+
+class GraphDrawable {
+ public:
+	GraphDrawable(
+		  GLuint shaderProgram,
+		  const std::map<int, rtabmap::Transform> & poses,
+		  const std::multimap<int, rtabmap::Link> & links);
+  virtual ~GraphDrawable();
+
+  void setPose(const rtabmap::Transform & mapToOdom);
+  void setVisible(bool visible) {visible_=visible;}
+
+  void Render(const glm::mat4 & projectionMatrix, const glm::mat4 & viewMatrix);
+
+ private:
+  // Vertex buffer of the point cloud geometry.
+  GLuint vertex_buffers_;
+  std::vector<GLushort> neighborIndices_;
+  std::vector<GLushort> loopClosureIndices_;
+  glm::mat4 pose_;
+  bool visible_;
+  float lineWidth_;
+
+  GLuint shader_program_;
+};
+
+#endif  // GRAPH_DRAWABLE_H_

rtabmap/app/android/jni/jni_interface.cpp

@@ -0,0 +1,981 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * 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.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+*/
+
+#define GLM_FORCE_RADIANS
+
+#include <jni.h>
+#include <RTABMapApp.h>
+#include <scene.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void GetJStringContent(JNIEnv *AEnv, jstring AStr, std::string &ARes) {
+  if (!AStr) {
+    ARes.clear();
+    return;
+  }
+
+  const char *s = AEnv->GetStringUTFChars(AStr,NULL);
+  ARes=s;
+  AEnv->ReleaseStringUTFChars(AStr,s);
+}
+
+inline jlong jptr(RTABMapApp *native_computer_vision_application) {
+  return reinterpret_cast<intptr_t>(native_computer_vision_application);
+}
+
+inline RTABMapApp *native(jlong ptr) {
+  return reinterpret_cast<RTABMapApp *>(ptr);
+}
+
+JNIEXPORT jlong JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_createNativeApplication(
+    JNIEnv* env, jclass, jobject activity)
+{
+	return jptr(new RTABMapApp(env, activity));
+}
+
+JNIEXPORT void Java_com_introlab_rtabmap_RTABMapLib_destroyNativeApplication(
+	JNIEnv *, jclass, jlong native_application) {
+	if(native_application)
+	{
+		delete native(native_application);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setScreenRotation(
+    JNIEnv* env, jclass, jlong native_application, int displayRotation, int cameraRotation)
+{
+	if(native_application)
+	{
+		return native(native_application)->setScreenRotation(displayRotation, cameraRotation);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+
+JNIEXPORT int JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_openDatabase(
+    JNIEnv* env, jclass, jlong native_application, jstring databasePath, bool databaseInMemory, bool optimize, bool clearDatabase)
+{
+	std::string databasePathC;
+	GetJStringContent(env,databasePath,databasePathC);
+	if(native_application)
+	{
+		return native(native_application)->openDatabase(databasePathC, databaseInMemory, optimize, clearDatabase);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return -1;
+	}
+}
+
+JNIEXPORT bool JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_recover(
+    JNIEnv* env, jclass, jlong native_application, jstring from, jstring to)
+{
+	if(native_application)
+	{
+		std::string toC;
+		GetJStringContent(env,to,toC);
+		std::string fromC;
+		GetJStringContent(env,from,fromC);
+		return native(native_application)->recover(fromC, toC);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return -1;
+	}
+}
+
+JNIEXPORT bool JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_isBuiltWith(
+		JNIEnv* env, jclass, jlong native_application, int cameraDriver) {
+	if(native_application)
+	{
+		return native(native_application)->isBuiltWith(cameraDriver);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return false;
+	}
+}
+
+JNIEXPORT bool JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_startCamera(
+		JNIEnv* env, jclass, jlong native_application, jobject iBinder, jobject context, jobject activity, int driver) {
+	if(native_application)
+	{
+		return native(native_application)->startCamera(env, iBinder, context, activity, driver);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return false;
+	}
+}
+
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_initGlContent(
+    JNIEnv*, jclass, jlong native_application) {
+	if(native_application)
+	{
+		native(native_application)->InitializeGLContent();
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setupGraphic(
+    JNIEnv*, jclass, jlong native_application, jint width, jint height) {
+	if(native_application)
+	{
+		native(native_application)->SetViewPort(width, height);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+
+JNIEXPORT int JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_render(
+    JNIEnv*, jclass, jlong native_application) {
+	if(native_application)
+	{
+		return native(native_application)->Render();
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return -1;
+	}
+}
+
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_stopCamera(
+    JNIEnv*, jclass, jlong native_application) {
+	if(native_application)
+	{
+		native(native_application)->stopCamera();
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setCamera(
+    JNIEnv*, jclass, jlong native_application, int camera_index) {
+	if(native_application)
+	{
+		using namespace tango_gl;
+		GestureCamera::CameraType cam_type =
+		  static_cast<GestureCamera::CameraType>(camera_index);
+		native(native_application)->SetCameraType(cam_type);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_onTouchEvent(
+    JNIEnv*, jclass, jlong native_application, int touch_count, int event, float x0, float y0, float x1,
+    float y1) {
+	if(native_application)
+	{
+		using namespace tango_gl;
+		GestureCamera::TouchEvent touch_event =
+		  static_cast<GestureCamera::TouchEvent>(event);
+		native(native_application)->OnTouchEvent(touch_count, touch_event, x0, y0, x1, y1);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setPausedMapping(
+		JNIEnv*, jclass, jlong native_application, bool paused)
+{
+	if(native_application)
+	{
+		return native(native_application)->setPausedMapping(paused);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setOnlineBlending(
+		JNIEnv*, jclass, jlong native_application, bool enabled)
+{
+	if(native_application)
+	{
+		return native(native_application)->setOnlineBlending(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMapCloudShown(
+		JNIEnv*, jclass, jlong native_application, bool shown)
+{
+	if(native_application)
+	{
+		return native(native_application)->setMapCloudShown(shown);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setOdomCloudShown(
+		JNIEnv*, jclass, jlong native_application, bool shown)
+{
+	if(native_application)
+	{
+		return native(native_application)->setOdomCloudShown(shown);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMeshRendering(
+		JNIEnv*, jclass, jlong native_application, bool enabled, bool withTexture)
+{
+	if(native_application)
+	{
+		return native(native_application)->setMeshRendering(enabled, withTexture);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setPointSize(
+		JNIEnv*, jclass, jlong native_application, float value)
+{
+	if(native_application)
+	{
+		return native(native_application)->setPointSize(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setFOV(
+		JNIEnv*, jclass, jlong native_application, float fov)
+{
+	if(native_application)
+	{
+		return native(native_application)->setFOV(fov);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setOrthoCropFactor(
+		JNIEnv*, jclass, jlong native_application, float value)
+{
+	if(native_application)
+	{
+		return native(native_application)->setOrthoCropFactor(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setGridRotation(
+		JNIEnv*, jclass, jlong native_application, float value)
+{
+	if(native_application)
+	{
+		return native(native_application)->setGridRotation(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setLighting(
+		JNIEnv*, jclass, jlong native_application, bool enabled)
+{
+	if(native_application)
+	{
+		return native(native_application)->setLighting(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setBackfaceCulling(
+		JNIEnv*, jclass, jlong native_application, bool enabled)
+{
+	if(native_application)
+	{
+		return native(native_application)->setBackfaceCulling(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setWireframe(
+		JNIEnv*, jclass, jlong native_application, bool enabled)
+{
+	if(native_application)
+	{
+		return native(native_application)->setWireframe(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setLocalizationMode(
+		JNIEnv*, jclass, jlong native_application, bool enabled)
+{
+	if(native_application)
+	{
+		return native(native_application)->setLocalizationMode(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setTrajectoryMode(
+		JNIEnv*, jclass, jlong native_application, bool enabled)
+{
+	if(native_application)
+	{
+		return native(native_application)->setTrajectoryMode(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setGraphOptimization(
+		JNIEnv*, jclass, jlong native_application, bool enabled)
+{
+	if(native_application)
+	{
+		return native(native_application)->setGraphOptimization(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setNodesFiltering(
+		JNIEnv*, jclass, jlong native_application, bool enabled)
+{
+	if(native_application)
+	{
+		return native(native_application)->setNodesFiltering(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setGraphVisible(
+		JNIEnv*, jclass, jlong native_application, bool visible)
+{
+	if(native_application)
+	{
+		return native(native_application)->setGraphVisible(visible);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setGridVisible(
+		JNIEnv*, jclass, jlong native_application, bool visible)
+{
+	if(native_application)
+	{
+		return native(native_application)->setGridVisible(visible);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setRawScanSaved(
+		JNIEnv*, jclass, jlong native_application, bool enabled)
+{
+	if(native_application)
+	{
+		return native(native_application)->setRawScanSaved(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setFullResolution(
+		JNIEnv*, jclass, jlong native_application, bool enabled)
+{
+	if(native_application)
+	{
+		return native(native_application)->setFullResolution(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setSmoothing(
+		JNIEnv*, jclass, jlong native_application, bool enabled)
+{
+	if(native_application)
+	{
+		return native(native_application)->setSmoothing(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setDepthBleedingError(
+		JNIEnv*, jclass, jlong native_application, float value)
+{
+	if(native_application)
+	{
+		return native(native_application)->setDepthBleedingError(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setDepthFromMotion(
+		JNIEnv*, jclass, jlong native_application, bool enabled)
+{
+	if(native_application)
+	{
+		return native(native_application)->setDepthFromMotion(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setCameraColor(
+		JNIEnv*, jclass, jlong native_application, bool enabled)
+{
+	if(native_application)
+	{
+		return native(native_application)->setCameraColor(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setAppendMode(
+		JNIEnv*, jclass, jlong native_application, bool enabled)
+{
+	if(native_application)
+	{
+		return native(native_application)->setAppendMode(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setUpstreamRelocalizationAccThr(
+		JNIEnv*, jclass, jlong native_application, float value)
+{
+	if(native_application)
+	{
+		return native(native_application)->setUpstreamRelocalizationAccThr(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setDataRecorderMode(
+		JNIEnv*, jclass, jlong native_application, bool enabled)
+{
+	if(native_application)
+	{
+		return native(native_application)->setDataRecorderMode(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMaxCloudDepth(
+		JNIEnv*, jclass, jlong native_application, float value)
+{
+	if(native_application)
+	{
+		return native(native_application)->setMaxCloudDepth(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMinCloudDepth(
+		JNIEnv*, jclass, jlong native_application, float value)
+{
+	if(native_application)
+	{
+		return native(native_application)->setMinCloudDepth(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setCloudDensityLevel(
+		JNIEnv*, jclass, jlong native_application, int value)
+{
+	if(native_application)
+	{
+		return native(native_application)->setCloudDensityLevel(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMeshAngleTolerance(
+		JNIEnv*, jclass, jlong native_application, float value)
+{
+	if(native_application)
+	{
+		return native(native_application)->setMeshAngleTolerance(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMeshTriangleSize(
+		JNIEnv*, jclass, jlong native_application, int value)
+{
+	if(native_application)
+	{
+		return native(native_application)->setMeshTriangleSize(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setClusterRatio(
+		JNIEnv*, jclass, jlong native_application, float value)
+{
+	if(native_application)
+	{
+		return native(native_application)->setClusterRatio(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMaxGainRadius(
+		JNIEnv*, jclass, jlong native_application, float value)
+{
+	if(native_application)
+	{
+		return native(native_application)->setMaxGainRadius(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setRenderingTextureDecimation(
+		JNIEnv*, jclass, jlong native_application, int value)
+{
+	if(native_application)
+	{
+		return native(native_application)->setRenderingTextureDecimation(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setBackgroundColor(
+		JNIEnv*, jclass, jlong native_application, float value)
+{
+	if(native_application)
+	{
+		return native(native_application)->setBackgroundColor(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT jint JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMappingParameter(
+    JNIEnv* env, jclass, jlong native_application, jstring key, jstring value)
+{
+	if(native_application)
+	{
+		std::string keyC, valueC;
+		GetJStringContent(env,key,keyC);
+		GetJStringContent(env,value,valueC);
+		return native(native_application)->setMappingParameter(keyC, valueC);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return -1;
+	}
+}
+
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setGPS(
+		JNIEnv*, jclass, jlong native_application,
+		double stamp,
+		double longitude,
+		double latitude,
+		double altitude,
+		double accuracy,
+		double bearing)
+{
+	if(native_application)
+	{
+		return native(native_application)->setGPS(rtabmap::GPS(stamp,
+					longitude,
+					latitude,
+					altitude,
+					accuracy,
+					bearing));
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_addEnvSensor(
+		JNIEnv*, jclass, jlong native_application,
+		int type,
+		float value)
+{
+	if(native_application)
+	{
+		return native(native_application)->addEnvSensor(type, value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_save(
+		JNIEnv* env, jclass, jlong native_application, jstring databasePath)
+{
+	if(native_application)
+	{
+		std::string databasePathC;
+		GetJStringContent(env,databasePath,databasePathC);
+		return native(native_application)->save(databasePathC);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_cancelProcessing(
+		JNIEnv* env, jclass, jlong native_application)
+{
+	if(native_application)
+	{
+		return native(native_application)->cancelProcessing();
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+
+JNIEXPORT bool JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
+		JNIEnv* env, jclass, jlong native_application,
+		float cloudVoxelSize,
+		bool regenerateCloud,
+		bool meshing,
+		int textureSize,
+		int textureCount,
+		int normalK,
+		bool optimized,
+		float optimizedVoxelSize,
+		int optimizedDepth,
+		int optimizedMaxPolygons,
+		float optimizedColorRadius,
+		bool optimizedCleanWhitePolygons,
+		int optimizedMinClusterSize,
+		float optimizedMaxTextureDistance,
+		int optimizedMinTextureClusterSize,
+		bool blockRendering)
+{
+	if(native_application)
+	{
+		return native(native_application)->exportMesh(
+					cloudVoxelSize,
+					regenerateCloud,
+					meshing,
+					textureSize,
+					textureCount,
+					normalK,
+					optimized,
+					optimizedVoxelSize,
+					optimizedDepth,
+					optimizedMaxPolygons,
+					optimizedColorRadius,
+					optimizedCleanWhitePolygons,
+					optimizedMinClusterSize,
+					optimizedMaxTextureDistance,
+					optimizedMinTextureClusterSize,
+					0,
+					blockRendering);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return false;
+	}
+}
+
+JNIEXPORT bool JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_postExportation(
+		JNIEnv* env, jclass, jlong native_application, bool visualize)
+{
+	if(native_application)
+	{
+		return native(native_application)->postExportation(visualize);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return false;
+	}
+}
+
+JNIEXPORT bool JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_writeExportedMesh(
+		JNIEnv* env, jclass, jlong native_application, jstring directory, jstring name)
+{
+	if(native_application)
+	{
+		std::string directoryC;
+		GetJStringContent(env,directory,directoryC);
+		std::string nameC;
+		GetJStringContent(env,name,nameC);
+		return native(native_application)->writeExportedMesh(directoryC, nameC);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return false;
+	}
+}
+
+
+JNIEXPORT int JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_postProcessing(
+		JNIEnv* env, jclass, jlong native_application, int approach)
+{
+	if(native_application)
+	{
+		return native(native_application)->postProcessing(approach);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return -1;
+	}
+}
+
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_postOdometryEvent(
+		JNIEnv* env, jclass, jlong native_application,
+		float x, float y, float z, float qx, float qy, float qz, float qw,
+		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
+		float rgbFrameX, float rgbFrameY, float rgbFrameZ, float rgbFrameQX, float rgbFrameQY, float rgbFrameQZ, float rgbFrameQW,
+		double stamp,
+		jobject yPlane, jobject uPlane, jobject vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+		jobject points, int pointsLen,
+		float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw, //view matrix
+		float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix
+		float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7) // tex coord
+{
+	if(native_application)
+	{
+		void *yPtr = env->GetDirectBufferAddress(yPlane);
+		void *uPtr = env->GetDirectBufferAddress(uPlane);
+		void *vPtr = env->GetDirectBufferAddress(vPlane);
+		float *pointsPtr = (float *)env->GetDirectBufferAddress(points);
+		native(native_application)->postOdometryEvent(
+				rtabmap::Transform(x,y,z,qx,qy,qz,qw),
+				rgb_fx,rgb_fy,rgb_cx,rgb_cy,
+				0,0,0,0,
+				rtabmap::Transform(rgbFrameX, rgbFrameY, rgbFrameZ, rgbFrameQX, rgbFrameQY, rgbFrameQZ, rgbFrameQW),
+				rtabmap::Transform(),
+				stamp,
+				0,
+				yPtr, uPtr, vPtr, yPlaneLen, rgbWidth, rgbHeight, rgbFormat,
+				0,0,0,0,0, //depth
+				0,0,0,0,0, //conf
+				pointsPtr, pointsLen, 4,
+				rtabmap::Transform(vx, vy, vz, vqx, vqy, vqz, vqw),
+				p00, p11, p02, p12, p22, p32, p23,
+				t0, t1, t2, t3, t4, t5, t6, t7);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+		return;
+	}
+}
+
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_postOdometryEventDepth(
+		JNIEnv* env, jclass, jlong native_application,
+		float x, float y, float z, float qx, float qy, float qz, float qw,
+		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
+		float depth_fx, float depth_fy, float depth_cx, float depth_cy,
+		float rgbFrameX, float rgbFrameY, float rgbFrameZ, float rgbFrameQX, float rgbFrameQY, float rgbFrameQZ, float rgbFrameQW,
+		float depthFrameX, float depthFrameY, float depthFrameZ, float depthFrameQX, float depthFrameQY, float depthFrameQZ, float depthFrameQW,
+		double rgbStamp,
+		double depthStamp,
+		jobject yPlane, jobject uPlane, jobject vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+ 		jobject depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+ 		jobject points, int pointsLen,
+		float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw, //view matrix
+		float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix
+		float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7) // tex coord)
+ {
+		void *yPtr = env->GetDirectBufferAddress(yPlane);
+		void *uPtr = env->GetDirectBufferAddress(uPlane);
+		void *vPtr = env->GetDirectBufferAddress(vPlane);
+ 		void *depthPtr = env->GetDirectBufferAddress(depth);
+ 		float *pointsPtr = (float *)env->GetDirectBufferAddress(points);
+ 		native(native_application)->postOdometryEvent(
+				rtabmap::Transform(x,y,z,qx,qy,qz,qw),
+				rgb_fx,rgb_fy,rgb_cx,rgb_cy,
+				depth_fx,depth_fy,depth_cx,depth_cy,
+				rtabmap::Transform(rgbFrameX, rgbFrameY, rgbFrameZ, rgbFrameQX, rgbFrameQY, rgbFrameQZ, rgbFrameQW),
+				rtabmap::Transform(depthFrameX, depthFrameY, depthFrameZ, depthFrameQX, depthFrameQY, depthFrameQZ, depthFrameQW),
+				rgbStamp,
+				depthStamp,
+ 				yPtr, uPtr, vPtr, yPlaneLen, rgbWidth, rgbHeight, rgbFormat,
+ 				depthPtr, depthLen, depthWidth, depthHeight, depthFormat,
+				0,0,0,0,0, // conf
+ 				pointsPtr, pointsLen, 4,
+				rtabmap::Transform(vx, vy, vz, vqx, vqy, vqz, vqw),
+				p00, p11, p02, p12, p22, p32, p23,
+				t0, t1, t2, t3, t4, t5, t6, t7);
+ }
+
+
+#ifdef __cplusplus
+}
+#endif

rtabmap/app/android/jni/point_cloud_drawable.h

@@ -0,0 +1,141 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * 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.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS 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 TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_
+#define TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_
+
+#ifdef __ANDROID__
+#include <jni.h>
+#endif
+
+#include <tango-gl/util.h>
+#include <vector>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <rtabmap/core/Transform.h>
+#include <pcl/Vertices.h>
+#include "util.h"
+
+// PointCloudDrawable is responsible for the point cloud rendering.
+class PointCloudDrawable {
+public:
+	static void createShaderPrograms();
+	static void releaseShaderPrograms();
+
+private:
+	static std::vector<GLuint> shaderPrograms_;
+
+ public:
+  PointCloudDrawable(
+  		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		  const pcl::IndicesPtr & indices,
+		  float gainR = 1.0f,
+		  float gainG = 1.0f,
+		  float gainB = 1.0f);
+  PointCloudDrawable(
+    	  const rtabmap::Mesh & mesh,
+		  bool createWireframe = false);
+  virtual ~PointCloudDrawable();
+
+  void updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes = std::vector<pcl::Vertices>(), bool createWireframe = false);
+  void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices);
+  void updateMesh(const rtabmap::Mesh & mesh, bool createWireframe = false);
+  void setPose(const rtabmap::Transform & pose);
+  void setVisible(bool visible) {visible_=visible;}
+  void setGains(float gainR, float gainG, float gainB) {gainR_ = gainR; gainG_ = gainG; gainB_ = gainB;}
+  rtabmap::Transform getPose() const {return pose_;}
+  const glm::mat4 & getPoseGl() const {return poseGl_;}
+  bool isVisible() const {return visible_;}
+  bool hasMesh() const {return index_buffers_[0] != 0;}
+  bool hasTexture() const {return texture_ != 0;}
+  float getMinHeight() const {return minHeight_;}
+  const pcl::PointXYZ & aabbMinModel() const {return aabbMinModel_;}
+  const pcl::PointXYZ & aabbMaxModel() const {return aabbMaxModel_;}
+  const pcl::PointXYZ & aabbMinWorld() const {return aabbMinWorld_;}
+  const pcl::PointXYZ & aabbMaxWorld() const {return aabbMaxWorld_;}
+
+  // Update current point cloud data.
+  //
+  // @param projection_mat: projection matrix from current render camera.
+  // @param view_mat: view matrix from current render camera.
+  // @param model_mat: model matrix for this point cloud frame.
+  // @param vertices: all vertices in this point cloud frame.
+  void Render(
+		  const glm::mat4 & projectionMatrix,
+		  const glm::mat4 & viewMatrix,
+		  bool meshRendering = true,
+		  float pointSize = 3.0f,
+		  bool textureRendering = false,
+		  bool lighting = true,
+		  float distanceToCamSqr = 0.0f,
+		  const GLuint & depthTexture = 0,
+		  int screenWidth = 0,     // nonnull if depthTexture>0
+		  int screenHeight = 0,    // nonnull if depthTexture>0
+		  float nearClipPlane = 0, // nonnull if depthTexture>0
+		  float farClipPlane = 0,  // nonnull if depthTexture>0
+		  bool packDepthToColorChannel = false,
+		  bool wireFrame = false,
+          bool hideSeams = false) const;
+
+ private:
+  template<class PointT>
+  void updateAABBMinMax(const PointT & pt, pcl::PointXYZ & min, pcl::PointXYZ & max)
+  {
+	  if(pt.x<min.x) min.x = pt.x;
+	  if(pt.y<min.y) min.y = pt.y;
+	  if(pt.z<min.z) min.z = pt.z;
+	  if(pt.x>max.x) max.x = pt.x;
+	  if(pt.y>max.y) max.y = pt.y;
+	  if(pt.z>max.z) max.z = pt.z;
+  }
+  void updateAABBWorld(const rtabmap::Transform & pose);
+
+ private:
+  // Vertex buffer of the point cloud geometry.
+  GLuint vertex_buffer_;
+  GLuint texture_;
+  std::vector<GLuint> index_buffers_;
+  std::vector<int> index_buffers_count_;
+  int nPoints_;
+  rtabmap::Transform pose_;
+  glm::mat4 poseGl_;
+  bool visible_;
+  bool hasNormals_;
+  std::vector<unsigned int> organizedToDenseIndices_;
+  float minHeight_; // odom frame
+
+  float gainR_;
+  float gainG_;
+  float gainB_;
+
+  pcl::PointXYZ aabbMinModel_;
+  pcl::PointXYZ aabbMaxModel_;
+  pcl::PointXYZ aabbMinWorld_;
+  pcl::PointXYZ aabbMaxWorld_;
+};
+
+#endif  // TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_

rtabmap/app/android/jni/quad_color.cpp

@@ -0,0 +1,69 @@
+/*
+ * Copyright 2014 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "quad_color.h"
+#include "tango-gl/util.h"
+
+static const float vertices[] = {-1.0f, -1.0f, 1.0f, -1.0f,
+                                 -1.0f, 1.0f,  1.0f, 1.0f};
+
+QuadColor::QuadColor(float size) {
+	SetShader();
+
+	vertices_.resize(8);
+	for(int i=0; i<8; ++i)
+	{
+		vertices_[i] = vertices[i]*size;
+	}
+}
+
+QuadColor::QuadColor(
+		float widthLeft,
+		float widthRight,
+		float heightBottom,
+		float heightTop) {
+	SetShader();
+
+	vertices_.resize(8);
+	vertices_[0] = vertices[0]*widthLeft;
+	vertices_[1] = vertices[1]*heightBottom;
+	vertices_[2] = vertices[2]*widthRight;
+	vertices_[3] = vertices[3]*heightBottom;
+	vertices_[4] = vertices[4]*widthLeft;
+	vertices_[5] = vertices[5]*heightTop;
+	vertices_[6] = vertices[6]*widthRight;
+	vertices_[7] = vertices[7]*heightTop;
+}
+
+void QuadColor::Render(const glm::mat4& projection_mat,
+                  const glm::mat4& view_mat) const {
+  glUseProgram(shader_program_);
+
+  // Calculate MVP matrix and pass it to shader.
+  glm::mat4 model_mat = GetTransformationMatrix();
+  glm::mat4 mvp_mat = projection_mat * view_mat * model_mat;
+  glUniformMatrix4fv(uniform_mvp_mat_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
+
+  glUniform4f(uniform_color_, red_, green_, blue_, alpha_);
+
+  // Vertice binding
+  glEnableVertexAttribArray(attrib_vertices_);
+  glVertexAttribPointer(attrib_vertices_, 2, GL_FLOAT, GL_FALSE, 0, &vertices_[0]);
+  glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+  glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
+  glUseProgram(0);
+}

rtabmap/app/android/jni/quad_color.h

@@ -0,0 +1,35 @@
+/*
+ * Copyright 2014 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef TANGO_GL_QUADCOLOR_H_
+#define TANGO_GL_QUADCOLOR_H_
+
+#include "tango-gl/drawable_object.h"
+
+class QuadColor : public tango_gl::DrawableObject {
+ public:
+	QuadColor(float size);
+	QuadColor(float widthLeft,
+			float widthRight,
+			float heightBottom,
+			float heightTop);
+	QuadColor(const QuadColor& other) = delete;
+	QuadColor& operator=(const QuadColor&) = delete;
+	virtual ~QuadColor() {}
+
+	void Render(const glm::mat4& projection_mat, const glm::mat4& view_mat) const;
+};
+#endif  // TANGO_GL_QUADCOLOR_H_

rtabmap/app/android/jni/scene.h

@@ -0,0 +1,272 @@
+/*
+ * Copyright 2014 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef TANGO_POINT_CLOUD_SCENE_H_
+#define TANGO_POINT_CLOUD_SCENE_H_
+
+#ifdef __ANDROID__
+#include <jni.h>
+#endif
+#include <memory>
+#include <set>
+
+#include "CameraMobile.h"
+#include <tango-gl/axis.h>
+#include <tango-gl/camera.h>
+#include <tango-gl/color.h>
+#include <tango-gl/gesture_camera.h>
+#include <tango-gl/grid.h>
+#include <tango-gl/frustum.h>
+#include <tango-gl/trace.h>
+#include <tango-gl/transform.h>
+#include <tango-gl/util.h>
+#include <tango-gl/circle.h>
+
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Link.h>
+
+#include "point_cloud_drawable.h"
+#include "graph_drawable.h"
+#include "bounding_box_drawable.h"
+#include "background_renderer.h"
+#include "text_drawable.h"
+#include "quad_color.h"
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+// Scene provides OpenGL drawable objects and renders them for visualization.
+class Scene {
+ public:
+    static const glm::vec3 kHeightOffset;
+ public:
+  // Constructor and destructor.
+  Scene();
+  ~Scene();
+
+  // Allocate OpenGL resources for rendering.
+  void InitGLContent();
+
+  // Release non-OpenGL allocated resources.
+  void DeleteResources();
+
+  // Setup GL view port.
+  void SetupViewPort(int w, int h);
+  int getViewPortWidth() const {return screenWidth_;}
+  int getViewPortHeight() const {return screenHeight_;}
+
+  rtabmap::ScreenRotation getScreenRotation() const {return color_camera_to_display_rotation_;}
+  void setScreenRotation(rtabmap::ScreenRotation colorCameraToDisplayRotation) {color_camera_to_display_rotation_ = colorCameraToDisplayRotation;}
+
+  void clear(); // removed all point clouds
+  void clearLines();
+  void clearTexts();
+  void clearQuads();
+  void clearCircles();
+
+  // Render loop.
+  // @param: cur_pose_transformation, latest pose's transformation.
+  // @param: point_cloud_transformation, pose transformation at point cloud
+  //         frame's timestamp.
+  // @param: point_cloud_vertices, point cloud's vertices of the current point
+  //         frame.
+  int Render(const float * uvsTransformed = 0, glm::mat4 arViewMatrix = glm::mat4(0), glm::mat4 arProjectionMatrix=glm::mat4(0), const rtabmap::Mesh & occlusionMesh=rtabmap::Mesh(), bool mapping=false);
+
+  // Set render camera's viewing angle, first person, third person or top down.
+  //
+  // @param: camera_type, camera type includes first person, third person and
+  //         top down
+  void SetCameraType(tango_gl::GestureCamera::CameraType camera_type);
+  tango_gl::GestureCamera::CameraType GetCameraType() const {return gesture_camera_->GetCameraType();}
+
+  void SetCameraPose(const rtabmap::Transform & pose); // opengl camera
+  rtabmap::Transform GetCameraPose() const {return currentPose_!=0?*currentPose_:rtabmap::Transform();}
+  rtabmap::Transform GetOpenGLCameraPose(float * fov = 0) const;
+
+  // Touch event passed from android activity. This function only support two
+  // touches.
+  //
+  // @param: touch_count, total count for touches.
+  // @param: event, touch event of current touch.
+  // @param: x0, normalized touch location for touch 0 on x axis.
+  // @param: y0, normalized touch location for touch 0 on y axis.
+  // @param: x1, normalized touch location for touch 1 on x axis.
+  // @param: y1, normalized touch location for touch 1 on y axis.
+  void OnTouchEvent(int touch_count, tango_gl::GestureCamera::TouchEvent event,
+                    float x0, float y0, float x1, float y1);
+
+  void updateGraph(
+  		const std::map<int, rtabmap::Transform> & poses,
+  		const std::multimap<int, rtabmap::Link> & links);
+
+  void setGraphVisible(bool visible);
+  void setGridVisible(bool visible);
+  void setTraceVisible(bool visible);
+  void setFrustumVisible(bool visible);
+
+  void addMarker(int id, const rtabmap::Transform & pose);
+  void setMarkerPose(int id, const rtabmap::Transform & pose);
+  bool hasMarker(int id) const;
+  void removeMarker(int id);
+  std::set<int> getAddedMarkers() const;
+
+  void addCloud(
+  		  int id,
+  		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		  const pcl::IndicesPtr & indices,
+  		  const rtabmap::Transform & pose);
+  void removeCloudOrMesh(int id);
+  void addMesh(
+  		int id,
+  		const rtabmap::Mesh & mesh,
+  		const rtabmap::Transform & pose,
+		bool createWireframe = false);
+  void addLine(
+        int id,
+        const cv::Point3f & pt1,
+        const cv::Point3f & pt2,
+        const tango_gl::Color & color = tango_gl::Color(1.0f, 1.0f, 1.0f));
+  void removeLine(int id);
+  void addText(
+        int id,
+        const std::string & text,
+        const rtabmap::Transform & pose,
+        float size,
+        const tango_gl::Color & color);
+  void removeText(int id);
+  void addQuad(
+        int id,
+        float size,
+        const rtabmap::Transform & pose,
+        const tango_gl::Color & color,
+        float alpha = 1.0f);
+  void addQuad(
+        int id,
+        float widthLeft,
+        float widthRight,
+        float heightBottom,
+        float heightTop,
+        const rtabmap::Transform & pose,
+        const tango_gl::Color & color,
+        float alpha =1.0f);
+  void removeQuad(int id);
+  bool hasQuad(int id) const;
+  void addCircle(
+          int id,
+          float radius,
+          const rtabmap::Transform & pose,
+          const tango_gl::Color & color,
+          float alpha = 1.0f);
+  void removeCircle(int id);
+  bool hasCircle(int id) const;
+
+  void setCloudPose(int id, const rtabmap::Transform & pose);
+  void setCloudVisible(int id, bool visible);
+  bool hasCloud(int id) const;
+  bool hasMesh(int id) const;
+  bool hasTexture(int id) const;
+  std::set<int> getAddedClouds() const;
+  void updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons);
+  void updateMesh(int id, const rtabmap::Mesh & mesh);
+  void updateGains(int id, float gainR, float gainG, float gainB);
+
+  void setBlending(bool enabled) {blending_ = enabled;}
+  void setMapRendering(bool enabled) {mapRendering_ = enabled;}
+  void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;}
+  void setPointSize(float size) {pointSize_ = size;}
+  void setFOV(float angle);
+  void setOrthoCropFactor(float value);
+  void setGridRotation(float angleDeg);
+  void setLighting(bool enabled) {lighting_ = enabled;}
+  void setBackfaceCulling(bool enabled) {backfaceCulling_ = enabled;}
+  void setWireframe(bool enabled) {wireFrame_ = enabled;}
+  void setTextureColorSeamsHidden(bool hidden) {textureColorSeamsHidden_ = hidden;}
+  void setBackgroundColor(float r, float g, float b) {r_=r; g_=g; b_=b;} // 0.0f <> 1.0f
+  void setGridColor(float r, float g, float b);
+
+  bool isBlending() const {return blending_;}
+  bool isMapRendering() const {return mapRendering_;}
+  bool isMeshRendering() const {return meshRendering_;}
+  bool isMeshTexturing() const {return meshRendering_ && meshRenderingTexture_;}
+  float getPointSize() const {return pointSize_;}
+  bool isLighting() const {return lighting_;}
+  bool isBackfaceCulling() const {return backfaceCulling_;}
+  bool isWireframe() const {return wireFrame_;}
+
+  BackgroundRenderer * background_renderer_;
+
+ private:
+  // Camera object that allows user to use touch input to interact with.
+  tango_gl::GestureCamera* gesture_camera_;
+
+  // Device axis (in device frame of reference).
+  tango_gl::Axis* axis_;
+
+  // Device frustum.
+  tango_gl::Frustum* frustum_;
+
+  // Ground grid.
+  tango_gl::Grid* grid_;
+
+  // Bounding box
+  BoundingBoxDrawable * box_;
+
+  // Trace of pose data.
+  tango_gl::Trace* trace_;
+  GraphDrawable * graph_;
+  bool graphVisible_;
+  bool gridVisible_;
+  bool traceVisible_;
+  bool frustumVisible_;
+
+  std::map<int, tango_gl::Axis*> markers_;
+
+  rtabmap::ScreenRotation color_camera_to_display_rotation_;
+
+  std::map<int, PointCloudDrawable*> pointClouds_;
+  std::map<int, tango_gl::Line*> lines_;
+  std::map<int, TextDrawable*> texts_;
+  std::map<int, QuadColor*> quads_;
+  std::map<int, tango_gl::Circle*> circles_;
+
+  rtabmap::Transform * currentPose_;
+
+  // Shader to display point cloud.
+  GLuint graph_shader_program_;
+
+  bool blending_;
+  bool mapRendering_;
+  bool meshRendering_;
+  bool meshRenderingTexture_;
+  float pointSize_;
+  bool boundingBoxRendering_;
+  bool lighting_;
+  bool backfaceCulling_;
+  bool wireFrame_;
+  bool textureColorSeamsHidden_;
+  float r_;
+  float g_;
+  float b_;
+  GLuint fboId_;
+  GLuint rboId_;
+  GLuint depthTexture_; // 0=objects+occlusion
+  GLsizei screenWidth_;
+  GLsizei screenHeight_;
+  bool doubleTapOn_;
+  cv::Point2f doubleTapPos_;
+};
+
+#endif  // TANGO_POINT_CLOUD_SCENE_H_

rtabmap/app/android/jni/tango-gl/axis.cpp

@@ -0,0 +1,75 @@
+/*
+ * Copyright 2014 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "tango-gl/axis.h"
+#include "tango-gl/shaders.h"
+
+namespace tango_gl {
+
+static const float float_vertices[] = {0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
+                                       0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
+                                       0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f};
+
+static const float float_colors[] = {
+    1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f,
+    0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 1.0f};
+
+Axis::Axis() : Line(3.0f, GL_LINES) {
+  // Implement SetShader here, not using the dedault one.
+  shader_program_ =
+      util::CreateProgram(shaders::GetColorVertexShader().c_str(),
+                          shaders::GetBasicFragmentShader().c_str());
+  if (!shader_program_) {
+    LOGE("Could not create program.");
+  }
+  uniform_mvp_mat_ = glGetUniformLocation(shader_program_, "mvp");
+  attrib_colors_ = glGetAttribLocation(shader_program_, "color");
+  attrib_vertices_ = glGetAttribLocation(shader_program_, "vertex");
+
+  size_t size = sizeof(float_vertices) / (sizeof(float) * 3);
+  for (size_t i = 0; i < size; i++) {
+    vec_vertices_.push_back(glm::vec3(float_vertices[i * 3],
+                                      float_vertices[i * 3 + 1],
+                                      float_vertices[i * 3 + 2]));
+    vec_colors_.push_back(
+        glm::vec4(float_colors[i * 4], float_colors[i * 4 + 1],
+                  float_colors[i * 4 + 2], float_colors[i * 4 + 3]));
+  }
+}
+
+void Axis::Render(const glm::mat4& projection_mat,
+                  const glm::mat4& view_mat) const {
+  glUseProgram(shader_program_);
+  glLineWidth(line_width_);
+  glm::mat4 model_mat = GetTransformationMatrix();
+  glm::mat4 mvp_mat = projection_mat * view_mat * model_mat;
+  glUniformMatrix4fv(uniform_mvp_mat_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
+
+  glEnableVertexAttribArray(attrib_vertices_);
+  glVertexAttribPointer(attrib_vertices_, 3, GL_FLOAT, GL_FALSE,
+                        sizeof(glm::vec3), &vec_vertices_[0]);
+
+  glEnableVertexAttribArray(attrib_colors_);
+  glVertexAttribPointer(attrib_colors_, 4, GL_FLOAT, GL_FALSE,
+                        sizeof(glm::vec4), &vec_colors_[0]);
+
+  glDrawArrays(render_mode_, 0, vec_vertices_.size());
+
+  glDisableVertexAttribArray(attrib_vertices_);
+  glDisableVertexAttribArray(attrib_colors_);
+  glUseProgram(0);
+}
+}  // namespace tango_gl

rtabmap/app/android/jni/tango-gl/band.cpp

@@ -0,0 +1,149 @@
+/*
+ * Copyright 2014 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "tango-gl/band.h"
+#include "tango-gl/util.h"
+
+namespace tango_gl {
+
+// Set band resolution to 0.01m(1cm) when using UpdateVertexArray()
+static const float kMinDistanceSquared = 0.0001f;
+
+Band::Band(const unsigned int max_length)
+    : band_width_(0.2), max_length_(max_length) {
+  SetShader();
+  vertices_v_.reserve(max_length);
+  pivot_left = glm::vec3(0, 0, 0);
+  pivot_right = glm::vec3(0, 0, 0);
+}
+
+void Band::SetWidth(const float width) {
+  band_width_ = width;
+}
+
+void Band::UpdateVertexArray(const glm::mat4 m, BandMode mode) {
+  // First 2 vertices of a band + 3 arrow head vertices.
+  bool need_to_initialize = (vertices_v_.size() < 5);
+
+  bool sufficient_delta = false;
+  if (!need_to_initialize) {
+    // Band head is the first two vertices after arrow head.
+    glm::vec3 band_front = 0.5f * (vertices_v_[vertices_v_.size() - 4] +
+                                   vertices_v_[vertices_v_.size() - 5]);
+    sufficient_delta = kMinDistanceSquared <
+        util::DistanceSquared(band_front, util::GetTranslationFromMatrix(m));
+  }
+
+  if (need_to_initialize || sufficient_delta) {
+    glm::vec3 left  = glm::vec3(-band_width_ * 0.5f, 0, 0);
+    glm::vec3 right = glm::vec3(band_width_ * 0.5f, 0, 0);
+    glm::vec3 arrow_left = glm::vec3(-band_width_ * 0.75f, 0, 0);
+    glm::vec3 arrow_right = glm::vec3(band_width_ * 0.75f, 0, 0);
+    glm::vec3 arrow_front = glm::vec3(0, 0, -band_width_ * 0.75f);
+
+    // If keep right pivot point, or normal mode,
+    // then only update left pivot point.
+    if (mode == BandMode::kNormal || mode == BandMode::kKeepRight) {
+      pivot_left = util::ApplyTransform(m, left);
+    }
+    // If keep left pivot point, or normal mode,
+    // then only update right pivot point.
+    if (mode == BandMode::kNormal || mode == BandMode::kKeepLeft) {
+      pivot_right = util::ApplyTransform(m, right);
+    }
+
+    glm::mat4 head_m = m;
+
+    if (mode != BandMode::kNormal) {
+      glm::vec3 up = glm::vec3(0, 1.0f, 0);
+      glm::vec3 position = 0.5f * (pivot_left + pivot_right);
+      glm::vec3 heading = glm::cross(up, pivot_right-pivot_left);
+      head_m = glm::inverse(glm::lookAt(glm::vec3(0, 0, 0), heading, up));
+      head_m[3][0] = position.x;
+      head_m[3][1] = position.y;
+      head_m[3][2] = position.z;
+    }
+
+    if (need_to_initialize) {
+      vertices_v_.resize(5);
+    } else {
+      vertices_v_.resize(vertices_v_.size() + 2);
+    }
+
+    size_t insertion_start = vertices_v_.size() - 5;
+    vertices_v_[insertion_start + 0] = pivot_left;
+    vertices_v_[insertion_start + 1] = pivot_right;
+    vertices_v_[insertion_start + 2] = util::ApplyTransform(head_m, arrow_left);
+    vertices_v_[insertion_start + 3] = util::ApplyTransform(head_m, arrow_right);
+    vertices_v_[insertion_start + 4] = util::ApplyTransform(head_m, arrow_front);
+
+    if (vertices_v_.size() > max_length_) {
+      vertices_v_.erase(vertices_v_.begin(), vertices_v_.begin() + 2);
+    }
+  }
+}
+
+void Band::UpdateVertexArray(const glm::mat4 m) {
+  // Defualt to call update with normal mode.
+  UpdateVertexArray(m, BandMode::kNormal);
+}
+
+void Band::SetVertexArray(const std::vector<glm::vec3>& v,
+                          const glm::vec3& up) {
+  vertices_v_.clear();
+  vertices_v_.reserve(2 * v.size());
+  if (v.size() < 2)
+    return;
+
+  for (size_t i = 0; i < v.size() - 1; ++i) {
+    glm::vec3 gl_p_world_a = v[i];
+    glm::vec3 gl_p_world_b = v[i + 1];
+    glm::vec3 dir = glm::normalize(gl_p_world_b - gl_p_world_a);
+    glm::vec3 left = glm::cross(up, dir);
+    glm::normalize(left);
+
+    vertices_v_.push_back(gl_p_world_a + (band_width_ / 2.0f * left));
+    vertices_v_.push_back(gl_p_world_a - (band_width_ / 2.0f * left));
+
+    // Cap the end of the path.
+    if (i == v.size() - 2) {
+      vertices_v_.push_back(gl_p_world_b + (band_width_ / 2.0f * left));
+      vertices_v_.push_back(gl_p_world_b - (band_width_ / 2.0f * left));
+    }
+  }
+
+}
+
+void Band::ClearVertexArray() { vertices_v_.clear(); }
+
+void Band::Render(const glm::mat4& projection_mat,
+                  const glm::mat4& view_mat) const {
+  glUseProgram(shader_program_);
+  glm::mat4 model_mat = GetTransformationMatrix();
+  glm::mat4 mvp_mat = projection_mat * view_mat * model_mat;
+  glUniformMatrix4fv(uniform_mvp_mat_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
+
+  glUniform4f(uniform_color_, red_, green_, blue_, alpha_);
+
+  glEnableVertexAttribArray(attrib_vertices_);
+  glVertexAttribPointer(attrib_vertices_, 3, GL_FLOAT, GL_FALSE,
+                        sizeof(glm::vec3), &vertices_v_[0]);
+  glDrawArrays(GL_TRIANGLE_STRIP, 0, vertices_v_.size());
+  glDisableVertexAttribArray(attrib_vertices_);
+  glUseProgram(0);
+}
+
+}  // namespace tango_gl

rtabmap/app/android/jni/tango-gl/bounding_box.cpp

@@ -0,0 +1,62 @@
+#include "tango-gl/bounding_box.h"
+
+namespace tango_gl {
+BoundingBox::BoundingBox(const std::vector<float>& vertices) {
+  // Set min and max to the first vertice.
+  bounding_min_ = glm::vec3(vertices[0], vertices[1], vertices[2]);
+  bounding_max_ = bounding_min_;
+  size_t vertices_count = vertices.size() / 3;
+  for (size_t i = 1; i < vertices_count; i += 3) {
+    bounding_min_.x = std::min(vertices[i * 3], bounding_min_.x);
+    bounding_min_.y = std::min(vertices[i * 3 + 1], bounding_min_.y);
+    bounding_min_.z = std::min(vertices[i * 3 + 2], bounding_min_.z);
+
+    bounding_max_.x = std::max(vertices[i * 3], bounding_max_.x);
+    bounding_max_.y = std::max(vertices[i * 3 + 1], bounding_max_.y);
+    bounding_max_.z = std::max(vertices[i * 3 + 2], bounding_max_.z);
+  }
+}
+
+bool BoundingBox::IsIntersecting(const Segment& segment,
+                                 const glm::quat& rotation,
+                                 const glm::mat4& transformation) {
+  // The current bounding box.
+  glm::vec3 min, max;
+
+  // If the mesh has been rotated, we need to derive a new bounding box
+  // based on the original one, if it just been translated or scaled,
+  // we can still use the original one with current model matrix applied.
+  if (rotation == glm::quat(1.0f, 0.0f, 0.0f, 0.0f)) {
+    min = util::ApplyTransform(transformation, bounding_min_);
+    max = util::ApplyTransform(transformation, bounding_max_);
+  } else {
+    std::vector<glm::vec3> box;
+    // Derive 8 vertices of the new bounding box from original min and max.
+    box.push_back(bounding_min_);
+    box.push_back(bounding_max_);
+
+    box.push_back(glm::vec3(bounding_min_.x, bounding_max_.y, bounding_max_.z));
+    box.push_back(glm::vec3(bounding_max_.x, bounding_min_.y, bounding_min_.z));
+
+    box.push_back(glm::vec3(bounding_min_.x, bounding_min_.y, bounding_max_.z));
+    box.push_back(glm::vec3(bounding_max_.x, bounding_max_.y, bounding_min_.z));
+
+    box.push_back(glm::vec3(bounding_max_.x, bounding_min_.y, bounding_max_.z));
+    box.push_back(glm::vec3(bounding_min_.x, bounding_max_.y, bounding_min_.z));
+
+    min = util::ApplyTransform(transformation, bounding_min_);
+    max = min;
+    for (size_t i = 1; i < box.size(); i++) {
+      glm::vec3 temp = util::ApplyTransform(transformation, box[i]);
+      min.x = std::min(temp.x, min.x);
+      min.y = std::min(temp.y, min.y);
+      min.z = std::min(temp.z, min.z);
+
+      max.x = std::max(temp.x, max.x);
+      max.y = std::max(temp.y, max.y);
+      max.z = std::max(temp.z, max.z);
+    }
+  }
+  return util::SegmentAABBIntersect(min, max, segment.start, segment.end);
+}
+}  // namespace tango_gl

rtabmap/app/android/jni/tango-gl/camera.cpp

@@ -0,0 +1,83 @@
+/*
+ * Copyright 2014 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "tango-gl/camera.h"
+#include "tango-gl/util.h"
+
+namespace tango_gl {
+
+Camera::Camera() {
+  field_of_view_ = 45.0f * DEGREE_2_RADIANS;
+  aspect_ratio_ = 4.0f / 3.0f;
+  width_ = 800.0f;
+  height_ = 600.0f;
+  near_clip_plane_ = 0.5f;
+  far_clip_plane_ = 50.0f;
+  ortho_ = false;
+  orthoScale_ = 2.0f;
+  orthoCropFactor_ = -1.0f;
+}
+
+glm::mat4 Camera::GetViewMatrix() {
+  return glm::inverse(GetTransformationMatrix());
+}
+
+glm::mat4 Camera::GetProjectionMatrix() {
+	if(ortho_)
+	{
+		return glm::ortho(-orthoScale_*aspect_ratio_, orthoScale_*aspect_ratio_, -orthoScale_, orthoScale_, orthoScale_ + orthoCropFactor_, far_clip_plane_);
+	}
+  return glm::perspective(field_of_view_, aspect_ratio_, near_clip_plane_, far_clip_plane_);
+}
+
+void Camera::SetWindowSize(float width, float height) {
+	width_ = width;
+	height_ = height;
+  aspect_ratio_ = width/height;
+}
+
+void Camera::SetFieldOfView(float fov) {
+  field_of_view_ = fov * DEGREE_2_RADIANS;
+}
+
+void Camera::SetNearFarClipPlanes(const float near, const float far)
+{
+	near_clip_plane_ = near;
+	far_clip_plane_ = far;
+}
+
+Camera::~Camera() {
+}
+
+glm::mat4 Camera::ProjectionMatrixForCameraIntrinsics(float width, float height,
+                                                      float fx, float fy,
+                                                      float cx, float cy,
+                                                      float near, float far) {
+  const float xscale = near / fx;
+  const float yscale = near / fy;
+
+  const float xoffset =  (cx - (width  / 2.0)) * xscale;
+  // Color camera's coordinates has y pointing downwards so we negate this term.
+  const float yoffset = -(cy - (height / 2.0)) * yscale;
+
+  return  glm::frustum(xscale * -width  / 2.0f - xoffset,
+                       xscale *  width  / 2.0f - xoffset,
+                       yscale * -height / 2.0f - yoffset,
+                       yscale *  height / 2.0f - yoffset,
+                       near, far);
+}
+
+}  // namespace tango_gl

rtabmap/app/android/jni/tango-gl/circle.cpp

@@ -0,0 +1,36 @@
+/*
+ * Copyright 2014 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "tango-gl/circle.h"
+
+namespace tango_gl {
+Circle::Circle(float radius, int resolution) : Mesh(GL_TRIANGLE_FAN){
+  SetShader();
+  std::vector<GLfloat> vertices;
+  vertices.reserve(3 * (resolution + 2));
+  vertices.push_back(0);
+  vertices.push_back(0);
+  vertices.push_back(0);
+  float delta_theta = M_PI * 2.0f / static_cast<float>(resolution);
+  for (int i = resolution; i >= 0; i--) {
+    float theta = delta_theta * static_cast<float>(i);
+    vertices.push_back(cos(theta) * radius);
+    vertices.push_back(sin(theta) * radius);
+    vertices.push_back(0);
+  }
+  SetVertices(vertices);
+}
+}  // namespace tango_gl

rtabmap/app/android/jni/tango-gl/conversions.cpp

@@ -0,0 +1,52 @@
+/*
+ * Copyright 2014 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "tango-gl/conversions.h"
+
+namespace tango_gl {
+namespace conversions {
+
+glm::mat4 opengl_world_T_tango_world() {
+  // Note glm is column-wise.
+  return glm::mat4(1.0f, 0.0f,  0.0f, 0.0f,
+                   0.0f, 0.0f, -1.0f, 0.0f,
+                   0.0f, 1.0f,  0.0f, 0.0f,
+                   0.0f, 0.0f,  0.0f, 1.0f);
+}
+
+glm::mat4 color_camera_T_opengl_camera() {
+  // Note glm is column-wise.
+  return glm::mat4(1.0f,  0.0f,  0.0f, 0.0f,
+                   0.0f, -1.0f,  0.0f, 0.0f,
+                   0.0f,  0.0f, -1.0f, 0.0f,
+                   0.0f,  0.0f,  0.0f, 1.0f);
+}
+
+glm::mat4 depth_camera_T_opengl_camera() {
+  // Note glm is column-wise.
+  return glm::mat4(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                   -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
+}
+
+glm::quat QuatTangoToGl(const glm::quat& tango_q_frame) {
+  const float kSqrt2Over2 = std::sqrt(2.0) / 2.0f;
+  // Tango frame is a -90 degree rotation about +X from the GL frame.
+  glm::quat gl_q_tango = glm::quat(kSqrt2Over2, -kSqrt2Over2, 0.0f, 0.0f);
+  return gl_q_tango * tango_q_frame;
+}
+
+}  // namespace gl_tango_conversions
+}  // namespace tango_gl

rtabmap/app/android/jni/tango-gl/cube.cpp

@@ -0,0 +1,54 @@
+/*
+ * Copyright 2014 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "tango-gl/cube.h"
+
+namespace tango_gl {
+
+static const GLfloat const_vertices[] = {
+    -1.0f, 1.0f,  1.0f,  -1.0f, -1.0f, 1.0f,  1.0f,  1.0f,  1.0f,  -1.0f, -1.0f,
+    1.0f,  1.0f,  -1.0f, 1.0f,  1.0f,  1.0f,  1.0f,  1.0f,  1.0f,  1.0f,  1.0f,
+    -1.0f, 1.0f,  1.0f,  1.0f,  -1.0f, 1.0f,  -1.0f, 1.0f,  1.0f,  -1.0f, -1.0f,
+    1.0f,  1.0f,  -1.0f, 1.0f,  1.0f,  -1.0f, 1.0f,  -1.0f, -1.0f, -1.0f, 1.0f,
+    -1.0f, 1.0f,  -1.0f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f,  -1.0f, -1.0f,
+    1.0f,  -1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f,  1.0f,  -1.0f, -1.0f, -1.0f,
+    -1.0f, -1.0f, 1.0f,  -1.0f, 1.0f,  1.0f,  -1.0f, 1.0f,  -1.0f, -1.0f, 1.0f,
+    1.0f,  1.0f,  1.0f,  -1.0f, -1.0f, 1.0f,  1.0f,  1.0f,  1.0f,  1.0f,  1.0f,
+    1.0f,  -1.0f, 1.0f,  -1.0f, -1.0f, 1.0f,  -1.0f, 1.0f,  -1.0f, -1.0f, -1.0f,
+    1.0f,  -1.0f, 1.0f,  -1.0f, -1.0f, 1.0f,  -1.0f, -1.0f, -1.0f};
+
+static const GLfloat const_normals[] = {
+    0.0f,  0.0f,  1.0f,  0.0f,  0.0f,  1.0f,  0.0f,  0.0f,  1.0f,  0.0f,  0.0f,
+    1.0f,  0.0f,  0.0f,  1.0f,  0.0f,  0.0f,  1.0f,  1.0f,  0.0f,  0.0f,  1.0f,
+    0.0f,  0.0f,  1.0f,  0.0f,  0.0f,  1.0f,  0.0f,  0.0f,  1.0f,  0.0f,  0.0f,
+    1.0f,  0.0f,  0.0f,  0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,
+    -1.0f, 0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,  -1.0f, -1.0f,
+    0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,
+    -1.0f, 0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,  0.0f,  1.0f,  0.0f,  0.0f,  1.0f,
+    0.0f,  0.0f,  1.0f,  0.0f,  0.0f,  1.0f,  0.0f,  0.0f,  1.0f,  0.0f,  0.0f,
+    1.0f,  0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,  -1.0f, 0.0f,
+    0.0f,  -1.0f, 0.0f,  0.0f,  -1.0f, 0.0f,  0.0f,  -1.0f, 0.0f};
+
+Cube::Cube() {
+  SetShader(true);
+  std::vector<GLfloat> vertices(
+      const_vertices,
+      const_vertices + sizeof(const_vertices) / sizeof(GLfloat));
+  std::vector<GLfloat> normals(
+      const_normals, const_normals + sizeof(const_normals) / sizeof(GLfloat));
+  SetVertices(vertices, normals);
+}
+}  // namespace tango_gl

rtabmap/app/android/jni/tango-gl/drawable_object.cpp

@@ -0,0 +1,66 @@
+/*
+ * Copyright 2014 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "tango-gl/drawable_object.h"
+#include "tango-gl/shaders.h"
+
+namespace tango_gl {
+
+void DrawableObject::SetShader() {
+  shader_program_ =
+      util::CreateProgram(shaders::GetBasicVertexShader().c_str(),
+                          shaders::GetBasicFragmentShader().c_str());
+  if (!shader_program_) {
+    LOGE("Could not create program.");
+  }
+  uniform_mvp_mat_ = glGetUniformLocation(shader_program_, "mvp");
+  attrib_vertices_ = glGetAttribLocation(shader_program_, "vertex");
+  uniform_color_ = glGetUniformLocation(shader_program_, "color");
+}
+
+void DrawableObject::DeleteGlResources() {
+  if (shader_program_) {
+    glDeleteShader(shader_program_);
+  }
+}
+
+void DrawableObject::SetColor(float red, float green, float blue) {
+  red_ = red;
+  green_ = green;
+  blue_ = blue;
+}
+void DrawableObject::SetColor(const Color& color) {
+  SetColor(color.r, color.g, color.b);
+}
+
+void DrawableObject::SetAlpha(const float alpha) { alpha_ = alpha; }
+
+void DrawableObject::SetVertices(const std::vector<GLfloat>& vertices) {
+  vertices_ = vertices;
+}
+
+void DrawableObject::SetVertices(const std::vector<GLfloat>& vertices,
+                                 const std::vector<GLushort>& indices) {
+  vertices_ = vertices;
+  indices_ = indices;
+}
+
+void DrawableObject::SetVertices(const std::vector<GLfloat>& vertices,
+                                 const std::vector<GLfloat>& normals) {
+  vertices_ = vertices;
+  normals_ = normals;
+}
+}  // namespace tango_gl

rtabmap/app/android/jni/tango-gl/frustum.cpp

@@ -0,0 +1,38 @@
+/*
+ * Copyright 2014 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "tango-gl/frustum.h"
+
+namespace tango_gl {
+
+static const float float_vertices[] = {
+    0.0f,  0.0f,  0.0f,  -1.0f, 1.0f,  -1.0f,
+	0.0f,  0.0f,  0.0f,  1.0f, 1.0f,  -1.0f,
+	0.0f,  0.0f,  0.0f,  -1.0f, -1.0f, -1.0f,
+	0.0f,  0.0f,  0.0f,  1.0f,  -1.0f, -1.0f,
+	-1.0f, 1.0f,  -1.0f, 1.0f,  1.0f,  -1.0f,
+    1.0f,  1.0f,  -1.0f, 1.0f,  -1.0f, -1.0f,
+	1.0f,  -1.0f, -1.0f, -1.0f, -1.0f, -1.0f,
+	-1.0f, -1.0f, -1.0f, -1.0f, 1.0f,  -1.0f};
+
+Frustum::Frustum() : Line(3.0f, GL_LINES) {
+  SetShader();
+  size_t size = sizeof(float_vertices) / sizeof(float);
+  for (size_t i = 0; i < size; i += 3) {
+    vec_vertices_.push_back(glm::vec3(float_vertices[i], float_vertices[i + 1],
+                                      float_vertices[i + 2]));
+  }
+}
+}  // namespace tango_gl

rtabmap/app/android/jni/tango-gl/gesture_camera.cpp

@@ -0,0 +1,254 @@
+/*
+ * Copyright 2014 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "tango-gl/gesture_camera.h"
+#include "tango-gl/util.h"
+#include "glm/gtx/quaternion.hpp"
+
+namespace {
+// Render camera observation distance in third person camera mode.
+const float kThirdPersonCameraDist = 7.0f;
+
+// Render camera observation distance in third person camera mode.
+const float kThirdPersonFollowCameraDist = 2.0f;
+
+// Render camera observation distance in top down camera mode.
+const float kTopDownCameraDist = 5.0f;
+
+// Zoom in speed.
+const float kZoomSpeed = 10.0f;
+
+// Move speed
+const float kMoveSpeed = 10.0f;
+
+// Rotation speed
+const float kRotationSpeed = 2.0f;
+
+// Min/max clamp value of camera observation distance.
+const float kCamViewMinDist = .1f;
+const float kCamViewMaxDist = 100.f;
+
+// FOV set up values.
+// Third and top down camera's FOV is 65 degrees.
+// First person camera's FOV is 85 degrees.
+const float kHighestFov = 120.0f;
+const float kHighFov = 85.0f;
+const float kLowFov = 65.0f;
+const float kLowestFov = 40.0f;
+}
+
+namespace tango_gl {
+
+GestureCamera::GestureCamera() :
+		cam_cur_target_rot_(1,0,0,0),
+		start_touch_dist_(0.0f),
+		cur_touch_dist_(0.0f)
+{
+  cam_parent_transform_ = new Transform();
+  SetParent(cam_parent_transform_);
+}
+
+GestureCamera::~GestureCamera() { delete cam_parent_transform_; }
+
+void GestureCamera::OnTouchEvent(int touch_count, TouchEvent event, float x0,
+		float y0, float x1, float y1) {
+
+	if (camera_type_!=kFirstPerson && touch_count == 1) {
+		switch (event) {
+		case kTouch0Down: {
+			cam_start_angle_ = cam_cur_angle_;
+
+			touch0_start_position_.x = x0;
+			touch0_start_position_.y = y0;
+			break;
+		}
+		case kTouchMove: {
+			glm::vec2 offset;
+
+			float rotation_x = (touch0_start_position_.y - y0) * kRotationSpeed;
+			float rotation_y = (touch0_start_position_.x - x0) * kRotationSpeed;
+
+			if(camera_type_!=kTopOrtho)
+				cam_cur_angle_.x = cam_start_angle_.x + rotation_x;
+			cam_cur_angle_.y = cam_start_angle_.y + rotation_y;
+
+			StartCameraToCurrentTransform();
+
+			break;
+		}
+		default: { break; }
+		}
+	}
+	if (touch_count == 2) {
+		switch (event) {
+		case kTouch1Down: {
+			float abs_x = x0 - x1;
+			float abs_y = y0 - y1;
+			start_touch_dist_ = std::sqrt(abs_x * abs_x + abs_y * abs_y);
+			cam_start_dist_ = GetPosition().z;
+			cam_start_fov_ = this->getFOV();
+
+			// center touch
+			touch0_start_position_.x = (x0+x1)/2.0f;
+			touch0_start_position_.y = (y0+y1)/2.0f;
+			break;
+		}
+		case kTouchMove: {
+			float abs_x = x0 - x1;
+			float abs_y = y0 - y1;
+			float dist = start_touch_dist_ - std::sqrt(abs_x * abs_x + abs_y * abs_y);
+
+			if(camera_type_ == kFirstPerson)
+			{
+				this->SetFieldOfView(tango_gl::util::Clamp(cam_start_fov_ + dist * kZoomSpeed*10.0f, kLowestFov, kHighestFov));
+			}
+			else
+			{
+				cam_cur_dist_ = tango_gl::util::Clamp(cam_start_dist_ + dist * kZoomSpeed,
+								kCamViewMinDist, kCamViewMaxDist);
+
+				this->SetOrthoMode(camera_type_ == kTopOrtho);
+				if(camera_type_ == kTopOrtho)
+				{
+					this->SetOrthoScale(cam_cur_dist_);
+				}
+
+				glm::vec2 touch_center_position((x0+x1)/2.0f, (y0+y1)/2.0f);
+				glm::vec2 offset;
+				offset.x = (touch_center_position.x - touch0_start_position_.x) * kMoveSpeed;
+				offset.y = (touch_center_position.y - touch0_start_position_.y) * kMoveSpeed;
+				touch0_start_position_ = touch_center_position;
+
+				StartCameraToCurrentTransform();
+
+				anchor_offset_ += glm::rotate(cam_parent_transform_->GetRotation(), glm::vec3(-offset.x, offset.y, 0));
+			}
+			break;
+		}
+		default: { break; }
+		}
+	}
+}
+
+Segment GestureCamera::GetSegmentFromTouch(float normalized_x,
+                                           float normalized_y,
+                                           float touch_range) {
+  float screen_height = touch_range * (2.0f * glm::tan(field_of_view_ * 0.5f));
+  float screen_width = screen_height * aspect_ratio_;
+  // normalized_x and normalized_x are from OnTouchEvent, top-left corner of the
+  // screen
+  // is [0, 0], transform it to opengl frame.
+  normalized_x = normalized_x - 0.5f;
+  normalized_y = 0.5f - normalized_y;
+  glm::vec3 start =
+      util::ApplyTransform(GetTransformationMatrix(), glm::vec3(0, 0, 0));
+  glm::vec3 end = util::ApplyTransform(GetTransformationMatrix(), 
+      glm::vec3(normalized_x * screen_width, normalized_y * screen_height,
+                -touch_range));
+  Segment segment(start, end);
+  return segment;
+}
+
+void GestureCamera::SetAnchorPosition(const glm::vec3& pos, const glm::quat & rotation) {
+	// Anchor position
+	cam_parent_transform_->SetPosition(pos+anchor_offset_);
+
+	// Anchor rotation
+	if(camera_type_ == kThirdPersonFollow)
+	{
+		cam_cur_target_rot_ = rotation;
+		cam_cur_target_rot_.x = 0;
+		cam_cur_target_rot_.z = 0;
+		cam_cur_target_rot_ = glm::normalize(cam_cur_target_rot_);
+		StartCameraToCurrentTransform();
+	}
+}
+
+void GestureCamera::SetCameraType(CameraType camera_index) {
+  camera_type_ = camera_index;
+  switch (camera_index) {
+    case kFirstPerson:
+      SetOrthoMode(false);
+      SetFieldOfView(kLowestFov);
+      SetNearFarClipPlanes(0.25, 25);
+      SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
+      SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
+      cam_cur_dist_ = 0.0f;
+      anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
+      cam_cur_angle_.x = 0.0f;
+      cam_cur_angle_.y = 0.0f;
+      cam_cur_target_rot_ = glm::quat(1,0,0,0);
+      cam_parent_transform_->SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
+      StartCameraToCurrentTransform();
+      break;
+    case kThirdPerson:
+    case kThirdPersonFollow:
+      SetOrthoMode(false);
+      SetFieldOfView(kLowFov);
+      SetNearFarClipPlanes(1, 50);
+      SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
+      SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
+      cam_cur_dist_ = camera_index==kThirdPersonFollow?kThirdPersonFollowCameraDist:kThirdPersonCameraDist;
+      anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
+      cam_cur_angle_.x = -M_PI / 12.0f;
+      cam_cur_angle_.y = kThirdPersonFollow?0:M_PI / 2.0f;
+      cam_cur_target_rot_ = glm::quat(1,0,0,0);
+      StartCameraToCurrentTransform();
+      break;
+    case kTopDown:
+      SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
+      SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
+      SetOrthoMode(false);
+      SetFieldOfView(kLowFov);
+      SetNearFarClipPlanes(1, 50);
+      cam_cur_dist_ = kTopDownCameraDist;
+      anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
+      cam_cur_angle_.x = -M_PI / 2.0f;
+      cam_cur_angle_.y = 0.0f;
+      cam_cur_target_rot_ = glm::quat(1,0,0,0);
+      StartCameraToCurrentTransform();
+      break;
+    case kTopOrtho:
+	  SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
+	  SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
+	  SetOrthoMode(true);
+	  SetOrthoScale(kTopDownCameraDist);
+	  SetOrthoCropFactor(-1.0f);
+      SetFieldOfView(kLowFov);
+      SetNearFarClipPlanes(1, 50);
+	  cam_cur_dist_ = kTopDownCameraDist;
+	  anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
+	  cam_cur_angle_.x = -M_PI / 2.0f;
+	  cam_cur_angle_.y = 0.0f;
+	  cam_cur_target_rot_ = glm::quat(1,0,0,0);
+	  StartCameraToCurrentTransform();
+	  break;
+    default:
+      break;
+  }
+}
+
+void GestureCamera::StartCameraToCurrentTransform()
+{
+  //Anchor rotation
+  glm::quat parent_cam_rot = glm::rotate(cam_cur_target_rot_, cam_cur_angle_.y, glm::vec3(0, 1, 0));
+  parent_cam_rot = glm::rotate(parent_cam_rot, cam_cur_angle_.x, glm::vec3(1, 0, 0));
+  cam_parent_transform_->SetRotation(parent_cam_rot);
+
+  //Camera position
+  SetPosition(glm::vec3(0, 0, cam_cur_dist_));
+}
+}  // namespace tango_gl

rtabmap/app/android/jni/tango-gl/goal_marker.cpp

@@ -0,0 +1,59 @@
+
+/*
+ * Copyright 2014 Google Inc. All Rights Reserved.
+ * Distributed under the Project Tango Preview Development Kit (PDK) Agreement.
+ * CONFIDENTIAL. AUTHORIZED USE ONLY. DO NOT REDISTRIBUTE.
+ */
+
+#include "tango-gl/goal_marker.h"
+
+namespace tango_gl {
+
+static const GLfloat const_vertices[] = {
+    -4.5298f,  -0.2676f,  0.0,       -4.3209f,  -1.3857f,  0.0,      -3.7242f,
+    -11.1445f, 0.0,       -5.6799f,  -8.9811f,  0.0,       -4.4540f, 0.8673f,
+    0.0,       -7.8376f,  -6.4508f,  0.0,       -9.5223f,  -3.0538f, 0.0,
+    -9.9826f,  -0.5898f,  0.0,       -9.8157f,  1.9113f,   0.0,      -9.0320f,
+    4.2923f,   0.0,       -7.6808f,  6.4036f,   0.0,       -5.8469f, 8.1125f,
+    0.0,       -3.6457f,  9.3117f,   0.0,       -4.0984f,  1.9477f,  0.0,
+    -1.2155f,  9.9259f,   0.0,       -3.4853f,  2.9057f,   0.0,      1.2912f,
+    9.9163f,   0.0,       -2.6532f,  3.6812f,   0.0,       3.7167f,  9.2837f,
+    0.0,       -1.6543f,  4.2254f,   0.0,       5.9087f,   8.0677f,  0.0,
+    -0.5515f,  4.5040f,   0.0,       7.7294f,   6.3448f,   0.0,      0.5859f,
+    4.4997f,   0.0,       9.0645f,   4.2232f,   0.0,       1.6865f,  4.2126f,
+    0.0,       9.8300f,   1.8363f,   0.0,       2.6812f,   3.6609f,  0.0,
+    9.9778f,   -0.6660f,  0.0,       3.5074f,   2.8791f,   0.0,      9.4987f,
+    -3.1264f,  0.0,       4.1132f,   1.9164f,   0.0,       7.7964f,  -6.5204f,
+    0.0,       4.4605f,   0.8333f,   0.0,       5.6245f,   -9.0444f, 0.0,
+    4.5276f,   -0.3022f,  0.0,       3.6572f,   -11.1925f, 0.0,      4.3102f,
+    -1.4187f,  0.0,       1.5165f,   -13.5960f, 0.0,       3.8220f,  -2.4460f,
+    0.0,       -0.0382f,  -16.4245f, 0.0,       3.0936f,   -3.3197f, 0.0,
+    -1.5902f,  -13.5657f, 0.0,       2.1709f,   -3.9847f,  0.0,      -3.8405f,
+    -2.4168f,  0.0,       -3.1189f,  -3.2959f,  0.0,       -2.2012f, -3.9680f,
+    0.0,       -1.1452f,  -4.3908f,  0.0,       -0.0173f,  -4.5377f, 0.0,
+    1.1117f,   -4.3994f,  0.0};
+
+static const GLushort const_indices[] = {
+    1,  2,  3,  1,  3,  4,  5,  1,  4,  4,  6,  7,  5,  4,  7,  7,  8,  9,  9,
+    10, 11, 7,  9,  11, 5,  7,  11, 5,  11, 12, 5,  12, 13, 14, 5,  13, 14, 13,
+    15, 16, 14, 15, 16, 15, 17, 18, 16, 17, 18, 17, 19, 20, 18, 19, 20, 19, 21,
+    22, 20, 21, 22, 21, 23, 24, 22, 23, 24, 23, 25, 26, 24, 25, 26, 25, 27, 28,
+    26, 27, 28, 27, 29, 30, 28, 29, 30, 29, 31, 32, 30, 31, 32, 31, 33, 34, 32,
+    33, 34, 33, 35, 36, 34, 35, 36, 35, 37, 38, 36, 37, 38, 37, 39, 40, 38, 39,
+    40, 39, 41, 42, 40, 41, 42, 41, 43, 44, 42, 43, 44, 43, 3,  3,  2,  45, 3,
+    45, 46, 3,  46, 47, 3,  47, 48, 3,  48, 49, 3,  49, 50, 44, 3,  50};
+
+GoalMarker::GoalMarker() {
+  SetShader();
+  std::vector<GLfloat> vertices(
+      const_vertices,
+      const_vertices + sizeof(const_vertices) / sizeof(GLfloat));
+  std::vector<GLushort> indices(
+      const_indices, const_indices + sizeof(const_indices) / sizeof(GLushort));
+  // Change indices so they are zero-indexed.
+  for (size_t i = 0; i < indices.size(); ++i) {
+    indices[i] = indices[i] - 1;
+  }
+  SetVertices(vertices, indices);
+}
+}  // namespace tango_gl

rtabmap/app/android/jni/tango-gl/grid.cpp

@@ -0,0 +1,47 @@
+/*
+ * Copyright 2014 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "tango-gl/grid.h"
+
+namespace tango_gl {
+
+// Initialize Grid with x and y grid count,
+// qx, quantity in x
+// qy, quantity in y.
+Grid::Grid(float density, int qx, int qy) : Line(1.0f, GL_LINES) {
+  SetShader();
+
+  // 3 float in 1 vertex, 2 vertices form a line.
+  // Horizontal line and vertical line forms the grid.
+  float width = density * qx / 2;
+  float height = density * qy / 2;
+
+  // Horizontal line.
+  for (int i = 0; i < (qy + 1); i++) {
+	  for (int j = 0; j < (qx + 1); j++) {
+		vec_vertices_.push_back(glm::vec3(-width + j*density, 0.0f, -height + i * density));
+		vec_vertices_.push_back(glm::vec3(-width+ + (j+1)*density, 0.0f, -height + i * density));
+	  }
+  }
+
+  for (int i = 0; i < (qx + 1); i++) {
+	  for (int j = 0; j < (qy + 1); j++) {
+		vec_vertices_.push_back(glm::vec3(-width + i * density, 0.0f, -height + j*density));
+		vec_vertices_.push_back(glm::vec3(-width + i * density, 0.0f, -height + (j+1)*density));
+	  }
+  }
+}
+}  // namespace tango_gl

rtabmap/app/android/jni/tango-gl/include/tango-gl/axis.h

@@ -0,0 +1,32 @@
+/*
+ * Copyright 2014 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef TANGO_GL_AXIS_H_
+#define TANGO_GL_AXIS_H_
+
+#include "tango-gl/line.h"
+
+namespace tango_gl {
+class Axis : public Line {
+ public:
+  Axis();
+  void Render(const glm::mat4& projection_mat, const glm::mat4& view_mat) const;
+ private:
+  GLuint attrib_colors_;
+  std::vector<glm::vec4> vec_colors_;
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_AXIS_H_

rtabmap/app/android/jni/tango-gl/include/tango-gl/band.h

@@ -0,0 +1,55 @@
+/*
+ * Copyright 2014 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef TANGO_GL_BAND_H_
+#define TANGO_GL_BAND_H_
+
+#include <stdlib.h>
+#include <vector>
+
+#include "tango-gl/drawable_object.h"
+
+namespace tango_gl {
+class Band : public DrawableObject {
+ public:
+  enum BandMode {
+    kNormal = 0,
+    kKeepLeft = 1,
+    kKeepRight = 2
+  };
+
+  Band(const unsigned int max_legnth);
+
+  void SetWidth(const float width);
+  // Render a Band with arrow head, pass in the mode for rendering,
+  // kKeepLeft is left turn, kKeepRight is right turn,
+  // when making a turn, vertices only get updated in one side to avoid overlapping.
+  void UpdateVertexArray(const glm::mat4 m, BandMode mode);
+  void UpdateVertexArray(const glm::mat4 m);
+  void SetVertexArray(const std::vector<glm::vec3>& v, const glm::vec3& up);
+  void ClearVertexArray();
+  void Render(const glm::mat4& projection_mat, const glm::mat4& view_mat) const;
+
+ private:
+  float band_width_;
+  unsigned int max_length_;
+  std::vector<glm::vec3> vertices_v_;
+  // Current band head's left and right position in world frame.
+  glm::vec3 pivot_left;
+  glm::vec3 pivot_right;
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_BAND_H_

rtabmap/app/android/jni/tango-gl/include/tango-gl/bounding_box.h

@@ -0,0 +1,42 @@
+/*
+ * Copyright 2014 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef TANGO_GL_BOUNDING_BOX_H_
+#define TANGO_GL_BOUNDING_BOX_H_
+
+#include <vector>
+
+#include "tango-gl/segment.h"
+#include "tango-gl/util.h"
+
+namespace tango_gl {
+class BoundingBox {
+ public:
+  BoundingBox()
+      : bounding_min_(glm::vec3(0, 0, 0)), bounding_max_(glm::vec3(0, 0, 0)) {}
+  BoundingBox(const std::vector<float>& vertices);
+  BoundingBox(const glm::vec3& min, const glm::vec3& max)
+      : bounding_min_(min), bounding_max_(max) {}
+  bool IsIntersecting(const Segment& segment, const glm::quat& rotation,
+                      const glm::mat4& transformation);
+
+ private:
+  // Axis-aligned bounding box minimum and maximum point.
+  glm::vec3 bounding_min_;
+  glm::vec3 bounding_max_;
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_BOUNDING_BOX_H_

rtabmap/app/android/jni/tango-gl/include/tango-gl/camera.h

@@ -0,0 +1,69 @@
+/*
+ * Copyright 2014 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef TANGO_GL_CAMERA_H_
+#define TANGO_GL_CAMERA_H_
+
+#include "tango-gl/transform.h"
+
+namespace tango_gl {
+class Camera : public Transform {
+ public:
+  Camera();
+  Camera(const Camera& other) = delete;
+  Camera& operator=(const Camera&) = delete;
+  ~Camera();
+
+  void SetWindowSize(const float width, const float height);
+  void SetFieldOfView(const float fov);
+  void SetOrthoMode(bool enabled) {ortho_ = enabled;}
+  void SetOrthoScale(float scale) {orthoScale_ = scale;}
+  void SetOrthoCropFactor(float value) {orthoCropFactor_ = value;}
+  void SetNearFarClipPlanes(const float near, const float far);
+
+  glm::mat4 GetViewMatrix();
+  glm::mat4 GetProjectionMatrix();
+  float getNearClipPlane() const {return near_clip_plane_;}
+  float getFarClipPlane() const {return far_clip_plane_;}
+
+  /**
+   * Create an OpenGL perspective matrix from window size, camera intrinsics, and clip settings.
+   *
+   * @param width  - The width of the camera image.
+   * @param height - The height of the camera image.
+   * @param fx     - The x-axis focal length of the camera.
+   * @param fy     - The y-axis focal length of the camera.
+   * @param cx     - The x-coordinate principal point in pixels.
+   * @param cy     - The y-coordinate principal point in pixels.
+   * @param near   - The desired near z-clipping plane.
+   * @param far    - The desired far z-clipping plane.
+   */
+  static glm::mat4 ProjectionMatrixForCameraIntrinsics(float width, float height,
+                                                       float fx,    float fy,
+                                                       float cx,    float cy,
+                                                       float near,  float far);
+ protected:
+  float field_of_view_;
+  float aspect_ratio_;
+  float width_;
+  float height_;
+  float near_clip_plane_, far_clip_plane_;
+  bool ortho_;
+  float orthoScale_;
+  float orthoCropFactor_;
+};
+}  // namespace tango_gl
+#endif  // TANGO_GL_CAMERA_H_