aruco_pose nodelet cleanup (#239)

* aruco_pose: Unhardcode contour refinement

Besides, this was basically a no-op anyway, since dynamic parameters
overwrote that anyway.

* aruco_pose: Late-construct objects that use ROS

* aruco_map: Don't create/store node handle

* aruco_pose: Don't assume dist_coeffs size

* aruco_pose: more const == more better

* aruco_pose: Be more obvious about changing variables

* aruco_pose: Fix building for Kinetic

* aruco_pose: Remove global add_definitions

.editorconfig

@@ -9,5 +9,12 @@ charset = utf-8
 indent_style = space
 indent_size = 4
 
-[*.{js,html}]
+[*.{cpp,h,js,html,txt}]
 indent_style = tab
+
+[*.txt]
+tab_width = 8
+
+[CMakeLists.txt]
+indent_style = space
+indent_size = 2

.gitattributes

@@ -1,2 +1,6 @@
-apps/ios/cleverrc/roslib.js linguist-vendored
 apps/ios/cleverrc/BinUtils.swift linguist-vendored
+roslib.js linguist-vendored
+eventemitter2.js linguist-vendored
+ros3d.js linguist-vendored
+three.min.js linguist-vendored
+aruco_pose/vendor/* linguist-vendored

.markdownlint.json

@@ -13,17 +13,100 @@
     "MD040": false,
     "MD044": {
         "names": [
+            "COEX",
+            "Copter Express",
+            "Коптер Экспресс",
+            "Клевер",
             "MAVLink",
             "ROS",
+            "ROS Kinetic",
+            "ROS Melodic",
+            "OpenCV",
+            "Gazebo",
+            "GitHub",
+            "FPV",
+            "PPM",
+            "PWM",
             "Python",
             "C++",
+            "JavaScript",
+            "Node.js",
+            "Django",
+            "Flask",
+            "HTTP",
+            "HTTPS",
+            "WebSocket",
+            "RPC",
             "PX4",
+            "ArduPilot",
+            "jMAVSim",
+            "px4.io",
+            "logs.px4.io",
+            "QGroundControl",
+            "QGC",
             "WireShark",
+            "FlightPlot",
+            "OFFBOARD",
+            "ACRO",
+            "RPY",
+            "LPE",
+            "EKF2",
+            "IMU",
+            "VPE",
+            "SITL",
+            "PID",
             "Wi-Fi",
             "Raspberry Pi",
+            "RPi",
+            "Linux",
+            "GNU",
+            "GNU/Linux",
+            "Windows",
+            "Docker",
+            "RFC",
+            "Travis",
+            "travis-ci.org",
+            "travis-ci.com",
+            "macOS",
+            "iOS",
+            "Android",
+            "Bluetooth",
+            "Raspbian",
+            "Raspbian Jesse",
+            "Raspbian Stretch",
+            "Raspbian Buster",
             "Pixhawk",
             "Pixracer",
-            "ArUco"
+            "Arduino",
+            "GPS",
+            "ArUco",
+            "LIRC",
+            "GPIO",
+            "HC-SR04",
+            "RCW-0001",
+            "RealSense",
+            "NUC",
+            "NVIDIA",
+            "Jetson",
+            "Jetson Nano",
+            "STM",
+            "LED",
+            "USB",
+            "FAT32",
+            "uORB",
+            "SSH",
+            "PuTTY",
+            "API",
+            "UART",
+            "GND",
+            "VCC",
+            "SCL",
+            "SDA",
+            "TCP",
+            "UDP",
+            "QR",
+            "Li-ion",
+            "Nvidia"
         ],
         "code_blocks": false
     },

.travis.yml

@@ -1,33 +1,119 @@
-sudo: required
+os: linux
+dist: xenial
 language: generic
 services:
   - docker
 env:
   global:
-    - DOCKER="goldarte/img-tool:builder-mod"
+    - DOCKER="sfalexrog/img-tool:qemu-update"
     - TARGET_REPO="https://github.com/${TRAVIS_REPO_SLUG}.git"
     - if [[ -z ${TRAVIS_TAG} ]]; then IMAGE_VERSION="${TRAVIS_COMMIT}}"; else IMAGE_VERSION="${TRAVIS_TAG}"; fi
     - IMAGE_NAME="$(basename -s '.git' ${TARGET_REPO})_${IMAGE_VERSION}.img"
 git:
-  depth: 1
-before_script:
-  - docker pull ${DOCKER}
-script:
-  - docker run --privileged --rm -v /dev:/dev -v $(pwd):/builder/repo -e TRAVIS_TAG="${TRAVIS_TAG}" ${DOCKER}
-before_deploy:
-  # Set up git user name and tag this commit
-  - git config --local user.name "goldarte"
-  - git config --local user.email "goldartt@gmail.com"
-  - sudo chmod -R 777 *
-  - cd images && zip ${IMAGE_NAME}.zip ${IMAGE_NAME}
-deploy:
-  provider: releases
-  api_key: ${GITHUB_OAUTH_TOKEN}
-  file: ${IMAGE_NAME}.zip
-  skip_cleanup: true
-  on:
-    tags: true
-
+  depth: 50
+jobs:
+  fast_finish: true
+  include:
+    - stage: Build
+      name: "Raspberry Pi Image Build"
+      cache:
+        directories:
+        - imgcache
+      before_script:
+        - docker pull ${DOCKER}
+# Check if there are any cached images, copy them to our "images" directory
+        - if [ -n "$(ls -A imgcache/*.zip)" ]; then mkdir -p images && cp imgcache/*.zip images; fi
+      script:
+        - if [[ -z ${TRAVIS_TAG} && "${TRAVIS_PULL_REQUEST}" != "false" ]]; then
+            echo "Commit range is ${TRAVIS_COMMIT_RANGE}" &&
+            if [ $(git diff --name-only ${TRAVIS_COMMIT_RANGE} | grep -v ^"docs/" | wc -l) -eq 0 ]; then
+              echo " === Docs-only change; skipping build ===" &&
+              export SKIP_BUILD=true;
+            fi;
+          fi
+        - if [ -z ${SKIP_BUILD} ]; then
+            docker run --privileged --rm -v /dev:/dev -v $(pwd):/builder/repo -e TRAVIS_TAG="${TRAVIS_TAG}" ${DOCKER};
+          fi
+      before_cache:
+        - cp images/*.zip imgcache
+      before_deploy:
+        # Set up git user name and tag this commit
+        - git config --local user.name "goldarte"
+        - git config --local user.email "goldartt@gmail.com"
+        - sudo chmod -R 777 *
+        - cd images && zip ${IMAGE_NAME}.zip ${IMAGE_NAME}
+      deploy:
+        provider: releases
+        token: ${GITHUB_OAUTH_TOKEN}
+        file: ${IMAGE_NAME}.zip
+        skip_cleanup: true
+        on:
+          tags: true
+        draft: true
+        name: ${TRAVIS_TAG}
+    - stage: Build
+      name: "Native Kinetic build"
+      env:
+        - NATIVE_DOCKER=ros:kinetic-ros-base
+      before_script:
+        - docker pull ${NATIVE_DOCKER}
+      script:
+        - docker run --rm -v $(pwd):/root/catkin_ws/src/clover ${NATIVE_DOCKER} /root/catkin_ws/src/clover/builder/standalone-install.sh
+    - stage: Build
+      name: "Native Melodic build"
+      env:
+        - NATIVE_DOCKER=ros:melodic-ros-base
+      before_script:
+        - docker pull ${NATIVE_DOCKER}
+      script:
+        - docker run --rm -v $(pwd):/root/catkin_ws/src/clover ${NATIVE_DOCKER} /root/catkin_ws/src/clover/builder/standalone-install.sh
+    - stage: Build
+      name: "Documentation"
+      language: node_js
+      node_js:
+        - "10"
+      before_script:
+        - npm install gitbook-cli -g
+        - npm install markdownlint-cli -g
+        - gitbook -V
+        - markdownlint -V
+      script:
+        - markdownlint docs
+        - ./check_assets_size.py
+        - ./check_unused_assets.py
+        - gitbook install
+        - gitbook build
+      # deploy:
+      #   provider: pages
+      #   local_dir: _book
+      #   skip_cleanup: true
+      #   token: ${GITHUB_OAUTH_TOKEN}
+      #   keep_history: true
+      #   target_branch: master
+      #   repo: CopterExpress/clover.coex.tech
+      #   fqdn: clover.coex.tech
+      #   verbose: true
+      #   on:
+      #     branch: master
+    - stage: Annotate
+      name: Auto-generate changelog
+      language: python
+      python: 3.6
+      install:
+        - pip install GitPython PyGithub
+      script:
+        - PYTHONUNBUFFERED=1 python ./gen_changelog.py
+    - stage: Build
+      name: Editorconfig-lint
+      language: generic
+      before_script:
+        - wget https://github.com/okalachev/editorconfig-checker/releases/download/1.2.1-disable-spaces-amount/ec-linux-amd64
+        - chmod +x ec-linux-amd64
+      script:
+        - ./ec-linux-amd64 -spaces-after-tabs -e "roslib.js|ros3d.js|eventemitter2.js|draw.cpp|BinUtils.swift|\.idea|apps/android/app|Assets.xcassets|test_parser_pass.txt|test_node_failure.txt|aruco_pose/vendor|\.stl|\.dxf"
+stages:
+  - Build
+  - Annotate
 # More info there
 # https://github.com/travis-ci/travis-ci/issues/6893
 # https://docs.travis-ci.com/user/customizing-the-build/

README.md

@@ -1,77 +1,40 @@
-# CLEVER
+# COEX Clover Drone Kit
 
-<img src="docs/assets/clever3.png" align="right" width="300px" alt="CLEVER drone">
+<img src="docs/assets/clever4-front-white.png" align="right" width="400px" alt="Clover Drone">
 
-CLEVER is an educational programmable drone kit consisting of an unassembled quadcopter, open source software and documentation. The kit includes Pixhawk/Pixracer autopilot running PX4 firmware, Raspberry Pi 3 as companion computer, a camera for computer vision navigation as well as additional sensors and peripheral devices.
+Clover is an educational programmable drone kit consisting of an unassembled quadcopter, open source software and documentation. The kit includes Pixracer-compatible autopilot running PX4 firmware, Raspberry Pi 4 as companion computer, a camera for computer vision navigation as well as additional sensors and peripheral devices.
 
-Copter Express has implemented a large number of different autonomous drone projects using exactly the same platform: [automated pizza delivery](https://www.youtube.com/watch?v=hmkAoZOtF58) in Samara and Kazan, coffee delivery in Skolkovo Innovation Center, [autonomous quadcopter with charging station](https://www.youtube.com/watch?v=RjX6nUqw1mI) for site monitoring and security, winning drones on [Robocross-2016](https://www.youtube.com/watch?v=dGbDaz_VmYU) and [Robocross-2017](https://youtu.be/AQnd2CRczbQ) competitions and many others.
+The main documentation is available [on Gitbook](https://clover.coex.tech/).
 
-**The main documentation in Russian is available [on our Gitbook](https://clever.copterexpress.com/).**
+Official website: <a href="https://coex.tech/clover">coex.tech/clover</a>.
 
-Use it to learn how to assemble, configure, pilot and program autonomous CLEVER drone.
+## Video compilation
 
-## Preconfigured RPi 3 image
+[![Clover Drone Kit autonomy compilation](http://img.youtube.com/vi/u3omgsYC4Fk/hqdefault.jpg)](https://youtu.be/u3omgsYC4Fk)
 
-**Preconfigured image for Raspberry Pi 3 with installed and configured software, ready to fly, is available [in the Releases section](https://github.com/CopterExpress/clever/releases).**
+Clover drone is used on a wide range of educational events, including [Copter Hack](https://www.youtube.com/watch?v=xgXheg3TTs4), WorldSkills Drone Operation competition, [Autonomous Vehicles Track of NTI Olympics 2016–2020](https://www.youtube.com/watch?v=E1_ehvJRKxg), Quadro Hack 2019 (National University of Science and Technology MISiS), Russian Robot Olympiad (autonomous flights), and others.
 
-[![Build Status](https://travis-ci.org/CopterExpress/clever.svg?branch=master)](https://travis-ci.org/CopterExpress/clever)
+## Raspberry Pi image
 
-Image includes:
+Preconfigured image for Raspberry Pi with installed and configured software, ready to fly, is available [in the Releases section](https://github.com/CopterExpress/clover/releases).
 
-* Raspbian Stretch
-* ROS Kinetic
+[![Build Status](https://travis-ci.org/CopterExpress/clover.svg?branch=master)](https://travis-ci.org/CopterExpress/clover)
+
+Image features:
+
+* Raspbian Buster
+* [ROS Melodic](http://wiki.ros.org/melodic)
 * Configured networking
 * OpenCV
-* mavros
-* CLEVER software bundle for autonomous drone control
+* [`mavros`](http://wiki.ros.org/mavros)
+* Periphery drivers for ROS ([GPIO](https://clover.coex.tech/en/gpio.html), [LED strip](https://clover.coex.tech/en/leds.html), etc)
+* `aruco_pose` package for marker-assisted navigation
+* `clover` package for autonomous drone control
 
-API description (in Russian) for autonomous flights is available [on GitBook](https://clever.copterexpress.com/simple_offboard.html).
+API description for autonomous flights is available [on GitBook](https://clover.coex.tech/en/simple_offboard.html).
 
-## Manual installation
+For manual package installation and running see [`clover` package documentation](clover/README.md).
 
-Install ROS Kinetic according to the [documentation](http://wiki.ros.org/kinetic/Installation).
+## License
 
-Clone repo to directory `/home/pi/catkin_ws/src/clever`:
-
-```bash
-cd ~/catkin_ws/src
-git clone https://github.com/CopterExpress/clever.git clever
-```
-
-Build ROS packages:
-
-```bash
-cd ~/catkin_ws
-catkin_make -j1
-```
-
-Enable systemd service `roscore` (if not enabled):
-
-```bash
-sudo systemctl enable /home/pi/catkin_ws/src/clever/deploy/roscore.service
-sudo systemctl start roscore
-```
-
-Enable systemd service `clever`:
-
-```bash
-sudo systemctl enable /home/pi/catkin_ws/src/clever/deploy/clever.service
-sudo systemctl start clever
-```
-
-### Dependencies
-
-[ROS Kinetic](http://wiki.ros.org/kinetic).
-
-Necessary ROS packages:
-
-* `opencv3`
-* `mavros`
-* `rosbridge_suite`
-* `web_video_server`
-* `cv_camera`
-* `nodelet`
-* `dynamic_reconfigure`
-* `bondcpp`, branch `master`
-* `roslint`
-* `rosserial`
+While the Clover platform source code is available under the MIT License, note, that the [documentation](docs/) is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License.

apps/android/.gitignore

@@ -0,0 +1,11 @@
+*.iml
+.gradle
+/local.properties
+/.idea/caches/build_file_checksums.ser
+/.idea/libraries
+/.idea/modules.xml
+/.idea/workspace.xml
+.DS_Store
+/build
+/captures
+.externalNativeBuild

apps/android/.idea/assetWizardSettings.xml

@@ -0,0 +1,57 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="WizardSettings">
+    <option name="children">
+      <map>
+        <entry key="imageWizard">
+          <value>
+            <PersistentState>
+              <option name="children">
+                <map>
+                  <entry key="imageAssetPanel">
+                    <value>
+                      <PersistentState>
+                        <option name="children">
+                          <map>
+                            <entry key="launcher">
+                              <value>
+                                <PersistentState>
+                                  <option name="children">
+                                    <map>
+                                      <entry key="foregroundImage">
+                                        <value>
+                                          <PersistentState>
+                                            <option name="values">
+                                              <map>
+                                                <entry key="scalingPercent" value="54" />
+                                              </map>
+                                            </option>
+                                          </PersistentState>
+                                        </value>
+                                      </entry>
+                                    </map>
+                                  </option>
+                                  <option name="values">
+                                    <map>
+                                      <entry key="backgroundAssetType" value="COLOR" />
+                                      <entry key="backgroundColor" value="ffffff" />
+                                      <entry key="foregroundImage" value="C:\Users\Motoy\Desktop\COEX\logo.png" />
+                                    </map>
+                                  </option>
+                                </PersistentState>
+                              </value>
+                            </entry>
+                          </map>
+                        </option>
+                      </PersistentState>
+                    </value>
+                  </entry>
+                </map>
+              </option>
+            </PersistentState>
+          </value>
+        </entry>
+      </map>
+    </option>
+  </component>
+</project>

apps/android/.idea/codeStyles/Project.xml

@@ -0,0 +1,35 @@
+<component name="ProjectCodeStyleConfiguration">
+  <code_scheme name="Project" version="173">
+    <JetCodeStyleSettings>
+      <option name="CODE_STYLE_DEFAULTS" value="KOTLIN_OFFICIAL" />
+    </JetCodeStyleSettings>
+    <Objective-C-extensions>
+      <file>
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Import" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Macro" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Typedef" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Enum" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Constant" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Global" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Struct" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="FunctionPredecl" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Function" />
+      </file>
+      <class>
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Property" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Synthesize" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="InitMethod" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="StaticMethod" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="InstanceMethod" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="DeallocMethod" />
+      </class>
+      <extensions>
+        <pair source="cpp" header="h" fileNamingConvention="NONE" />
+        <pair source="c" header="h" fileNamingConvention="NONE" />
+      </extensions>
+    </Objective-C-extensions>
+    <codeStyleSettings language="kotlin">
+      <option name="CODE_STYLE_DEFAULTS" value="KOTLIN_OFFICIAL" />
+    </codeStyleSettings>
+  </code_scheme>
+</component>

apps/android/.idea/codeStyles/codeStyleConfig.xml

@@ -0,0 +1,5 @@
+<component name="ProjectCodeStyleConfiguration">
+  <state>
+    <option name="USE_PER_PROJECT_SETTINGS" value="true" />
+  </state>
+</component>

apps/android/.idea/gradle.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="GradleSettings">
+    <option name="linkedExternalProjectsSettings">
+      <GradleProjectSettings>
+        <option name="distributionType" value="DEFAULT_WRAPPED" />
+        <option name="externalProjectPath" value="$PROJECT_DIR$" />
+        <option name="modules">
+          <set>
+            <option value="$PROJECT_DIR$" />
+            <option value="$PROJECT_DIR$/app" />
+          </set>
+        </option>
+        <option name="resolveModulePerSourceSet" value="false" />
+      </GradleProjectSettings>
+    </option>
+  </component>
+</project>

apps/android/.idea/misc.xml

@@ -0,0 +1,38 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="NullableNotNullManager">
+    <option name="myDefaultNullable" value="android.support.annotation.Nullable" />
+    <option name="myDefaultNotNull" value="android.support.annotation.NonNull" />
+    <option name="myNullables">
+      <value>
+        <list size="7">
+          <item index="0" class="java.lang.String" itemvalue="org.jetbrains.annotations.Nullable" />
+          <item index="1" class="java.lang.String" itemvalue="javax.annotation.Nullable" />
+          <item index="2" class="java.lang.String" itemvalue="javax.annotation.CheckForNull" />
+          <item index="3" class="java.lang.String" itemvalue="edu.umd.cs.findbugs.annotations.Nullable" />
+          <item index="4" class="java.lang.String" itemvalue="android.support.annotation.Nullable" />
+          <item index="5" class="java.lang.String" itemvalue="androidx.annotation.Nullable" />
+          <item index="6" class="java.lang.String" itemvalue="androidx.annotation.RecentlyNullable" />
+        </list>
+      </value>
+    </option>
+    <option name="myNotNulls">
+      <value>
+        <list size="6">
+          <item index="0" class="java.lang.String" itemvalue="org.jetbrains.annotations.NotNull" />
+          <item index="1" class="java.lang.String" itemvalue="javax.annotation.Nonnull" />
+          <item index="2" class="java.lang.String" itemvalue="edu.umd.cs.findbugs.annotations.NonNull" />
+          <item index="3" class="java.lang.String" itemvalue="android.support.annotation.NonNull" />
+          <item index="4" class="java.lang.String" itemvalue="androidx.annotation.NonNull" />
+          <item index="5" class="java.lang.String" itemvalue="androidx.annotation.RecentlyNonNull" />
+        </list>
+      </value>
+    </option>
+  </component>
+  <component name="ProjectRootManager" version="2" languageLevel="JDK_1_7" project-jdk-name="1.8" project-jdk-type="JavaSDK">
+    <output url="file://$PROJECT_DIR$/build/classes" />
+  </component>
+  <component name="ProjectType">
+    <option name="id" value="Android" />
+  </component>
+</project>

apps/android/.idea/runConfigurations.xml

@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="RunConfigurationProducerService">
+    <option name="ignoredProducers">
+      <set>
+        <option value="org.jetbrains.plugins.gradle.execution.test.runner.AllInPackageGradleConfigurationProducer" />
+        <option value="org.jetbrains.plugins.gradle.execution.test.runner.TestClassGradleConfigurationProducer" />
+        <option value="org.jetbrains.plugins.gradle.execution.test.runner.TestMethodGradleConfigurationProducer" />
+      </set>
+    </option>
+  </component>
+</project>

apps/android/.idea/vcs.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="$PROJECT_DIR$" vcs="Git" />
+  </component>
+</project>

apps/android/app/.gitignore

@@ -0,0 +1 @@
+/build

apps/android/app/build.gradle

@@ -0,0 +1,33 @@
+apply plugin: 'com.android.application'
+
+apply plugin: 'kotlin-android'
+
+apply plugin: 'kotlin-android-extensions'
+
+android {
+    compileSdkVersion 28
+    defaultConfig {
+        applicationId "express.copter.cleverrc"
+        minSdkVersion 19
+        targetSdkVersion 28
+        versionCode 1
+        versionName "1.0"
+        testInstrumentationRunner "android.support.test.runner.AndroidJUnitRunner"
+    }
+    buildTypes {
+        release {
+            minifyEnabled false
+            proguardFiles getDefaultProguardFile('proguard-android.txt'), 'proguard-rules.pro'
+        }
+    }
+}
+
+dependencies {
+    implementation fileTree(dir: 'libs', include: ['*.jar'])
+    implementation"org.jetbrains.kotlin:kotlin-stdlib-jdk7:$kotlin_version"
+    implementation 'com.android.support:appcompat-v7:28.0.0'
+    implementation 'com.android.support.constraint:constraint-layout:1.1.3'
+    testImplementation 'junit:junit:4.12'
+    androidTestImplementation 'com.android.support.test:runner:1.0.2'
+    androidTestImplementation 'com.android.support.test.espresso:espresso-core:3.0.2'
+}

apps/android/app/proguard-rules.pro

@@ -0,0 +1,21 @@
+# Add project specific ProGuard rules here.
+# You can control the set of applied configuration files using the
+# proguardFiles setting in build.gradle.
+#
+# For more details, see
+#   http://developer.android.com/guide/developing/tools/proguard.html
+
+# If your project uses WebView with JS, uncomment the following
+# and specify the fully qualified class name to the JavaScript interface
+# class:
+#-keepclassmembers class fqcn.of.javascript.interface.for.webview {
+#   public *;
+#}
+
+# Uncomment this to preserve the line number information for
+# debugging stack traces.
+#-keepattributes SourceFile,LineNumberTable
+
+# If you keep the line number information, uncomment this to
+# hide the original source file name.
+#-renamesourcefileattribute SourceFile

apps/android/app/src/androidTest/java/express/copter/cleverrc/ExampleInstrumentedTest.kt

@@ -0,0 +1,24 @@
+package express.copter.cleverrc
+
+import android.support.test.InstrumentationRegistry
+import android.support.test.runner.AndroidJUnit4
+
+import org.junit.Test
+import org.junit.runner.RunWith
+
+import org.junit.Assert.*
+
+/**
+ * Instrumented test, which will execute on an Android device.
+ *
+ * See [testing documentation](http://d.android.com/tools/testing).
+ */
+@RunWith(AndroidJUnit4::class)
+class ExampleInstrumentedTest {
+    @Test
+    fun useAppContext() {
+        // Context of the app under test.
+        val appContext = InstrumentationRegistry.getTargetContext()
+        assertEquals("express.copter.cleverrc", appContext.packageName)
+    }
+}

apps/android/app/src/main/AndroidManifest.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0" encoding="utf-8"?>
+<manifest xmlns:android="http://schemas.android.com/apk/res/android"
+          package="express.copter.cleverrc">
+    <uses-permission android:name="android.permission.INTERNET"/>
+
+
+    <application
+            android:allowBackup="true"
+            android:icon="@mipmap/ic_launcher"
+            android:label="@string/app_name"
+            android:roundIcon="@mipmap/ic_launcher_round"
+            android:supportsRtl="true"
+            android:theme="@style/AppTheme">
+        <activity android:name=".MainActivity"
+                  android:screenOrientation="landscape"
+                  android:theme="@style/NoUiAppTheme">
+            <intent-filter>
+                <action android:name="android.intent.action.MAIN"/>
+
+                <category android:name="android.intent.category.LAUNCHER"/>
+            </intent-filter>
+        </activity>
+    </application>
+
+</manifest>

apps/android/app/src/main/assets/clever.svg

@@ -0,0 +1,57 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<svg
+   xmlns:dc="http://purl.org/dc/elements/1.1/"
+   xmlns:cc="http://creativecommons.org/ns#"
+   xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+   xmlns:svg="http://www.w3.org/2000/svg"
+   xmlns="http://www.w3.org/2000/svg"
+   viewBox="0 0 69.988266 69.987198"
+   height="69.987198"
+   width="69.988266"
+   xml:space="preserve"
+   id="svg2"
+   version="1.1"><metadata
+     id="metadata8"><rdf:RDF><cc:Work
+         rdf:about=""><dc:format>image/svg+xml</dc:format><dc:type
+           rdf:resource="http://purl.org/dc/dcmitype/StillImage" /></cc:Work></rdf:RDF></metadata><defs
+     id="defs6"><clipPath
+       id="clipPath18"
+       clipPathUnits="userSpaceOnUse"><path
+         id="path16"
+         d="M 0,52.49 H 52.491 V 0 H 0 Z" /></clipPath></defs><g
+     transform="matrix(1.3333333,0,0,-1.3333333,0,69.9872)"
+     id="g10"><g
+       id="g12"><g
+         clip-path="url(#clipPath18)"
+         id="g14"><g
+           transform="translate(35.6531,35.3361)"
+           id="g20"><path
+             id="path22"
+             style="fill:white;fill-opacity:0.5;fill-rule:nonzero;stroke:none"
+             d="M 0,0 C 0.279,0.322 0.5,0.686 0.657,1.081 0.827,1.513 0.914,1.968 0.917,2.434 0.92,2.924 0.829,3.403 0.647,3.857 0.458,4.329 0.165,4.77 -0.2,5.13 -0.587,5.512 -1.061,5.812 -1.571,5.995 -2.138,6.198 -2.756,6.262 -3.358,6.18 -3.821,6.115 -4.263,5.967 -4.671,5.739 -4.886,5.618 -5.094,5.472 -5.291,5.305 L -5.467,5.151 C -5.527,5.099 -5.587,5.049 -5.649,5.003 -5.775,4.909 -5.906,4.827 -6.04,4.759 -6.173,4.691 -6.314,4.633 -6.458,4.586 -6.575,4.549 -6.696,4.519 -6.819,4.496 -6.917,4.478 -7.017,4.465 -7.115,4.457 c -0.623,-0.052 -1.281,0.082 -1.9,0.385 -0.224,0.111 -0.439,0.241 -0.64,0.387 -0.042,0.03 -0.084,0.063 -0.127,0.095 -0.039,0.031 -0.069,0.054 -0.102,0.09 -0.023,0.025 -0.043,0.05 -0.061,0.077 -0.056,0.082 -0.093,0.176 -0.107,0.271 -0.004,0.022 -0.005,0.044 -0.006,0.066 l -0.002,6.486 c -0.534,0.589 -1.115,1.136 -1.728,1.626 -0.913,0.73 -1.913,1.352 -2.971,1.845 -0.867,0.405 -1.779,0.725 -2.711,0.952 -0.851,0.208 -1.731,0.34 -2.617,0.391 -0.912,0.053 -1.834,0.023 -2.741,-0.092 -0.741,-0.094 -1.479,-0.246 -2.194,-0.453 -1.274,-0.365 -2.494,-0.905 -3.628,-1.604 -0.824,-0.507 -1.603,-1.101 -2.316,-1.764 -0.687,-0.64 -1.317,-1.35 -1.871,-2.109 -0.505,-0.692 -0.951,-1.432 -1.327,-2.2 -0.375,-0.764 -0.684,-1.566 -0.919,-2.383 -0.201,-0.7 -0.351,-1.423 -0.446,-2.147 -0.12,-0.919 -0.153,-1.858 -0.099,-2.79 0.05,-0.838 0.171,-1.673 0.359,-2.482 0.201,-0.856 0.481,-1.7 0.833,-2.507 0.546,-1.253 1.265,-2.423 2.136,-3.479 0.317,-0.384 0.654,-0.753 1.002,-1.098 0.158,-0.157 0.32,-0.31 0.485,-0.459 h 6.121 c 0.051,0.08 0.099,0.161 0.143,0.245 0.037,0.071 0.072,0.144 0.104,0.218 0.026,0.062 0.05,0.128 0.073,0.193 0.132,0.394 0.159,0.784 0.077,1.127 -0.013,0.052 -0.028,0.105 -0.046,0.157 -0.024,0.068 -0.053,0.136 -0.087,0.201 -0.048,0.09 -0.105,0.174 -0.169,0.249 -0.035,0.041 -0.07,0.079 -0.107,0.116 l -0.024,0.026 c -0.025,0.029 -0.046,0.053 -0.066,0.079 -0.014,0.02 -0.02,0.029 -0.027,0.038 l -0.079,0.096 c -0.109,0.134 -0.212,0.277 -0.304,0.42 -0.207,0.318 -0.378,0.659 -0.506,1.013 -0.118,0.32 -0.202,0.651 -0.252,0.985 -0.085,0.571 -0.073,1.146 0.035,1.71 0.098,0.509 0.273,0.999 0.521,1.454 0.258,0.475 0.588,0.903 0.98,1.272 0.419,0.394 0.895,0.711 1.415,0.942 0.557,0.249 1.15,0.393 1.762,0.428 0.659,0.035 1.298,-0.05 1.914,-0.258 0.653,-0.221 1.268,-0.585 1.78,-1.05 0.541,-0.492 0.977,-1.106 1.261,-1.776 0.151,-0.357 0.26,-0.729 0.324,-1.106 0.072,-0.415 0.092,-0.843 0.058,-1.272 -0.031,-0.388 -0.108,-0.773 -0.227,-1.144 -0.11,-0.341 -0.257,-0.673 -0.441,-0.988 -0.103,-0.177 -0.216,-0.347 -0.335,-0.503 -0.037,-0.051 -0.077,-0.1 -0.117,-0.151 l -0.033,-0.04 -0.005,0.002 -0.049,-0.07 c -0.022,-0.028 -0.043,-0.054 -0.066,-0.08 l -0.076,-0.08 c -0.028,-0.03 -0.055,-0.062 -0.081,-0.094 -0.05,-0.063 -0.095,-0.131 -0.134,-0.202 -0.035,-0.064 -0.066,-0.131 -0.09,-0.199 -0.019,-0.053 -0.035,-0.105 -0.049,-0.158 -0.085,-0.34 -0.062,-0.729 0.066,-1.123 0.021,-0.066 0.045,-0.13 0.071,-0.193 0.03,-0.073 0.063,-0.145 0.1,-0.215 0.045,-0.088 0.096,-0.176 0.156,-0.269 h 7.162 l 0.002,7.527 c 10e-4,0.022 0.002,0.043 0.006,0.065 0.008,0.061 0.027,0.124 0.057,0.187 0.03,0.06 0.067,0.114 0.111,0.161 0.034,0.037 0.065,0.062 0.102,0.09 l 0.062,0.048 c 0.087,0.065 0.173,0.126 0.262,0.183 0.487,0.316 1.027,0.526 1.563,0.608 0.154,0.024 0.312,0.037 0.482,0.04 0.037,0.001 0.076,0 0.113,-0.001 0.062,-0.002 0.124,-0.005 0.186,-0.01 0.123,-0.011 0.247,-0.029 0.367,-0.053 0.338,-0.071 0.654,-0.2 0.939,-0.383 0.11,-0.07 0.214,-0.149 0.31,-0.231 0.031,-0.026 0.061,-0.053 0.09,-0.081 l 0.029,-0.028 -0.07,-0.106 c 0.003,-0.003 0.007,-0.007 0.01,-0.01 l 0.071,0.101 0.215,-0.169 c 0.13,-0.101 0.243,-0.179 0.358,-0.25 0.24,-0.146 0.497,-0.266 0.763,-0.355 0.656,-0.219 1.363,-0.253 2.04,-0.097 0.374,0.086 0.731,0.229 1.059,0.424 C -0.581,-0.571 -0.268,-0.309 0,0" /></g><g
+           transform="translate(41.5882,22.8337)"
+           id="g24"><path
+             id="path26"
+             style="fill:white;fill-opacity:0.5;fill-rule:nonzero;stroke:none"
+             d="M 0,0 V 0 L 0.053,0.044 0.013,0.018 Z" /></g><g
+           transform="translate(28.5515,3.2736)"
+           id="g28"><path
+             id="path30"
+             style="fill:white;fill-opacity:0.5;fill-rule:nonzero;stroke:none"
+             d="m 0,0 c 0.934,-0.757 1.959,-1.398 3.045,-1.906 0.84,-0.39 1.72,-0.702 2.615,-0.927 0.883,-0.222 1.796,-0.362 2.714,-0.416 0.944,-0.054 1.897,-0.019 2.832,0.106 0.707,0.094 1.414,0.242 2.102,0.439 1.276,0.368 2.496,0.908 3.628,1.605 0.798,0.492 1.556,1.065 2.251,1.704 0.724,0.666 1.384,1.408 1.962,2.205 0.468,0.646 0.886,1.334 1.242,2.044 0.409,0.815 0.742,1.672 0.99,2.547 0.179,0.628 0.316,1.274 0.409,1.921 0.14,0.979 0.182,1.98 0.123,2.973 -0.048,0.819 -0.165,1.638 -0.348,2.433 -0.2,0.872 -0.484,1.732 -0.843,2.556 -0.538,1.234 -1.247,2.392 -2.106,3.442 -0.313,0.381 -0.648,0.752 -0.998,1.1 l -0.52,0.493 H 12.982 L 12.831,22.068 C 12.797,22.003 12.763,21.931 12.732,21.857 12.705,21.794 12.681,21.73 12.659,21.665 12.527,21.268 12.5,20.878 12.582,20.536 c 0.012,-0.053 0.027,-0.105 0.045,-0.156 0.024,-0.069 0.054,-0.137 0.088,-0.202 0.046,-0.086 0.103,-0.171 0.169,-0.251 0.027,-0.032 0.056,-0.062 0.086,-0.093 l 0.045,-0.048 c 0.023,-0.025 0.044,-0.051 0.065,-0.078 l -0.046,-0.082 0.061,0.057 0.003,-0.002 0.022,-0.028 -10e-4,-10e-4 0.01,-0.015 0.004,0.003 0.05,-0.062 c 0.171,-0.212 0.322,-0.433 0.451,-0.659 0.275,-0.478 0.469,-0.996 0.575,-1.538 0.141,-0.718 0.124,-1.472 -0.05,-2.181 -0.152,-0.623 -0.431,-1.228 -0.807,-1.751 -0.303,-0.42 -0.667,-0.79 -1.082,-1.1 C 11.868,12.048 11.425,11.81 10.954,11.64 10.456,11.461 9.935,11.362 9.406,11.345 8.955,11.331 8.505,11.376 8.07,11.48 c -0.494,0.118 -0.965,0.308 -1.399,0.565 -0.509,0.301 -0.958,0.685 -1.333,1.14 -0.393,0.477 -0.692,1.014 -0.89,1.596 -0.218,0.645 -0.305,1.348 -0.25,2.031 0.029,0.355 0.095,0.704 0.194,1.037 0.114,0.383 0.273,0.751 0.474,1.095 0.095,0.164 0.207,0.334 0.334,0.503 0.038,0.051 0.078,0.102 0.118,0.151 L 5.45,19.56 v 0 l -0.101,0.074 0.055,0.073 c 0.023,0.028 0.045,0.054 0.066,0.078 l 0.076,0.08 c 0.03,0.031 0.057,0.062 0.082,0.095 0.05,0.063 0.095,0.131 0.135,0.203 0.034,0.063 0.065,0.13 0.09,0.199 0.019,0.052 0.034,0.103 0.047,0.156 0.086,0.341 0.063,0.73 -0.065,1.124 -0.021,0.065 -0.044,0.129 -0.071,0.193 -0.033,0.082 -0.071,0.162 -0.113,0.24 -0.044,0.084 -0.092,0.165 -0.143,0.244 h -7.162 l -0.002,-7.532 c -0.001,-0.02 -0.003,-0.041 -0.006,-0.06 -0.015,-0.099 -0.052,-0.192 -0.107,-0.272 -0.018,-0.027 -0.039,-0.052 -0.061,-0.076 -0.035,-0.037 -0.064,-0.061 -0.102,-0.09 -0.02,-0.016 -0.042,-0.033 -0.065,-0.05 -0.122,-0.091 -0.254,-0.181 -0.393,-0.264 -0.619,-0.37 -1.29,-0.565 -1.942,-0.565 -0.093,-0.003 -0.179,0.003 -0.268,0.011 -0.11,0.01 -0.221,0.025 -0.33,0.046 -0.36,0.071 -0.709,0.213 -1.008,0.411 -0.109,0.072 -0.213,0.152 -0.309,0.237 -0.029,0.026 -0.058,0.052 -0.085,0.079 h -0.026 l -0.022,0.042 -0.036,0.027 c -0.035,0.03 -0.061,0.051 -0.087,0.072 l -0.073,0.059 c -0.212,0.162 -0.427,0.296 -0.648,0.404 -0.416,0.204 -0.861,0.328 -1.324,0.367 -0.345,0.028 -0.695,0.012 -1.037,-0.053 -0.259,-0.05 -0.511,-0.126 -0.75,-0.228 -0.498,-0.211 -0.954,-0.534 -1.32,-0.936 -0.345,-0.381 -0.614,-0.838 -0.779,-1.322 -0.154,-0.455 -0.218,-0.933 -0.191,-1.421 0.026,-0.462 0.136,-0.909 0.326,-1.327 0.175,-0.386 0.412,-0.738 0.706,-1.047 0.283,-0.296 0.609,-0.543 0.97,-0.734 0.319,-0.168 0.661,-0.289 1.015,-0.36 0.449,-0.089 0.906,-0.095 1.357,-0.021 0.246,0.041 0.49,0.108 0.725,0.198 0.254,0.098 0.499,0.225 0.727,0.377 0.113,0.075 0.229,0.161 0.343,0.257 l 0.068,0.054 0.016,0.046 0.041,0.002 c 0.012,0.013 0.02,0.02 0.029,0.028 l 0.055,0.052 c 0.051,0.043 0.1,0.084 0.151,0.124 0.104,0.08 0.212,0.151 0.322,0.213 0.28,0.159 0.589,0.268 0.917,0.324 0.152,0.026 0.31,0.04 0.48,0.043 h 0.109 C -4.1,9.429 -3.938,9.414 -3.782,9.389 -3.261,9.303 -2.734,9.095 -2.256,8.787 -2.167,8.73 -2.08,8.668 -1.996,8.604 l 0.053,-0.04 c 0.048,-0.037 0.077,-0.061 0.111,-0.097 0.047,-0.05 0.084,-0.104 0.113,-0.162 0.029,-0.062 0.048,-0.125 0.057,-0.187 0.003,-0.02 0.005,-0.04 0.006,-0.06 L -1.654,8.021 V 1.566 C -1.142,1.001 -0.585,0.474 0,0" /></g><g
+           transform="translate(22.2707,17.571)"
+           id="g32"><path
+             id="path34"
+             style="fill:white;fill-opacity:0.5;fill-rule:nonzero;stroke:none"
+             d="m 0,0 v -0.008 l 0.011,0.019 v 0 z" /></g><g
+           transform="translate(40.657,22.3049)"
+           id="g36"><path
+             id="path38"
+             style="fill:white;fill-opacity:0.5;fill-rule:nonzero;stroke:none"
+             d="M 0,0 Z" /></g><g
+           transform="translate(49.1867,28.5134)"
+           id="g40"><path
+             id="path42"
+             style="fill:white;fill-opacity:0.5;fill-rule:nonzero;stroke:none"
+             d="m 0,0 c 0.721,0.884 1.337,1.848 1.832,2.865 0.359,0.739 0.659,1.516 0.89,2.311 0.244,0.839 0.413,1.706 0.503,2.579 0.173,1.678 0.063,3.352 -0.329,4.976 -0.323,1.339 -0.834,2.625 -1.518,3.823 -0.416,0.728 -0.898,1.425 -1.432,2.072 -0.597,0.724 -1.268,1.394 -1.993,1.994 -0.636,0.525 -1.32,0.999 -2.034,1.409 -0.75,0.431 -1.539,0.796 -2.345,1.086 -1,0.359 -2.041,0.609 -3.095,0.743 -0.401,0.051 -0.808,0.086 -1.209,0.104 -0.205,0.009 -0.41,0.014 -0.615,0.015 h -0.098 c -0.479,-0.003 -0.943,-0.026 -1.379,-0.069 -0.825,-0.08 -1.65,-0.231 -2.451,-0.45 -0.861,-0.235 -1.705,-0.551 -2.509,-0.941 -0.765,-0.371 -1.502,-0.811 -2.191,-1.308 -0.571,-0.411 -1.115,-0.866 -1.618,-1.352 -0.236,-0.227 -0.471,-0.469 -0.698,-0.72 v -6.121 c 0.079,-0.052 0.161,-0.1 0.245,-0.144 0.07,-0.037 0.143,-0.071 0.216,-0.102 0.064,-0.027 0.13,-0.051 0.195,-0.074 0.394,-0.132 0.784,-0.158 1.127,-0.077 0.053,0.012 0.105,0.027 0.156,0.045 0.069,0.024 0.136,0.054 0.201,0.088 0.088,0.047 0.173,0.104 0.251,0.169 0.04,0.034 0.078,0.071 0.116,0.107 l 0.029,0.028 c 0.035,0.029 0.055,0.046 0.077,0.063 l 0.107,0.085 c 0.164,0.132 0.314,0.24 0.466,0.338 0.319,0.205 0.662,0.373 1.018,0.502 0.317,0.115 0.649,0.198 0.987,0.245 0.56,0.082 1.126,0.068 1.681,-0.038 0.51,-0.098 1,-0.273 1.455,-0.521 0.473,-0.257 0.901,-0.587 1.272,-0.981 0.394,-0.418 0.711,-0.894 0.942,-1.414 0.248,-0.558 0.392,-1.151 0.427,-1.762 0.037,-0.645 -0.052,-1.307 -0.258,-1.914 -0.22,-0.652 -0.583,-1.267 -1.049,-1.781 -0.491,-0.54 -1.105,-0.976 -1.776,-1.26 -0.352,-0.149 -0.724,-0.259 -1.106,-0.325 -0.421,-0.072 -0.849,-0.091 -1.272,-0.057 -0.371,0.03 -0.739,0.1 -1.09,0.21 -0.365,0.113 -0.716,0.267 -1.042,0.458 -0.167,0.096 -0.336,0.209 -0.503,0.334 -0.051,0.039 -0.102,0.078 -0.151,0.118 l -0.108,0.086 c -0.028,0.022 -0.053,0.043 -0.079,0.065 -0.03,0.028 -0.056,0.054 -0.082,0.079 -0.029,0.026 -0.061,0.054 -0.094,0.081 -0.064,0.05 -0.132,0.095 -0.201,0.133 -0.064,0.035 -0.132,0.065 -0.201,0.091 -0.051,0.018 -0.101,0.034 -0.154,0.046 C -20.829,6.024 -21.219,6 -21.613,5.873 -21.677,5.852 -21.742,5.828 -21.806,5.802 -21.887,5.768 -21.967,5.73 -22.045,5.689 -22.129,5.645 -22.21,5.598 -22.289,5.546 v -7.162 l 7.532,-0.003 c 0.02,-0.001 0.04,-0.002 0.06,-0.005 0.098,-0.015 0.192,-0.052 0.272,-0.107 0.026,-0.018 0.053,-0.039 0.076,-0.062 0.035,-0.033 0.058,-0.061 0.085,-0.096 l 0.053,-0.068 c 0.094,-0.125 0.184,-0.257 0.267,-0.395 0.362,-0.608 0.557,-1.269 0.563,-1.912 0.002,-0.099 -0.002,-0.198 -0.01,-0.298 -0.009,-0.11 -0.025,-0.221 -0.046,-0.33 -0.072,-0.362 -0.213,-0.711 -0.41,-1.008 -0.074,-0.11 -0.153,-0.214 -0.238,-0.309 -0.026,-0.029 -0.053,-0.057 -0.08,-0.086 l -0.025,-0.026 v 10e-4 l -0.18,-0.227 c -0.083,-0.106 -0.161,-0.219 -0.23,-0.334 -0.143,-0.233 -0.26,-0.483 -0.349,-0.744 -0.223,-0.654 -0.261,-1.359 -0.109,-2.037 0.084,-0.378 0.229,-0.742 0.429,-1.083 0.208,-0.351 0.47,-0.665 0.779,-0.932 0.323,-0.28 0.687,-0.5 1.081,-0.656 0.429,-0.17 0.884,-0.257 1.352,-0.26 h 0.026 c 0.482,0 0.951,0.09 1.397,0.269 0.473,0.19 0.913,0.483 1.274,0.848 0.383,0.388 0.682,0.862 0.865,1.371 0.203,0.566 0.267,1.184 0.184,1.786 -0.066,0.477 -0.221,0.931 -0.461,1.349 -0.117,0.204 -0.256,0.4 -0.413,0.584 l -0.073,0.089 0.005,0.01 -0.002,0.006 0.007,0.002 0.017,0.033 0.035,0.065 -0.091,-0.085 -0.053,0.057 c -0.051,0.06 -0.101,0.12 -0.148,0.182 -0.091,0.121 -0.173,0.253 -0.243,0.391 -0.067,0.13 -0.125,0.27 -0.172,0.417 -0.038,0.116 -0.068,0.239 -0.091,0.363 -0.018,0.097 -0.031,0.196 -0.039,0.294 -0.052,0.624 0.082,1.281 0.385,1.901 0.109,0.221 0.239,0.436 0.386,0.639 0.031,0.043 0.064,0.085 0.096,0.127 0.032,0.041 0.055,0.069 0.09,0.102 0.025,0.023 0.051,0.045 0.078,0.063 0.079,0.054 0.172,0.091 0.27,0.106 0.02,0.003 0.04,0.004 0.06,0.005 l 0.038,0.003 h 6.454 C -0.981,-1.113 -0.465,-0.57 0,0" /></g>
+          </g></g></g></svg>

apps/android/app/src/main/assets/index.html

@@ -0,0 +1,24 @@
+<html>
+	<head>
+		<meta name="apple-mobile-web-app-capable" content="yes" />
+		<meta name="viewport" content="width=device-width, initial-scale=1, maximum-scale=1.0, minimum-scale=1.0, user-scalable=no, minimal-ui">
+		<link rel="stylesheet" href="main.css">
+		<script src="roslib.js"></script>
+	</head>
+	<body>
+		<div class="telemetry"><span class="mode">DISCONNECTED</span></div>
+		<div class="battery"></div>
+		<div class="logo"></div>
+		<div class="container">
+			<div class="stick stick-left">
+				<div class="stick-pointer"></div>
+			</div>
+			<div class="stick stick-right">
+				<div class="stick-pointer"></div>
+			</div>
+		</div>
+		<div class="notifications"></div>
+		<script src="main.js" type="text/javascript"></script>
+		<script src="telemetry.js" type="text/javascript"></script>
+	</body>
+</html>

apps/android/app/src/main/assets/main.css

@@ -0,0 +1,125 @@
+html, body {
+	margin: 0;
+	padding: 0;
+	user-select: none;
+	font-family: sans-serif;
+	background: #212121;
+	color: rgba(255, 255, 255, 0.9);
+}
+
+* {
+	user-select: none;
+}
+
+.stick {
+	border-radius: 50%;
+	width: 5cm;
+	height: 5cm;
+	position: relative;
+	transform: translateZ(0);
+	border: 4px solid rgba(255,255,255,.4);
+	box-shadow: 0 0 0 1px rgba(0,0,0,.2), inset 0 0 0 1px rgba(0,0,0,.2);
+}
+
+.stick-pointer {
+	position: absolute;
+	border-radius: 50%;
+	background-color: rgba(255,255,255,.25);
+	box-shadow: 0 0 10px rgba(0,0,0,.3);
+	width: 3cm;
+	height: 3cm;
+	margin-left: -1.5cm;
+	margin-top: -1.5cm;
+	top: 2.5cm;
+	left: 2.5cm;
+	pointer-events: none;
+	transform: translateZ(0);
+}
+
+.container {
+	display: flex;
+	justify-content: space-around;
+	align-items: center;
+	width: 100%;
+	height: 100%;
+}
+
+.telemetry {
+	position: absolute;
+	text-align: center;
+	width: 100%;
+	top: 30px;
+	font-size: 20px;
+	user-select: none;
+	pointer-events: none;
+}
+
+body.armed .telemetry .mode {
+	font-weight: bold;
+}
+
+@keyframes scale {
+	0% { transform: scale(1.0); }
+	50% { transform: scale(1.2); }
+	100% { transform: scale(1.0); }
+}
+
+.battery {
+	position: absolute;
+	text-align: center;
+	width: 100%;
+	bottom: 30px;
+	font-size: 20px;
+	user-select: none;
+	pointer-events: none;
+}
+
+body.low-battery .battery {
+	color: #ff554b;
+	animation: scale 0.3s 1 ease-in-out
+}
+
+.logo {
+	position: absolute;
+	background: url(clever.svg);
+	-webkit-background-size: 50px;
+	background-size: 50px;
+	width: 50px;
+	height: 50px;
+	top: 50%;
+	left: 50%;
+	margin-top: -25px;
+	margin-left: -25px;
+	font-size: 20px;
+	user-select: none;
+	pointer-events: none;
+}
+
+.notifications {
+	pointer-events: none;
+	position: absolute;
+	top: 0;
+	left: 0;
+	right: 0;
+	color: white;
+}
+
+.notifications.hidden {
+	transform: translateY(-100%);
+}
+
+.notifications.anim {
+	transition: transform 0.2s ease;
+}
+
+.notifications .item {
+	font-size: 4mm;
+	-webkit-text-size-adjust: none;
+	background: #fca83a;
+	padding: 3mm;
+	padding-bottom: 1.5mm;
+}
+
+.notifications .item:last-child {
+	padding-bottom: 3mm;
+}

apps/android/app/src/main/assets/main.js

@@ -0,0 +1,138 @@
+function throttle(func, ms) {
+	var isThrottled = false,
+		savedArgs,
+		savedThis;
+
+	function wrapper() {
+		if (isThrottled) {
+			savedArgs = arguments;
+			savedThis = this;
+			return;
+		}
+		func.apply(this, arguments);
+		isThrottled = true;
+		setTimeout(function() {
+			isThrottled = false;
+			if (savedArgs) {
+				wrapper.apply(savedThis, savedArgs);
+				savedArgs = savedThis = null;
+			}
+		}, ms);
+	}
+	return wrapper;
+}
+
+function postAppMessage(msg) {
+	if (window.webkit != undefined) {
+		if (window.webkit.messageHandlers.appInterface != undefined) {
+			window.webkit.messageHandlers.appInterface.postMessage(JSON.stringify(msg));
+		}
+	} else if (window.appInterface != undefined) {
+		window.appInterface.postMessage(JSON.stringify(msg));
+	}
+}
+
+function callNativeApp(name, msg) {
+	try {
+		postAppMessage(msg);
+		return true;
+	} catch(err) {
+		console.warn('The native context does not exist yet');
+		return false;
+	}
+}
+var rcLastPublish = null;
+
+function rcPublish() {
+	callNativeApp('control', controlMessage);
+	rcLastPublish = new Date();
+}
+
+rcPublishThrottled = throttle(rcPublish, 30);
+
+setInterval(function() {
+	if (rcLastPublish !== null && new Date() - rcLastPublish > 800) {
+		rcPublishThrottled();
+	}
+}, 50);
+
+var body = document.querySelector('body');
+var stickLeft = document.querySelector('.stick-left');
+var stickRight = document.querySelector('.stick-right');
+
+var controlMessage = { x: 0, y: 0, z: 0, r: 0 };
+
+function onStickTouchMove(touch) {
+	var target = touch.target;
+	var targetRect = target.getBoundingClientRect();
+	var stickPointer = target.querySelector('.stick-pointer');
+
+	var offsetX = touch.clientX - targetRect.left;
+	var offsetY = touch.clientY - targetRect.top;
+
+	var x = 2 * offsetX / targetRect.width;
+	var y = 2 * offsetY / targetRect.height;
+
+	x = Math.max(0, x);
+	x = Math.min(2, x);
+	y = Math.max(0, y);
+	y = Math.min(2, y);
+
+	stickPointer.style.left = (x * 50) + '%';
+	stickPointer.style.top = (y * 50) + '%';
+
+	x -= 1;
+	y = 1 - y;
+
+	if (target.matches('.stick-left')) {
+		controlMessage.z = Math.round((y + 1) * 500);
+		controlMessage.r = Math.round(x * 1000);
+	} else if (target.matches('.stick-right')) {
+		controlMessage.x = Math.round(y * 1000);
+		controlMessage.y = Math.round(x * 1000);
+	}
+}
+
+body.addEventListener('touchmove', function (e) {
+	e.preventDefault();
+});
+
+function stickTouchStart(e) {
+	setControlMode();
+	callNativeApp('controlStart');
+	onStickTouchMove(e.changedTouches[0]);
+	rcPublishThrottled();
+	e.stopPropagation();
+	e.preventDefault();
+}
+
+function stickTouchMove(e) {
+	for (touch of e.changedTouches) {
+		onStickTouchMove(touch);
+	}
+	//onStickTouchMove(e.changedTouches[0]);
+	rcPublishThrottled();
+	e.stopPropagation();
+	e.preventDefault();
+}
+
+function stickTouchEnd(e) {
+	var pointer = e.target.querySelector('.stick-pointer');
+	if (e.target.matches('.stick-left')) {
+		controlMessage.r = 0;
+		pointer.style.left = '50%';
+	} else if (e.target.matches('.stick-right')) {
+		controlMessage.x = 0;
+		controlMessage.y = 0;
+		pointer.style.left = '50%';
+		pointer.style.top = '50%';
+	}
+	rcPublishThrottled();
+}
+
+stickLeft.addEventListener('touchmove', stickTouchMove);
+stickRight.addEventListener('touchmove', stickTouchMove);
+stickLeft.addEventListener('touchstart', stickTouchStart);
+stickRight.addEventListener('touchstart', stickTouchStart);
+stickLeft.addEventListener('touchend', stickTouchEnd);
+stickRight.addEventListener('touchend', stickTouchEnd);

apps/android/app/src/main/assets/old.js

@@ -0,0 +1,142 @@
+function throttle(func, ms) {
+	var isThrottled = false,
+		savedArgs,
+		savedThis;
+
+	function wrapper() {
+		if (isThrottled) {
+			savedArgs = arguments;
+			savedThis = this;
+			return;
+		}
+		func.apply(this, arguments);
+		isThrottled = true;
+		setTimeout(function() {
+			isThrottled = false;
+			if (savedArgs) {
+				wrapper.apply(savedThis, savedArgs);
+				savedArgs = savedThis = null;
+			}
+		}, ms);
+	}
+	return wrapper;
+}
+
+function postAppMessage(msg) {
+    if (window.webkit != undefined) {
+        if (window.webkit.messageHandlers.appInterface != undefined) {
+            window.webkit.messageHandlers.appInterface.postMessage(JSON.stringify(msg));
+        }
+    }
+    else if (window.appInterface != undefined) {
+        window.appInterface.postMessage(JSON.stringify(msg));
+    }
+}
+
+function callNativeApp(name, msg) {
+	try {
+        postAppMessage(msg);
+		return true;
+	} catch(err) {
+		console.warn('The native context does not exist yet');
+		return false;
+	}
+}
+
+
+
+var rcLastPublish = null;
+
+function rcPublish() {
+	callNativeApp('control', controlMessage);
+	rcLastPublish = new Date();
+}
+
+rcPublishThrottled = throttle(rcPublish, 30);
+
+setInterval(function() {
+	if (rcLastPublish !== null && new Date() - rcLastPublish > 800) {
+		rcPublishThrottled();
+	}
+}, 50);
+
+var body = document.querySelector('body');
+var stickLeft = document.querySelector('.stick-left');
+var stickRight = document.querySelector('.stick-right');
+
+var controlMessage = { x: 0, y: 0, z: 0, r: 0 };
+
+function onStickTouchMove(touch) {
+	var target = touch.target;
+	var targetRect = target.getBoundingClientRect();
+	var stickPointer = target.querySelector('.stick-pointer');
+
+	var offsetX = touch.clientX - targetRect.left;
+	var offsetY = touch.clientY - targetRect.top;
+
+	var x = 2 * offsetX / targetRect.width;
+	var y = 2 * offsetY / targetRect.height;
+
+	x = Math.max(0, x);
+	x = Math.min(2, x);
+	y = Math.max(0, y);
+	y = Math.min(2, y);
+
+	stickPointer.style.left = (x * 50) + '%';
+	stickPointer.style.top = (y * 50) + '%';
+
+	x -= 1;
+	y = 1 - y;
+
+	if (target.matches('.stick-left')) {
+		controlMessage.z = Math.round((y + 1) * 500);
+		controlMessage.r = Math.round(x * 1000);
+	} else if (target.matches('.stick-right')) {
+		controlMessage.x = Math.round(y * 1000);
+		controlMessage.y = Math.round(x * 1000);
+	}
+}
+
+body.addEventListener('touchmove', function (e) {
+	e.preventDefault();
+});
+
+function stickTouchStart(e) {
+	setControlMode();
+	callNativeApp('controlStart');
+	onStickTouchMove(e.changedTouches[0]);
+	rcPublishThrottled();
+	e.stopPropagation();
+	e.preventDefault();
+}
+
+function stickTouchMove(e) {
+    for (touch of e.changedTouches) {
+        onStickTouchMove(touch);
+    }
+	//onStickTouchMove(e.changedTouches[0]);
+	rcPublishThrottled();
+	e.stopPropagation();
+	e.preventDefault();
+}
+
+function stickTouchEnd(e) {
+	var pointer = e.target.querySelector('.stick-pointer');
+	if (e.target.matches('.stick-left')) {
+		controlMessage.r = 0;
+		pointer.style.left = '50%';
+	} else if (e.target.matches('.stick-right')) {
+		controlMessage.x = 0;
+		controlMessage.y = 0;
+		pointer.style.left = '50%';
+		pointer.style.top = '50%';
+	}
+	rcPublishThrottled();
+}
+
+stickLeft.addEventListener('touchmove', stickTouchMove);
+stickRight.addEventListener('touchmove', stickTouchMove);
+stickLeft.addEventListener('touchstart', stickTouchStart);
+stickRight.addEventListener('touchstart', stickTouchStart);
+stickLeft.addEventListener('touchend', stickTouchEnd);
+stickRight.addEventListener('touchend', stickTouchEnd);

apps/android/app/src/main/assets/telemetry.js

@@ -0,0 +1,115 @@
+var url = 'ws://192.168.11.1:9090';
+var modeEl = document.querySelector('.telemetry .mode');
+var batteryEl = document.querySelector('.battery');
+var notificationsEl = document.querySelector('.notifications');
+
+var ros = new ROSLIB.Ros({ url: url });
+
+ros.on('connection', function () {
+	body.classList.add('connected');
+});
+
+ros.on('close', function () {
+	body.classList.remove('connected');
+	modeEl.classList.remove('armed');
+	modeEl.innerHTML = 'DISCONNECTED';
+	batteryEl.innerHTML = '';
+	setTimeout(function() {
+		modeEl.innerHTML = 'RECONNECTING';
+		ros.connect(url);
+	}, 2000);
+});
+
+var fcuState;
+
+new ROSLIB.Topic({
+	ros: ros,
+	name: '/state_latched',
+	messageType: 'mavros_msgs/State'
+}).subscribe(function(message) {
+	body.classList.toggle('fcu-disconnected', !message.connected);
+	body.classList.toggle('armed', message.armed);
+	fcuState = message;
+	modeEl.classList.toggle('armed', fcuState.armed);
+	modeEl.innerHTML = message.connected ? fcuState.mode : 'DISCONNECTED FROM FCU';
+	console.log('state', message);
+});
+
+function notifyLowBattery() {
+	console.log('low battery');
+	callNativeApp('lowBattery');
+	body.classList.remove('low-battery');
+	void body.offsetWidth; // trick for repeating animation
+	body.classList.add('low-battery');
+}
+
+notifyLowBatteryThrottled = throttle(notifyLowBattery, 15000);
+
+new ROSLIB.Topic({
+	ros: ros,
+	name: '/mavros/battery',
+	messageType: 'sensor_msgs/BatteryState',
+	throttle_rate: 5000
+}).subscribe(function(message) {
+	var LOW_BATTERY = 3.8;
+	batteryEl.innerHTML = (message.cell_voltage[0].toFixed(2) + ' V') || '';
+
+	if (message.cell_voltage[0] < LOW_BATTERY) {
+		notifyLowBatteryThrottled();
+	} else {
+		body.classList.remove('low-battery');
+	}
+});
+
+var notificationHideTimer;
+
+function notify(text, severity) {
+	var item = document.createElement('div');
+	item.innerHTML = text;
+	item.classList.add('item');
+	notificationsEl.prepend(item);
+	var itemHeight = item.offsetHeight;
+	notificationsEl.classList.remove('anim');
+	notificationsEl.style.transform = 'translateY(' + -itemHeight + 'px)';
+	setTimeout(function() {
+		notificationsEl.classList.add('anim');
+		notificationsEl.style.transform = 'translateY(0)';
+	}, 0);
+	clearTimeout(notificationHideTimer);
+	notificationHideTimer = setTimeout(function() {
+		notificationsEl.style.transform = '';
+		notificationsEl.classList.add('hidden');
+		setTimeout(function() {
+			notificationsEl.innerHTML = '';
+		}, 210);
+	}, 4000);
+}
+
+new ROSLIB.Topic({
+	ros: ros,
+	name: '/mavros/statustext/recv',
+	messageType: 'mavros_msgs/StatusText'
+}).subscribe(function(message) {
+	var BLACKLIST = ['CMD: ', 'PR: ', 'DROPPED', 'Clock skew detected', 'MANUAL CONTROL LOST'];
+	if (message.severity <= 4) {
+		if (BLACKLIST.some(function(e) {
+				return message.text.indexOf(e) != -1;
+			})) {
+			console.log('Filtered out message ' + message.text);
+			return;
+		}
+		notify(message.text, message.severity);
+		callNativeApp('notification', message);
+	}
+});
+
+var setMode = new ROSLIB.Service({
+	ros: ros,
+	name : '/mavros/set_mode',
+	serviceType : 'mavros_msgs/SetMode'
+});
+
+function setControlMode() {
+	var CONTROL_MODE = 'STABILIZED';
+	setMode.callService(new ROSLIB.ServiceRequest({ custom_mode: CONTROL_MODE }));
+}

apps/android/app/src/main/java/express/copter/cleverrc/MainActivity.kt

@@ -0,0 +1,86 @@
+package express.copter.cleverrc
+
+import android.content.Context
+import android.os.Build
+import android.support.v7.app.AppCompatActivity
+import android.os.Bundle
+import android.view.View
+import android.view.WindowManager
+import android.webkit.JavascriptInterface
+import kotlinx.android.synthetic.main.activity_main.*
+import org.json.JSONObject
+import java.net.DatagramPacket
+import java.net.DatagramSocket
+import java.net.InetAddress
+import java.nio.ByteBuffer
+
+fun pack(x: Short, y: Short, z: Short, r: Short): ByteArray {
+    val pump_on_buf: ByteBuffer = ByteBuffer.allocate(8)
+    pump_on_buf.putShort(r)
+    pump_on_buf.putShort(z)
+    pump_on_buf.putShort(y)
+    pump_on_buf.putShort(x)
+
+    return pump_on_buf.array().reversedArray()
+}
+
+fun send(host: String, port: Int, data: ByteArray, senderPort: Int = 0): Boolean {
+    var ret = false
+    var socket: DatagramSocket? = null
+    try {
+        socket = DatagramSocket(senderPort)
+        val address = InetAddress.getByName(host)
+        val packet = DatagramPacket(data, data.size, address, port)
+        socket.send(packet)
+        ret = true
+    } catch (e: Exception) {
+        e.printStackTrace()
+    } finally {
+        socket?.close()
+    }
+    return ret
+}
+
+class MainActivity : AppCompatActivity() {
+
+    override fun onCreate(savedInstanceState: Bundle?) {
+        super.onCreate(savedInstanceState)
+        setContentView(R.layout.activity_main)
+        fullScreenCall()
+        main_web.loadUrl("file:///android_asset/index.html")
+
+        main_web.settings.apply {
+            domStorageEnabled = true
+            javaScriptEnabled = true
+            loadWithOverviewMode = true
+            useWideViewPort = true
+            setSupportZoom(false)
+        }
+
+        main_web.addJavascriptInterface(WebAppInterface(this), "appInterface")
+    }
+
+    private fun fullScreenCall() {
+        window.setFlags(
+            WindowManager.LayoutParams.FLAG_FULLSCREEN,
+            WindowManager.LayoutParams.FLAG_FULLSCREEN
+        )
+        if (Build.VERSION.SDK_INT < 19) {
+            val v = this.window.decorView
+            v.systemUiVisibility = View.GONE
+        } else {
+            //for higher api versions.
+            val decorView = window.decorView
+            val uiOptions = View.SYSTEM_UI_FLAG_HIDE_NAVIGATION or View.SYSTEM_UI_FLAG_IMMERSIVE_STICKY
+            decorView.systemUiVisibility = uiOptions
+        }
+    }
+}
+
+class WebAppInterface(c: Context) {
+    @JavascriptInterface
+    public fun postMessage(message: String) {
+        val data = JSONObject(message)
+        send("255.255.255.255", 35602, pack(data.getInt("x").toShort(), data.getInt("y").toShort(), data.getInt("z").toShort(), data.getInt("r").toShort()))
+    }
+}

apps/android/app/src/main/res/drawable-v24/ic_launcher_foreground.xml

@@ -0,0 +1,34 @@
+<vector xmlns:android="http://schemas.android.com/apk/res/android"
+        xmlns:aapt="http://schemas.android.com/aapt"
+        android:width="108dp"
+        android:height="108dp"
+        android:viewportHeight="108"
+        android:viewportWidth="108">
+    <path
+            android:fillType="evenOdd"
+            android:pathData="M32,64C32,64 38.39,52.99 44.13,50.95C51.37,48.37 70.14,49.57 70.14,49.57L108.26,87.69L108,109.01L75.97,107.97L32,64Z"
+            android:strokeColor="#00000000"
+            android:strokeWidth="1">
+        <aapt:attr name="android:fillColor">
+            <gradient
+                    android:endX="78.5885"
+                    android:endY="90.9159"
+                    android:startX="48.7653"
+                    android:startY="61.0927"
+                    android:type="linear">
+                <item
+                        android:color="#44000000"
+                        android:offset="0.0"/>
+                <item
+                        android:color="#00000000"
+                        android:offset="1.0"/>
+            </gradient>
+        </aapt:attr>
+    </path>
+    <path
+            android:fillColor="#FFFFFF"
+            android:fillType="nonZero"
+            android:pathData="M66.94,46.02L66.94,46.02C72.44,50.07 76,56.61 76,64L32,64C32,56.61 35.56,50.11 40.98,46.06L36.18,41.19C35.45,40.45 35.45,39.3 36.18,38.56C36.91,37.81 38.05,37.81 38.78,38.56L44.25,44.05C47.18,42.57 50.48,41.71 54,41.71C57.48,41.71 60.78,42.57 63.68,44.05L69.11,38.56C69.84,37.81 70.98,37.81 71.71,38.56C72.44,39.3 72.44,40.45 71.71,41.19L66.94,46.02ZM62.94,56.92C64.08,56.92 65,56.01 65,54.88C65,53.76 64.08,52.85 62.94,52.85C61.8,52.85 60.88,53.76 60.88,54.88C60.88,56.01 61.8,56.92 62.94,56.92ZM45.06,56.92C46.2,56.92 47.13,56.01 47.13,54.88C47.13,53.76 46.2,52.85 45.06,52.85C43.92,52.85 43,53.76 43,54.88C43,56.01 43.92,56.92 45.06,56.92Z"
+            android:strokeColor="#00000000"
+            android:strokeWidth="1"/>
+</vector>

apps/android/app/src/main/res/drawable/ic_launcher_background.xml

@@ -0,0 +1,74 @@
+<?xml version="1.0" encoding="utf-8"?>
+<vector
+        xmlns:android="http://schemas.android.com/apk/res/android"
+        android:height="108dp"
+        android:width="108dp"
+        android:viewportHeight="108"
+        android:viewportWidth="108">
+    <path android:fillColor="#008577"
+          android:pathData="M0,0h108v108h-108z"/>
+    <path android:fillColor="#00000000" android:pathData="M9,0L9,108"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M19,0L19,108"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M29,0L29,108"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M39,0L39,108"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M49,0L49,108"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M59,0L59,108"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M69,0L69,108"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M79,0L79,108"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M89,0L89,108"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M99,0L99,108"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M0,9L108,9"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M0,19L108,19"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M0,29L108,29"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M0,39L108,39"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M0,49L108,49"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M0,59L108,59"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M0,69L108,69"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M0,79L108,79"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M0,89L108,89"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M0,99L108,99"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M19,29L89,29"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M19,39L89,39"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M19,49L89,49"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M19,59L89,59"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M19,69L89,69"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M19,79L89,79"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M29,19L29,89"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M39,19L39,89"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M49,19L49,89"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M59,19L59,89"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M69,19L69,89"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M79,19L79,89"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+</vector>

apps/android/app/src/main/res/layout/activity_main.xml

@@ -0,0 +1,14 @@
+<?xml version="1.0" encoding="utf-8"?>
+<FrameLayout
+        xmlns:android="http://schemas.android.com/apk/res/android"
+        xmlns:tools="http://schemas.android.com/tools"
+        xmlns:app="http://schemas.android.com/apk/res-auto"
+        android:layout_width="match_parent"
+        android:layout_height="match_parent"
+        tools:context=".MainActivity">
+
+    <WebView
+            android:layout_width="match_parent"
+            android:layout_height="match_parent"
+            android:id="@+id/main_web"/>
+</FrameLayout>

apps/android/app/src/main/res/mipmap-anydpi-v26/ic_launcher.xml

@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="utf-8"?>
+<adaptive-icon xmlns:android="http://schemas.android.com/apk/res/android">
+    <background android:drawable="@color/ic_launcher_background"/>
+    <foreground android:drawable="@mipmap/ic_launcher_foreground"/>
+</adaptive-icon>

apps/android/app/src/main/res/mipmap-anydpi-v26/ic_launcher_round.xml

@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="utf-8"?>
+<adaptive-icon xmlns:android="http://schemas.android.com/apk/res/android">
+    <background android:drawable="@color/ic_launcher_background"/>
+    <foreground android:drawable="@mipmap/ic_launcher_foreground"/>
+</adaptive-icon>

apps/android/app/src/main/res/values/colors.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="utf-8"?>
+<resources>
+    <color name="colorPrimary">#fafafa</color>
+    <color name="colorPrimaryDark">#d1d1d1</color>
+    <color name="colorAccent">#757575</color>
+</resources>

apps/android/app/src/main/res/values/ic_launcher_background.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="utf-8"?>
+<resources>
+    <color name="ic_launcher_background">#FFFFFF</color>
+</resources>

apps/android/app/src/main/res/values/strings.xml

@@ -0,0 +1,3 @@
+<resources>
+    <string name="app_name">CLEVER RC</string>
+</resources>

apps/android/app/src/main/res/values/styles.xml

@@ -0,0 +1,18 @@
+<resources>
+
+    <!-- Base application theme. -->
+    <style name="AppTheme" parent="Theme.AppCompat.Light.DarkActionBar">
+        <!-- Customize your theme here. -->
+        <item name="colorPrimary">@color/colorPrimary</item>
+        <item name="colorPrimaryDark">@color/colorPrimaryDark</item>
+        <item name="colorAccent">@color/colorAccent</item>
+    </style>
+
+    <style name="NoUiAppTheme"
+           parent="Theme.AppCompat.NoActionBar">
+        <item name="colorPrimary">@color/colorPrimary</item>
+        <item name="colorPrimaryDark">@color/colorPrimaryDark</item>
+        <item name="colorAccent">@color/colorAccent</item>
+    </style>
+
+</resources>

apps/android/app/src/test/java/express/copter/cleverrc/ExampleUnitTest.kt

@@ -0,0 +1,17 @@
+package express.copter.cleverrc
+
+import org.junit.Test
+
+import org.junit.Assert.*
+
+/**
+ * Example local unit test, which will execute on the development machine (host).
+ *
+ * See [testing documentation](http://d.android.com/tools/testing).
+ */
+class ExampleUnitTest {
+    @Test
+    fun addition_isCorrect() {
+        assertEquals(4, 2 + 2)
+    }
+}

apps/android/build.gradle

@@ -0,0 +1,27 @@
+// Top-level build file where you can add configuration options common to all sub-projects/modules.
+
+buildscript {
+    ext.kotlin_version = '1.2.71'
+    repositories {
+        google()
+        jcenter()
+    }
+    dependencies {
+        classpath 'com.android.tools.build:gradle:3.2.1'
+        classpath "org.jetbrains.kotlin:kotlin-gradle-plugin:$kotlin_version"
+
+        // NOTE: Do not place your application dependencies here; they belong
+        // in the individual module build.gradle files
+    }
+}
+
+allprojects {
+    repositories {
+        google()
+        jcenter()
+    }
+}
+
+task clean(type: Delete) {
+    delete rootProject.buildDir
+}

apps/android/gradle.properties

@@ -0,0 +1,15 @@
+# Project-wide Gradle settings.
+# IDE (e.g. Android Studio) users:
+# Gradle settings configured through the IDE *will override*
+# any settings specified in this file.
+# For more details on how to configure your build environment visit
+# http://www.gradle.org/docs/current/userguide/build_environment.html
+# Specifies the JVM arguments used for the daemon process.
+# The setting is particularly useful for tweaking memory settings.
+org.gradle.jvmargs=-Xmx1536m
+# When configured, Gradle will run in incubating parallel mode.
+# This option should only be used with decoupled projects. More details, visit
+# http://www.gradle.org/docs/current/userguide/multi_project_builds.html#sec:decoupled_projects
+# org.gradle.parallel=true
+# Kotlin code style for this project: "official" or "obsolete":
+kotlin.code.style=official

apps/android/gradle/wrapper/gradle-wrapper.properties

@@ -0,0 +1,5 @@
+distributionBase=GRADLE_USER_HOME
+distributionPath=wrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-4.6-all.zip
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=wrapper/dists

apps/android/gradlew

@@ -0,0 +1,172 @@
+#!/usr/bin/env sh
+
+##############################################################################
+##
+##  Gradle start up script for UN*X
+##
+##############################################################################
+
+# Attempt to set APP_HOME
+# Resolve links: $0 may be a link
+PRG="$0"
+# Need this for relative symlinks.
+while [ -h "$PRG" ] ; do
+    ls=`ls -ld "$PRG"`
+    link=`expr "$ls" : '.*-> \(.*\)$'`
+    if expr "$link" : '/.*' > /dev/null; then
+        PRG="$link"
+    else
+        PRG=`dirname "$PRG"`"/$link"
+    fi
+done
+SAVED="`pwd`"
+cd "`dirname \"$PRG\"`/" >/dev/null
+APP_HOME="`pwd -P`"
+cd "$SAVED" >/dev/null
+
+APP_NAME="Gradle"
+APP_BASE_NAME=`basename "$0"`
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS=""
+
+# Use the maximum available, or set MAX_FD != -1 to use that value.
+MAX_FD="maximum"
+
+warn () {
+    echo "$*"
+}
+
+die () {
+    echo
+    echo "$*"
+    echo
+    exit 1
+}
+
+# OS specific support (must be 'true' or 'false').
+cygwin=false
+msys=false
+darwin=false
+nonstop=false
+case "`uname`" in
+  CYGWIN* )
+    cygwin=true
+    ;;
+  Darwin* )
+    darwin=true
+    ;;
+  MINGW* )
+    msys=true
+    ;;
+  NONSTOP* )
+    nonstop=true
+    ;;
+esac
+
+CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
+
+# Determine the Java command to use to start the JVM.
+if [ -n "$JAVA_HOME" ] ; then
+    if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
+        # IBM's JDK on AIX uses strange locations for the executables
+        JAVACMD="$JAVA_HOME/jre/sh/java"
+    else
+        JAVACMD="$JAVA_HOME/bin/java"
+    fi
+    if [ ! -x "$JAVACMD" ] ; then
+        die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+    fi
+else
+    JAVACMD="java"
+    which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+fi
+
+# Increase the maximum file descriptors if we can.
+if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then
+    MAX_FD_LIMIT=`ulimit -H -n`
+    if [ $? -eq 0 ] ; then
+        if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then
+            MAX_FD="$MAX_FD_LIMIT"
+        fi
+        ulimit -n $MAX_FD
+        if [ $? -ne 0 ] ; then
+            warn "Could not set maximum file descriptor limit: $MAX_FD"
+        fi
+    else
+        warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT"
+    fi
+fi
+
+# For Darwin, add options to specify how the application appears in the dock
+if $darwin; then
+    GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\""
+fi
+
+# For Cygwin, switch paths to Windows format before running java
+if $cygwin ; then
+    APP_HOME=`cygpath --path --mixed "$APP_HOME"`
+    CLASSPATH=`cygpath --path --mixed "$CLASSPATH"`
+    JAVACMD=`cygpath --unix "$JAVACMD"`
+
+    # We build the pattern for arguments to be converted via cygpath
+    ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null`
+    SEP=""
+    for dir in $ROOTDIRSRAW ; do
+        ROOTDIRS="$ROOTDIRS$SEP$dir"
+        SEP="|"
+    done
+    OURCYGPATTERN="(^($ROOTDIRS))"
+    # Add a user-defined pattern to the cygpath arguments
+    if [ "$GRADLE_CYGPATTERN" != "" ] ; then
+        OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)"
+    fi
+    # Now convert the arguments - kludge to limit ourselves to /bin/sh
+    i=0
+    for arg in "$@" ; do
+        CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -`
+        CHECK2=`echo "$arg"|egrep -c "^-"`                                 ### Determine if an option
+
+        if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then                    ### Added a condition
+            eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"`
+        else
+            eval `echo args$i`="\"$arg\""
+        fi
+        i=$((i+1))
+    done
+    case $i in
+        (0) set -- ;;
+        (1) set -- "$args0" ;;
+        (2) set -- "$args0" "$args1" ;;
+        (3) set -- "$args0" "$args1" "$args2" ;;
+        (4) set -- "$args0" "$args1" "$args2" "$args3" ;;
+        (5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;;
+        (6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;;
+        (7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;;
+        (8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;;
+        (9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;;
+    esac
+fi
+
+# Escape application args
+save () {
+    for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done
+    echo " "
+}
+APP_ARGS=$(save "$@")
+
+# Collect all arguments for the java command, following the shell quoting and substitution rules
+eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS"
+
+# by default we should be in the correct project dir, but when run from Finder on Mac, the cwd is wrong
+if [ "$(uname)" = "Darwin" ] && [ "$HOME" = "$PWD" ]; then
+  cd "$(dirname "$0")"
+fi
+
+exec "$JAVACMD" "$@"

apps/android/gradlew.bat

@@ -0,0 +1,84 @@
+@if "%DEBUG%" == "" @echo off
+@rem ##########################################################################
+@rem
+@rem  Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%" == "" set DIRNAME=.
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS=
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if "%ERRORLEVEL%" == "0" goto init
+
+echo.
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto init
+
+echo.
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:init
+@rem Get command-line arguments, handling Windows variants
+
+if not "%OS%" == "Windows_NT" goto win9xME_args
+
+:win9xME_args
+@rem Slurp the command line arguments.
+set CMD_LINE_ARGS=
+set _SKIP=2
+
+:win9xME_args_slurp
+if "x%~1" == "x" goto execute
+
+set CMD_LINE_ARGS=%*
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS%
+
+:end
+@rem End local scope for the variables with windows NT shell
+if "%ERRORLEVEL%"=="0" goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+if  not "" == "%GRADLE_EXIT_CONSOLE%" exit 1
+exit /b 1
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega

apps/android/settings.gradle

@@ -0,0 +1 @@
+include ':app'

apps/ios/README.md

@@ -1,5 +1,4 @@
-iOS-приложение для управления Клевером
---------------------------------------
+# iOS-приложение для управления Клевером
 
 Для установки зависимостей необходим [CocoaPods](https://cocoapods.org):
 
@@ -8,3 +7,11 @@ pod install
 ```
 
 Для разработки и сборки откройте в XCode файл `cleverrc.xcworkspace`.
+
+## Политика конфиденциальности
+
+App Store приложение CLEVER RC не собирает и не хранит каких-либо личных данных пользователя.
+
+## Privacy policy
+
+The App Store app CLEVER RC does not collect and store any personal user data.

apps/ios/cleverrc/Assets.xcassets/AppIcon.appiconset/Contents.json

@@ -145,8 +145,15 @@
       "size" : "44x44",
       "idiom" : "watch",
       "scale" : "2x",
-      "role" : "longLook",
-      "subtype" : "42mm"
+      "role" : "appLauncher",
+      "subtype" : "40mm"
+    },
+    {
+      "size" : "50x50",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "appLauncher",
+      "subtype" : "44mm"
     },
     {
       "size" : "86x86",
@@ -162,10 +169,24 @@
       "role" : "quickLook",
       "subtype" : "42mm"
     },
+    {
+      "size" : "108x108",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "quickLook",
+      "subtype" : "44mm"
+    },
     {
       "idiom" : "watch-marketing",
       "size" : "1024x1024",
       "scale" : "1x"
+    },
+    {
+      "size" : "44x44",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "longLook",
+      "subtype" : "42mm"
     }
   ],
   "info" : {

apps/ios/cleverrc/Info.plist

@@ -17,9 +17,9 @@
 	<key>CFBundlePackageType</key>
 	<string>APPL</string>
 	<key>CFBundleShortVersionString</key>
-	<string>1.1</string>
+	<string>1.2</string>
 	<key>CFBundleVersion</key>
-	<string>6</string>
+	<string>7</string>
 	<key>LSRequiresIPhoneOS</key>
 	<true/>
 	<key>UILaunchStoryboardName</key>

aruco_pose/CMakeLists.txt

@@ -1,8 +1,6 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0)
 project(aruco_pose)
 
-add_definitions(-std=c++11 -Wall -g)
-
 ## Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++11)
 
@@ -16,11 +14,25 @@ find_package(catkin REQUIRED COMPONENTS
   image_transport
   cv_bridge
   tf
-  #tf2
-  #tf2_ros
-  #aruco_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+  sensor_msgs
+  message_generation
+  dynamic_reconfigure
 )
 
+find_package(OpenCV 3 REQUIRED COMPONENTS core imgproc calib3d)
+if ("${OpenCV_VERSION_MINOR}" LESS "9")
+  message(STATUS "OpenCV version too low, using vendored ArUco package")
+  include(vendor/VendorOpenCV.cmake)
+else()
+  message(STATUS "Using system OpenCV ArUco package")
+  find_package(OpenCV 3 REQUIRED COMPONENTS aruco)
+endif()
+message(STATUS "OpenCV include dirs: ${OpenCV_INCLUDE_DIRS}")
+message(STATUS "OpenCV libraries: ${OpenCV_LIBRARIES}")
+
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
 
@@ -55,11 +67,12 @@ find_package(catkin REQUIRED COMPONENTS
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-#add_message_files(
-#  FILES
-#  Marker.msg
-#  MarkerArray.msg
-#)
+add_message_files(
+  FILES
+  Point2D.msg
+  Marker.msg
+  MarkerArray.msg
+)
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -76,10 +89,11 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-#generate_messages(
-#  DEPENDENCIES
-#  std_msgs  # Or other packages containing msgs
-#)
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  geometry_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -96,10 +110,9 @@ find_package(catkin REQUIRED COMPONENTS
 ##     and list every .cfg file to be processed
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
+generate_dynamic_reconfigure_options(
+  cfg/DetectorParams.cfg
+)
 
 ###################################
 ## catkin specific configuration ##
@@ -111,9 +124,9 @@ find_package(catkin REQUIRED COMPONENTS
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-#  INCLUDE_DIRS include
+  INCLUDE_DIRS DEPENDS OpenCV
   LIBRARIES aruco_pose
-#  CATKIN_DEPENDS other_catkin_pkg
+  CATKIN_DEPENDS message_runtime
 #  DEPENDS system_lib
 )
 
@@ -126,17 +139,17 @@ catkin_package(
 include_directories(
 # include
   ${catkin_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
 )
 
 ## Declare a C++ library
-add_library(${PROJECT_NAME}
-  src/aruco_pose.cpp
+add_library(aruco_pose
+  src/aruco_detect.cpp
+  src/aruco_map.cpp
+  src/draw.cpp
 )
 
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp ${PROJECT_NAME}_gencfg)
 
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
@@ -154,11 +167,9 @@ add_library(${PROJECT_NAME}
 # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
-link_directories(/opt/ros/kinetic/lib)
-
-target_link_libraries(${PROJECT_NAME}
+target_link_libraries(aruco_pose
   ${catkin_LIBRARIES}
-  "/opt/ros/kinetic/lib/libopencv_aruco3.so" # TODO: fix launch fails with .so loading
+  ${OpenCV_LIBRARIES}
 )
 
 #############
@@ -169,7 +180,7 @@ target_link_libraries(${PROJECT_NAME}
 # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
 
 ## Mark executable scripts (Python etc.) for installation
- ## in contrast to setup.py, you can choose the destination
+## in contrast to setup.py, you can choose the destination
 # install(PROGRAMS
 #   scripts/my_python_script
 #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -208,3 +219,13 @@ target_link_libraries(${PROJECT_NAME}
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_rostest(test/basic.test)
+  add_rostest(test/test_parser_pass.test)
+  add_rostest(test/test_parser_empty_map.test)
+  add_rostest(test/test_node_failure.test)
+  add_rostest(test/largemap.test)
+  add_rostest(test/crash_opencv.test)
+endif()

aruco_pose/README.md

@@ -0,0 +1,120 @@
+# Positioning with ArUco markers
+
+`aruco_pose` package consists of two nodelets: `aruco_detect` detects individual ArUco-markers and estimates their poses, `aruco_map` detects maps of markers using `aruco_detect` output.
+
+## Quick start
+
+To run a camera nodelet, markers and maps detector:
+
+```bash
+roslaunch aruco_pose sample.launch
+```
+
+You're going to need [`cv_camera`](http://wiki.ros.org/cv_camera) package installed.
+
+## aruco_detect nodelet
+
+`aruco_detect` detects ArUco markers on the image, publishes list of them (with poses), TF transformations, visualization markers and processed image for debugging.
+
+It's recommended to run it within the same nodelet manager with the camera nodelet (e. g. [`cv_camera`](http://wiki.ros.org/cv_camera)).
+
+### Parameters
+
+* `~dictionary` (*int*) – ArUco dictionary (default: 2)
+  * 0 = DICT_4X4_50
+  * 1 = DICT_4X4_100,
+  * 2 = DICT_4X4_250,
+  * 3 = DICT_4X4_1000,
+  * 4 = DICT_5X5_50,
+  * 5 = DICT_5X5_100,
+  * 6 = DICT_5X5_250,
+  * 7 = DICT_5X5_1000,
+  * 8 = DICT_6X6_50,
+  * 9 = DICT_6X6_100,
+  * 10 = DICT_6X6_250,
+  * 11 = DICT_6X6_1000,
+  * 12 = DICT_7X7_50,
+  * 13 = DICT_7X7_100,
+  * 14 = DICT_7X7_250,
+  * 15 = DICT_7X7_1000,
+  * 16 = DICT_ARUCO_ORIGINAL
+* `~estimate_poses` (*bool*) – estimate single markers' poses (default: true)
+* `~send_tf` (*bool*) – send TF transforms (default: true)
+* `~frame_id_prefix` (*string*) – prefix for TF transforms names, marker's ID is appended (default: `aruco_`)
+* `~length` (*double*) – markers' sides length
+* `~length_override` (*map*) – lengths of markers with specified ids
+* `~known_tilt` (*string*) – known tilt (pitch and roll) of all the markers as a frame
+
+### Topics
+
+#### Subscribed
+
+* `image_raw` (*sensor_msgs/Image*) – camera image
+* `camera_info` (*sensor_msgs/CameraInfo*) – camera calibration info
+
+#### Published
+
+* `~markers` (*aruco_pose/MarkerArray*) – list of detected markers with their corners and poses
+* `~visualization` (*visualization_msgs/MarkerArray*) – visualization markers for rviz
+* `~debug` (*sensor_msgs/Image*) – debug image with detected markers
+
+### Published transforms
+
+* `<camera_frame>` => `<frame_id_prefix><id>` – markers' poses
+
+## aruco_map nodelet
+
+`aruco_map` nodelet estimates position of markers map.
+
+### Parameters
+
+* `~map` – path to text file with markers list
+* `~frame_id` – published frame id (default: `aruco_map`)
+* `~known_tilt` – debug image width
+* `~image_width` – debug image width (default: 2000)
+* `~image_height` – debug image height (default: 2000)
+* `~image_margin` – debug image margin (default: 200)
+* `~dictionary` (*int*) – ArUco dictionary (default: 2) - should be the same as `dictionary` parameter of `aruco_detect` nodelet
+
+Map file has one marker per line with the following line format:
+
+```
+marker_id marker_length x y z yaw pitch roll
+```
+
+Where yaw, pitch and roll are extrinsic rotation around Z, Y, X axis, respectively.
+
+See examples in [`map`](map/) directory.
+
+### Topics
+
+#### Subscribed
+
+* `image_raw` (*sensor_msgs/Image*) – camera image (used for debug image)
+* `camera_info` (*sensor_msgs/CameraInfo*) – camera calibration info (used for debug image)
+* `markers` (*aruco_pose/MarkerArray*) – list of markers detected by `aruco_pose` nodelet
+
+#### Published
+
+* `~pose` (*geometry_msgs/PoseWithCovarianceStamped*) – estimated map pose
+* `~image` (*sensor_msgs/Image*) – planarized map image
+* `~visualization` (*visualization_msgs/MarkerArray*) – markers map visualization for rviz
+* `~debug` (*sensor_msgs/Image*) – debug image with detected markers and map axis
+
+### Published transforms
+
+* `<camera_frame>` => `<map_name>` – markers map pose
+
+## Running tests
+
+Command for running tests:
+
+```bash
+catkin_make run_tests && catkin_test_results
+```
+
+## Copyright
+
+Copyright © 2018 Copter Express Technologies. Author: Oleg Kalachev.
+
+Distributed under MIT License (https://opensource.org/licenses/MIT).

aruco_pose/cfg/DetectorParams.cfg

@@ -0,0 +1,107 @@
+#!/usr/bin/env python
+PACKAGE = "aruco_pose"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+import cv2.aruco
+
+p = cv2.aruco.DetectorParameters_create()
+
+gen = ParameterGenerator()
+
+gen.add("adaptiveThreshConstant",                 double_t, 0,
+        "Constant for adaptive thresholding before finding contours",
+         p.adaptiveThreshConstant, 0, 100)
+
+gen.add("adaptiveThreshWinSizeMin",               int_t,    0,
+        "Minimum window size for adaptive thresholding before finding contours",
+         p.adaptiveThreshWinSizeMin, 1, 100)
+
+gen.add("adaptiveThreshWinSizeMax",               int_t,    0,
+        "Maximum window size for adaptive thresholding before finding contours",
+        p.adaptiveThreshWinSizeMax, 1, 100)
+
+gen.add("adaptiveThreshWinSizeStep",              int_t,    0,
+        "Increments from adaptiveThreshWinSizeMin to adaptiveThreshWinSizeMax during the thresholding",
+        p.adaptiveThreshWinSizeStep, 1, 100)
+
+gen.add("cornerRefinementMaxIterations",          int_t,    0,
+        "Maximum number of iterations for stop criteria of the corner refinement process",
+        p.cornerRefinementMaxIterations, 1, 1000)
+
+corner_refine_enum = gen.enum([ gen.const("CORNER_REFINE_NONE",      int_t, 0, "No refinement"),
+                       gen.const("CORNER_REFINE_SUBPIX",     int_t, 1, "Do subpixel refinement"),
+                       gen.const("CORNER_REFINE_CONTOUR",      int_t, 2, "Use contour-Points"),
+                       gen.const("CORNER_REFINE_APRILTAG", int_t, 3, "Use the AprilTag2 approach")],
+                     "An enum to set corner refinement method")
+
+gen.add("cornerRefinementMethod", int_t, 0, "Corner refinement method", 0, 0, 3, edit_method=corner_refine_enum)
+
+gen.add("cornerRefinementMinAccuracy",            double_t, 0,
+        "Minimum error for the stop criteria of the corner refinement process",
+        p.cornerRefinementMinAccuracy, 0, 1)
+
+gen.add("cornerRefinementWinSize",                int_t,    0,
+        "Window size for the corner refinement process (in pixels)",
+         p.cornerRefinementWinSize, 1, 100)
+
+gen.add("detectInvertedMarker",                   bool_t,   0,
+        "check if there is a white marker. In order to generate a 'white' marker just invert a normal marker by using a tilde",
+        False)
+
+gen.add("errorCorrectionRate",                    double_t, 0,
+        "Error correction rate respect to the maximum error correction capability for each dictionary",
+        p.errorCorrectionRate, 0, 1)
+
+gen.add("minCornerDistanceRate",                  double_t, 0,
+        "Minimum distance between corners for detected markers relative to its perimeter",
+        p.minCornerDistanceRate, 0, 0.25)
+
+gen.add("markerBorderBits",                       int_t,    0,
+        "Number of bits of the marker border, i.e. marker border width",
+        p.markerBorderBits, 1, 10)
+
+gen.add("maxErroneousBitsInBorderRate",           double_t, 0,
+        "Maximum number of accepted erroneous bits in the border (i.e. number of allowed white bits in the border)",
+        p.maxErroneousBitsInBorderRate, 0, 1)
+
+gen.add("minDistanceToBorder",                    int_t,    0,
+        "Minimum distance of any corner to the image border for detected markers (in pixels)",
+        p.minDistanceToBorder, 0, 1000)
+
+gen.add("minMarkerDistanceRate",                  double_t, 0,
+        "minimum mean distance beetween two marker corners to be considered similar, so that the smaller one is removed. The rate is relative to the smaller perimeter of the two markers",
+        p.minMarkerDistanceRate, 0, 1)
+
+gen.add("minMarkerPerimeterRate",                 double_t, 0,
+        "Determine minimum perimeter for marker contour to be detected. This is defined as a rate respect to the maximum dimension of the input image",
+        p.minMarkerPerimeterRate, 0, 4)
+
+gen.add("maxMarkerPerimeterRate",                 double_t, 0,
+        "Determine maximum perimeter for marker contour to be detected. This is defined as a rate respect to the maximum dimension of the input image",
+        p.maxMarkerPerimeterRate, 0, 4)
+
+gen.add("minOtsuStdDev",                          double_t, 0,
+        "Minimun standard deviation in pixels values during the decodification step to apply Otsu thresholding (otherwise, all the bits are set to 0 or 1 depending on mean higher than 128 or not)",
+        p.minOtsuStdDev, 0, 100)
+
+gen.add("perspectiveRemoveIgnoredMarginPerCell",  double_t, 0,
+        "Width of the margin of pixels on each cell not considered for the determination of the cell bit. Represents the rate respect to the total size of the cell, i.e. perpectiveRemovePixelPerCell",
+        p.perspectiveRemoveIgnoredMarginPerCell, 0, 1)
+
+gen.add("perspectiveRemovePixelPerCell",          int_t,    0,
+        "Number of bits (per dimension) for each cell of the marker when removing the perspective",
+        p.perspectiveRemovePixelPerCell, 1, 100)
+
+gen.add("polygonalApproxAccuracyRate",            double_t, 0,
+        "Minimum accuracy during the polygonal approximation process to determine which contours are squares",
+        p.polygonalApproxAccuracyRate, 0, 1)
+
+gen.add("aprilTagQuadDecimate",            double_t, 0,
+        "Detection of quads can be done on a lower-resolution image, improving speed at a cost of pose accuracy and a slight decrease in detection rate. Decoding the binary payload is still done at full resolution",
+        0, 0, 1000)
+
+gen.add("aprilTagQuadSigma",            double_t, 0,
+        "What Gaussian blur should be applied to the segmented image (used for quad detection?) Parameter is the standard deviation in pixels. Very noisy images benefit from non-zero values",
+        0, 0, 1000)
+
+exit(gen.generate(PACKAGE, "aruco_pose", "Detector"))

aruco_pose/launch/sample.launch

@@ -0,0 +1,26 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
+
+    <!-- camera node -->
+    <node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet nodelet_manager">
+        <param name="frame_id" value="main_camera_optical"/>
+        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
+        <param name="image_width" value="640"/>
+        <param name="image_height" value="480"/>
+    </node>
+
+    <!-- detect aruco markers -->
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect" args="load aruco_pose/aruco_detect nodelet_manager">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <param name="length" value="0.33"/>
+    </node>
+
+    <!-- aruco map -->
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_map" args="load aruco_pose/aruco_map nodelet_manager">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="map" value="$(find aruco_pose)/map/map.txt"/>
+    </node>
+</launch>

aruco_pose/map/cmit.txt

@@ -0,0 +1,100 @@
+0	0.33	0.0	9.0	0	0	0	0	
+1	0.33	1.0	9.0	0	0	0	0	
+2	0.33	2.0	9.0	0	0	0	0	
+3	0.33	3.0	9.0	0	0	0	0	
+4	0.33	4.0	9.0	0	0	0	0	
+5	0.33	5.0	9.0	0	0	0	0	
+6	0.33	6.0	9.0	0	0	0	0	
+7	0.33	7.0	9.0	0	0	0	0	
+8	0.33	8.0	9.0	0	0	0	0	
+9	0.33	9.0	9.0	0	0	0	0	
+10	0.33	0.0	8.0	0	0	0	0	
+11	0.33	1.0	8.0	0	0	0	0	
+12	0.33	2.0	8.0	0	0	0	0	
+13	0.33	3.0	8.0	0	0	0	0	
+14	0.33	4.0	8.0	0	0	0	0	
+15	0.33	5.0	8.0	0	0	0	0	
+16	0.33	6.0	8.0	0	0	0	0	
+#17	0.33	7.0	8.0	0	0	0	0	
+18	0.33	8.0	8.0	0	0	0	0	
+19	0.33	9.0	8.0	0	0	0	0	
+20	0.33	0.0	7.0	0	0	0	0	
+21	0.33	1.0	7.0	0	0	0	0	
+22	0.33	2.0	7.0	0	0	0	0	
+23	0.33	3.0	7.0	0	0	0	0	
+24	0.33	4.0	7.0	0	0	0	0	
+25	0.33	5.0	7.0	0	0	0	0	
+26	0.33	6.0	7.0	0	0	0	0	
+27	0.33	7.0	7.0	0	0	0	0	
+28	0.33	8.0	7.0	0	0	0	0	
+29	0.33	9.0	7.0	0	0	0	0	
+30	0.33	0.0	6.0	0	0	0	0	
+31	0.33	1.0	6.0	0	0	0	0	
+32	0.33	2.0	6.0	0	0	0	0	
+33	0.33	3.0	6.0	0	0	0	0	
+34	0.33	4.0	6.0	0	0	0	0	
+35	0.33	5.0	6.0	0	0	0	0	
+36	0.33	6.0	6.0	0	0	0	0	
+37	0.33	7.0	6.0	0	0	0	0	
+38	0.33	8.0	6.0	0	0	0	0	
+39	0.33	9.0	6.0	0	0	0	0	
+40	0.33	0.0	5.0	0	0	0	0	
+41	0.33	1.0	5.0	0	0	0	0	
+42	0.33	2.0	5.0	0	0	0	0	
+43	0.33	3.0	5.0	0	0	0	0	
+44	0.33	4.0	5.0	0	0	0	0	
+45	0.33	5.0	5.0	0	0	0	0	
+46	0.33	6.0	5.0	0	0	0	0	
+47	0.33	7.0	5.0	0	0	0	0	
+48	0.33	8.0	5.0	0	0	0	0	
+49	0.33	9.0	5.0	0	0	0	0	
+50	0.33	0.0	4.0	0	0	0	0	
+51	0.33	1.0	4.0	0	0	0	0	
+52	0.33	2.0	4.0	0	0	0	0	
+53	0.33	3.0	4.0	0	0	0	0	
+54	0.33	4.0	4.0	0	0	0	0	
+55	0.33	5.0	4.0	0	0	0	0	
+56	0.33	6.0	4.0	0	0	0	0	
+57	0.33	7.0	4.0	0	0	0	0	
+58	0.33	8.0	4.0	0	0	0	0	
+59	0.33	9.0	4.0	0	0	0	0	
+60	0.33	0.0	3.0	0	0	0	0	
+61	0.33	1.0	3.0	0	0	0	0	
+62	0.33	2.0	3.0	0	0	0	0	
+63	0.33	3.0	3.0	0	0	0	0	
+64	0.33	4.0	3.0	0	0	0	0	
+65	0.33	5.0	3.0	0	0	0	0	
+66	0.33	6.0	3.0	0	0	0	0	
+67	0.33	7.0	3.0	0	0	0	0	
+68	0.33	8.0	3.0	0	0	0	0	
+69	0.33	9.0	3.0	0	0	0	0	
+70	0.33	0.0	2.0	0	0	0	0	
+71	0.33	1.0	2.0	0	0	0	0	
+72	0.33	2.0	2.0	0	0	0	0	
+73	0.33	3.0	2.0	0	0	0	0	
+74	0.33	4.0	2.0	0	0	0	0	
+75	0.33	5.0	2.0	0	0	0	0	
+76	0.33	6.0	2.0	0	0	0	0	
+77	0.33	7.0	2.0	0	0	0	0	
+78	0.33	8.0	2.0	0	0	0	0	
+79	0.33	9.0	2.0	0	0	0	0	
+80	0.33	0.0	1.0	0	0	0	0	
+81	0.33	1.0	1.0	0	0	0	0	
+82	0.33	2.0	1.0	0	0	0	0	
+83	0.33	3.0	1.0	0	0	0	0	
+84	0.33	4.0	1.0	0	0	0	0	
+85	0.33	5.0	1.0	0	0	0	0	
+86	0.33	6.0	1.0	0	0	0	0	
+87	0.33	7.0	1.0	0	0	0	0	
+88	0.33	8.0	1.0	0	0	0	0	
+89	0.33	9.0	1.0	0	0	0	0	
+90	0.33	0.0	0.0	0	0	0	0	
+91	0.33	1.0	0.0	0	0	0	0	
+92	0.33	2.0	0.0	0	0	0	0	
+93	0.33	3.0	0.0	0	0	0	0	
+94	0.33	4.0	0.0	0	0	0	0	
+95	0.33	5.0	0.0	0	0	0	0	
+96	0.33	6.0	0.0	0	0	0	0	
+97	0.33	7.0	0.0	0	0	0	0	
+98	0.33	8.0	0.0	0	0	0	0	
+99	0.33	9.0	0.0	0	0	0	0	

aruco_pose/map/map.txt

@@ -0,0 +1,5 @@
+# id	length	x	y	z	rot_z	rot_y	rot_x
+1	0.33	0	0	0	0	0	0
+2	0.33	1	0	0	0	0	0
+3	0.33	0	1	0	0	0	0
+4	0.33	1	1	0	0	0	0

aruco_pose/map/office.txt

@@ -0,0 +1,8 @@
+107	0.33	0	0	0	0	0	0
+106	0.33	0.77	0	0	0	0	0
+105	0.33	0	0.77	0	0	0	0
+104	0.33	0.77	0.77	0	0	0	0
+103	0.33	0	1.54	0	0	0	0
+102	0.33	0.77	1.54	0	0	0	0
+101	0.33	0	2.31	0	0	0	0
+100	0.33	0.77	2.31	0	0	0	0

aruco_pose/map/office_ceiling.txt

@@ -0,0 +1,31 @@
+14	0.365	0.000	0.0	0	0	0	0
+15	0.365	1.335	0.0	0	0	0	0
+30	0.365	2.865	0.0	0	0	0	0
+31	0.365	4.200	0.0	0	0	0	0
+12	0.365	0.000	1.8	0	0	0	0
+13	0.365	1.335	1.8	0	0	0	0
+28	0.365	2.865	1.8	0	0	0	0
+29	0.365	4.200	1.8	0	0	0	0
+10	0.365	0.000	3.6	0	0	0	0
+11	0.365	1.335	3.6	0	0	0	0
+26	0.365	2.865	3.6	0	0	0	0
+27	0.365	4.200	3.6	0	0	0	0
+8	0.365	0.000	5.4	0	0	0	0
+9	0.365	1.335	5.4	0	0	0	0
+24	0.365	2.865	5.4	0	0	0	0
+25	0.365	4.200	5.4	0	0	0	0
+6	0.365	0.000	7.2	0	0	0	0
+7	0.365	1.335	7.2	0	0	0	0
+22	0.365	2.865	7.2	0	0	0	0
+23	0.365	4.200	7.2	0	0	0	0
+4	0.365	0.000	9.0	0	0	0	0
+5	0.365	1.335	9.0	0	0	0	0
+20	0.365	2.865	9.0	0	0	0	0
+21	0.365	4.200	9.0	0	0	0	0
+2	0.365	0.000	10.8	0	0	0	0
+3	0.365	1.335	10.8	0	0	0	0
+18	0.365	2.865	10.8	0	0	0	0
+19	0.365	4.200	10.8	0	0	0	0
+1	0.365	0.000	12.6	0	0	0	0
+0	0.365	1.335	12.6	0	0	0	0
+16	0.365	2.865	12.6	0	0	0	0

aruco_pose/msg/Marker.msg

@@ -0,0 +1,7 @@
+uint32 id
+float32 length
+geometry_msgs/Pose pose
+Point2D c1
+Point2D c2
+Point2D c3
+Point2D c4

aruco_pose/msg/MarkerArray.msg

@@ -0,0 +1,2 @@
+Header header
+Marker[] markers

aruco_pose/msg/Point2D.msg

@@ -0,0 +1,2 @@
+float32 x
+float32 y

aruco_pose/nodelet_plugins.xml

@@ -1,5 +1,8 @@
 <library path="lib/libaruco_pose">
-   <class name="aruco_pose/aruco_pose" type="ArucoPose" base_class_type="nodelet::Nodelet">
+   <class name="aruco_pose/aruco_detect" type="ArucoDetect" base_class_type="nodelet::Nodelet">
+      <description/>
+   </class>
+   <class name="aruco_pose/aruco_map" type="ArucoMap" base_class_type="nodelet::Nodelet">
       <description/>
    </class>
 </library>

aruco_pose/package.xml

@@ -1,31 +1,36 @@
 <?xml version="1.0"?>
-<package>
+<package format="2">
   <name>aruco_pose</name>
-  <version>0.0.0</version>
-  <description>ArUco maps precise pose estimation nodelet</description>
+  <version>0.0.1</version>
+  <description>Positioning with ArUco markers</description>
 
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
   <license>MIT</license>
 
   <!--url type="website">http://wiki.ros.org/aruco_pose</url-->
   <author email="okalachev@gmail.com">Oleg Kalachev</author>
-  <author email="urpylka@gmail.com">Artem Smirnov</author>
 
-  <!-- Use build_depend for packages you need at compile time: -->
-  <build_depend>nodelet</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>image_transport</build_depend>
-  <build_depend>cv_bridge</build_depend>
-  <build_depend>tf</build_depend>
-  <!-- Use buildtool_depend for build tool packages: -->
   <buildtool_depend>catkin</buildtool_depend>
-  <!-- Use run_depend for packages you need at runtime: -->
-  <run_depend>nodelet</run_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>image_transport</run_depend>
-  <run_depend>cv_bridge</run_depend>
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
+
+  <depend>roscpp</depend>
+  <depend>nodelet</depend>
+  <depend>tf</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  <depend>message_generation</depend>
+  <depend>message_runtime</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>rostest</depend>
+  <depend>dynamic_reconfigure</depend>
+
+  <test_depend>image_publisher</test_depend>
+  <test_depend>ros_pytest</test_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

aruco_pose/src/aruco_detect.cpp

@@ -0,0 +1,389 @@
+/*
+ * Detecting and pose estimation of ArUco markers
+ * Copyright (C) 2018 Copter Express Technologies
+ *
+ * Author: Oleg Kalachev <okalachev@gmail.com>
+ *
+ * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ */
+
+/*
+ * Code is based on https://github.com/UbiquityRobotics/fiducials, which is distributed
+ * under the BSD license.
+ */
+
+#include <math.h>
+#include <vector>
+#include <string>
+#include <map>
+#include <unordered_map>
+#include <unordered_set>
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+#include <tf/transform_datatypes.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <dynamic_reconfigure/server.h>
+#include <geometry_msgs/Vector3.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/aruco.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
+
+#include <aruco_pose/Marker.h>
+#include <aruco_pose/MarkerArray.h>
+#include <aruco_pose/DetectorConfig.h>
+
+#include "utils.h"
+#include <memory>
+#include <functional>
+
+using std::vector;
+using cv::Mat;
+
+class ArucoDetect : public nodelet::Nodelet {
+private:
+	std::unique_ptr<tf2_ros::TransformBroadcaster> br_;
+	std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+	std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
+	std::shared_ptr<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>> dyn_srv_;
+	cv::Ptr<cv::aruco::Dictionary> dictionary_;
+	cv::Ptr<cv::aruco::DetectorParameters> parameters_;
+	image_transport::Publisher debug_pub_;
+	image_transport::CameraSubscriber img_sub_;
+	ros::Publisher markers_pub_, vis_markers_pub_;
+	ros::Subscriber map_markers_sub_;
+	bool estimate_poses_, send_tf_, auto_flip_;
+	double length_;
+	std::unordered_map<int, double> length_override_;
+	std::string frame_id_prefix_, known_tilt_;
+	Mat camera_matrix_, dist_coeffs_;
+	aruco_pose::MarkerArray array_;
+	std::unordered_set<int> map_markers_ids_;
+	visualization_msgs::MarkerArray vis_array_;
+
+public:
+	virtual void onInit()
+	{
+		ros::NodeHandle& nh_ = getNodeHandle();
+		ros::NodeHandle& nh_priv_ = getPrivateNodeHandle();
+
+		br_.reset(new tf2_ros::TransformBroadcaster());
+		tf_buffer_.reset(new tf2_ros::Buffer());
+		tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_, nh_));
+
+		int dictionary;
+		dictionary = nh_priv_.param("dictionary", 2);
+		estimate_poses_ = nh_priv_.param("estimate_poses", true);
+		send_tf_ = nh_priv_.param("send_tf", true);
+		if (estimate_poses_ && !nh_priv_.getParam("length", length_)) {
+			NODELET_FATAL("can't estimate marker's poses as ~length parameter is not defined");
+			ros::shutdown();
+		}
+		readLengthOverride(nh_priv_);
+
+		known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
+		auto_flip_ = nh_priv_.param("auto_flip", false);
+
+		frame_id_prefix_ = nh_priv_.param<std::string>("frame_id_prefix", "aruco_");
+
+		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
+
+		dictionary_ = cv::aruco::getPredefinedDictionary(static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(dictionary));
+		parameters_ = cv::aruco::DetectorParameters::create();
+
+		image_transport::ImageTransport it(nh_);
+		image_transport::ImageTransport it_priv(nh_priv_);
+
+		map_markers_sub_ = nh_.subscribe("map_markers", 1, &ArucoDetect::mapMarkersCallback, this);
+		debug_pub_ = it_priv.advertise("debug", 1);
+		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1);
+		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1);
+		img_sub_ = it.subscribeCamera("image_raw", 1, &ArucoDetect::imageCallback, this);
+
+		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>>(nh_priv_);
+		dynamic_reconfigure::Server<aruco_pose::DetectorConfig>::CallbackType cb;
+
+		cb = std::bind(&ArucoDetect::paramCallback, this, std::placeholders::_1, std::placeholders::_2);
+		dyn_srv_->setCallback(cb);
+
+		NODELET_INFO("ready");
+	}
+
+private:
+	void imageCallback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo)
+	{
+		Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
+
+		vector<int> ids;
+		vector<vector<cv::Point2f>> corners, rejected;
+		vector<cv::Vec3d> rvecs, tvecs;
+		vector<cv::Point3f> obj_points;
+		geometry_msgs::TransformStamped snap_to;
+
+		// Detect markers
+		cv::aruco::detectMarkers(image, dictionary_, corners, ids, parameters_, rejected);
+
+		array_.header.stamp = msg->header.stamp;
+		array_.header.frame_id = msg->header.frame_id;
+		array_.markers.clear();
+
+		if (ids.size() != 0) {
+			parseCameraInfo(cinfo, camera_matrix_, dist_coeffs_);
+
+			// Estimate individual markers' poses
+			if (estimate_poses_) {
+				cv::aruco::estimatePoseSingleMarkers(corners, length_, camera_matrix_, dist_coeffs_,
+				                                     rvecs, tvecs);
+
+				// process length override, TODO: efficiency
+				if (!length_override_.empty()) {
+					for (unsigned int i = 0; i < ids.size(); i++) {
+						int id = ids[i];
+						auto item = length_override_.find(id);
+						if (item != length_override_.end()) { // found override
+							vector<cv::Vec3d> rvecs_current, tvecs_current;
+							vector<vector<cv::Point2f>> corners_current;
+							corners_current.push_back(corners[i]);
+							cv::aruco::estimatePoseSingleMarkers(corners_current, item->second,
+							                                     camera_matrix_, dist_coeffs_,
+										                         rvecs_current, tvecs_current);
+							rvecs[i] = rvecs_current[0];
+							tvecs[i] = tvecs_current[0];
+						}
+					}
+				}
+
+				if (!known_tilt_.empty()) {
+					try {
+						snap_to = tf_buffer_->lookupTransform(msg->header.frame_id, known_tilt_,
+						                                      msg->header.stamp, ros::Duration(0.02));
+					} catch (const tf2::TransformException& e) {
+						NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what());
+					}
+				}
+			}
+
+			array_.markers.reserve(ids.size());
+			aruco_pose::Marker marker;
+			geometry_msgs::TransformStamped transform;
+			transform.header.stamp = msg->header.stamp;
+			transform.header.frame_id = msg->header.frame_id;
+
+			for (unsigned int i = 0; i < ids.size(); i++) {
+				marker.id = ids[i];
+				marker.length = getMarkerLength(marker.id);
+				fillCorners(marker, corners[i]);
+
+				if (estimate_poses_) {
+					fillPose(marker.pose, rvecs[i], tvecs[i]);
+
+					// snap orientation (if enabled and snap frame available)
+					if (!known_tilt_.empty() && !snap_to.header.frame_id.empty()) {
+						snapOrientation(marker.pose.orientation, snap_to.transform.rotation, auto_flip_);
+					}
+
+					// TODO: check IDs are unique
+					if (send_tf_) {
+						transform.child_frame_id = getChildFrameId(ids[i]);
+
+						// check if such static transform is in the map
+						if (map_markers_ids_.find(ids[i]) == map_markers_ids_.end()) {
+							transform.transform.rotation = marker.pose.orientation;
+							fillTranslation(transform.transform.translation, tvecs[i]);
+							br_->sendTransform(transform);
+						}
+					}
+				}
+				array_.markers.push_back(marker);
+			}
+		}
+
+		markers_pub_.publish(array_);
+
+		// Publish visualization markers
+		if (estimate_poses_ && vis_markers_pub_.getNumSubscribers() != 0) {
+			// Delete all markers
+			visualization_msgs::Marker vis_marker;
+			vis_marker.action = visualization_msgs::Marker::DELETEALL;
+			vis_array_.markers.clear();
+			vis_array_.markers.reserve(ids.size() + 1);
+			vis_array_.markers.push_back(vis_marker);
+
+			for (unsigned int i = 0; i < ids.size(); i++)
+				pushVisMarkers(msg->header.frame_id, msg->header.stamp, array_.markers[i].pose,
+				               getMarkerLength(ids[i]), ids[i], i);
+
+			vis_markers_pub_.publish(vis_array_);
+		}
+
+		// Publish debug image
+		if (debug_pub_.getNumSubscribers() != 0) {
+			Mat debug = image.clone();
+			cv::aruco::drawDetectedMarkers(debug, corners, ids); // draw markers
+			if (estimate_poses_)
+				for (unsigned int i = 0; i < ids.size(); i++)
+					cv::aruco::drawAxis(debug, camera_matrix_, dist_coeffs_,
+					                    rvecs[i], tvecs[i], getMarkerLength(ids[i]));
+
+			cv_bridge::CvImage out_msg;
+			out_msg.header.frame_id = msg->header.frame_id;
+			out_msg.header.stamp = msg->header.stamp;
+			out_msg.encoding = sensor_msgs::image_encodings::BGR8;
+			out_msg.image = debug;
+			debug_pub_.publish(out_msg.toImageMsg());
+		}
+	}
+
+	inline void fillCorners(aruco_pose::Marker& marker, const vector<cv::Point2f>& corners) const
+	{
+		marker.c1.x = corners[0].x;
+		marker.c2.x = corners[1].x;
+		marker.c3.x = corners[2].x;
+		marker.c4.x = corners[3].x;
+		marker.c1.y = corners[0].y;
+		marker.c2.y = corners[1].y;
+		marker.c3.y = corners[2].y;
+		marker.c4.y = corners[3].y;
+	}
+
+	inline void fillPose(geometry_msgs::Pose& pose, const cv::Vec3d& rvec, const cv::Vec3d& tvec) const
+	{
+		pose.position.x = tvec[0];
+		pose.position.y = tvec[1];
+		pose.position.z = tvec[2];
+
+		double angle = norm(rvec);
+		cv::Vec3d axis = rvec / angle;
+
+		tf2::Quaternion q;
+		q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle);
+
+		pose.orientation.w = q.w();
+		pose.orientation.x = q.x();
+		pose.orientation.y = q.y();
+		pose.orientation.z = q.z();
+	}
+
+	inline void fillTranslation(geometry_msgs::Vector3& translation, const cv::Vec3d& tvec) const
+	{
+		translation.x = tvec[0];
+		translation.y = tvec[1];
+		translation.z = tvec[2];
+	}
+
+	void pushVisMarkers(const std::string& frame_id, const ros::Time& stamp,
+	                    const geometry_msgs::Pose &pose, double length, int id, int index)
+	{
+		visualization_msgs::Marker marker;
+		marker.header.frame_id = frame_id;
+		marker.header.stamp = stamp;
+		marker.action = visualization_msgs::Marker::ADD;
+		marker.id = index;
+
+		// Marker
+		marker.ns = "aruco_marker";
+		marker.type = visualization_msgs::Marker::CUBE;
+		marker.scale.x = length;
+		marker.scale.y = length;
+		marker.scale.z = 0.001;
+		marker.color.r = 1;
+		marker.color.g = 1;
+		marker.color.b = 1;
+		marker.color.a = 0.9;
+		marker.pose = pose;
+		vis_array_.markers.push_back(marker);
+
+		// Label
+		marker.ns = "aruco_marker_label";
+		marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+		marker.scale.z = length * 0.6;
+		marker.color.r = 0;
+		marker.color.g = 0;
+		marker.color.b = 0;
+		marker.color.a = 1;
+		marker.text = std::to_string(id);
+		marker.pose = pose;
+		vis_array_.markers.push_back(marker);
+	}
+
+	inline std::string getChildFrameId(int id) const
+	{
+		return frame_id_prefix_ + std::to_string(id);
+	}
+
+	void readLengthOverride(ros::NodeHandle& nh)
+	{
+		std::map<std::string, double> length_override;
+		nh.getParam("length_override", length_override);
+		for (auto const& item : length_override) {
+			length_override_[std::stoi(item.first)] = item.second;
+		}
+	}
+
+	inline double getMarkerLength(int id)
+	{
+		auto item = length_override_.find(id);
+		if (item != length_override_.end()) {
+			return item->second;
+		} else {
+			return length_;
+		}
+	}
+
+	void mapMarkersCallback(const aruco_pose::MarkerArray& msg)
+	{
+		map_markers_ids_.clear();
+		for (auto const& marker : msg.markers) {
+			map_markers_ids_.insert(marker.id);
+		}
+	}
+
+	void paramCallback(aruco_pose::DetectorConfig &config, uint32_t level)
+	{
+		parameters_->adaptiveThreshConstant = config.adaptiveThreshConstant;
+		parameters_->adaptiveThreshWinSizeMin = config.adaptiveThreshWinSizeMin;
+		parameters_->adaptiveThreshWinSizeMax = config.adaptiveThreshWinSizeMax;
+		parameters_->adaptiveThreshWinSizeStep = config.adaptiveThreshWinSizeStep;
+		parameters_->cornerRefinementMaxIterations = config.cornerRefinementMaxIterations;
+		parameters_->cornerRefinementMethod = config.cornerRefinementMethod;
+		parameters_->cornerRefinementMinAccuracy = config.cornerRefinementMinAccuracy;
+		parameters_->cornerRefinementWinSize = config.cornerRefinementWinSize;
+#if ((CV_VERSION_MAJOR == 3) && (CV_VERSION_MINOR >= 4) && (CV_VERSION_REVISION >= 7)) || (CV_VERSION_MAJOR > 3)
+		parameters_->detectInvertedMarker = config.detectInvertedMarker;
+#endif
+		parameters_->errorCorrectionRate = config.errorCorrectionRate;
+		parameters_->minCornerDistanceRate = config.minCornerDistanceRate;
+		parameters_->markerBorderBits = config.markerBorderBits;
+		parameters_->maxErroneousBitsInBorderRate = config.maxErroneousBitsInBorderRate;
+		parameters_->minDistanceToBorder = config.minDistanceToBorder;
+		parameters_->minMarkerDistanceRate = config.minMarkerDistanceRate;
+		parameters_->minMarkerPerimeterRate = config.minMarkerPerimeterRate;
+		parameters_->maxMarkerPerimeterRate = config.maxMarkerPerimeterRate;
+		parameters_->minOtsuStdDev = config.minOtsuStdDev;
+		parameters_->perspectiveRemoveIgnoredMarginPerCell = config.perspectiveRemoveIgnoredMarginPerCell;
+		parameters_->perspectiveRemovePixelPerCell = config.perspectiveRemovePixelPerCell;
+		parameters_->polygonalApproxAccuracyRate = config.polygonalApproxAccuracyRate;
+#if ((CV_VERSION_MAJOR == 3) && (CV_VERSION_MINOR >= 4) && (CV_VERSION_REVISION >= 2)) || (CV_VERSION_MAJOR > 3)
+		parameters_->aprilTagQuadDecimate = config.aprilTagQuadDecimate;
+		parameters_->aprilTagQuadSigma = config.aprilTagQuadSigma;
+#endif
+	}
+};
+
+PLUGINLIB_EXPORT_CLASS(ArucoDetect, nodelet::Nodelet)

aruco_pose/src/aruco_map.cpp

@@ -0,0 +1,514 @@
+/*
+ * Detecting and pose estimation of ArUco markers maps
+ * Copyright (C) 2018 Copter Express Technologies
+ *
+ * Author: Oleg Kalachev <okalachev@gmail.com>
+ *
+ * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ */
+
+/*
+ * Code is based on https://github.com/UbiquityRobotics/fiducials, which is distributed
+ * under the BSD license.
+ */
+
+#include <math.h>
+#include <string>
+#include <vector>
+#include <fstream>
+#include <algorithm>
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+#include <image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <tf/transform_datatypes.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <sensor_msgs/Image.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+#include <aruco_pose/MarkerArray.h>
+#include <aruco_pose/Marker.h>
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/aruco.hpp>
+
+#include "draw.h"
+#include "utils.h"
+
+using std::vector;
+using cv::Mat;
+using sensor_msgs::Image;
+using sensor_msgs::CameraInfo;
+using aruco_pose::MarkerArray;
+
+typedef message_filters::sync_policies::ExactTime<Image, CameraInfo, MarkerArray> SyncPolicy;
+
+class ArucoMap : public nodelet::Nodelet {
+private:
+	ros::Publisher img_pub_, pose_pub_, markers_pub_, vis_markers_pub_;
+	image_transport::Publisher debug_pub_;
+	message_filters::Subscriber<Image> image_sub_;
+	message_filters::Subscriber<CameraInfo> info_sub_;
+	message_filters::Subscriber<MarkerArray> markers_sub_;
+	boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> > sync_;
+	cv::Ptr<cv::aruco::Board> board_;
+	Mat camera_matrix_, dist_coeffs_;
+	geometry_msgs::TransformStamped transform_;
+	geometry_msgs::PoseWithCovarianceStamped pose_;
+	vector<geometry_msgs::TransformStamped> markers_transforms_;
+	aruco_pose::MarkerArray markers_;
+	tf2_ros::TransformBroadcaster br_;
+	tf2_ros::StaticTransformBroadcaster static_br_;
+	tf2_ros::Buffer tf_buffer_;
+	tf2_ros::TransformListener tf_listener_{tf_buffer_};
+	visualization_msgs::MarkerArray vis_array_;
+	std::string known_tilt_, map_, markers_frame_, markers_parent_frame_;
+	int image_width_, image_height_, image_margin_;
+	bool auto_flip_, image_axis_;
+
+public:
+	virtual void onInit()
+	{
+		ros::NodeHandle &nh_ = getNodeHandle();
+		ros::NodeHandle &nh_priv_ = getPrivateNodeHandle();
+
+		image_transport::ImageTransport it_priv(nh_priv_);
+
+		// TODO: why image_transport doesn't work here?
+		img_pub_ = nh_priv_.advertise<sensor_msgs::Image>("image", 1, true);
+		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1, true);
+
+		board_ = cv::makePtr<cv::aruco::Board>();
+		board_->dictionary = cv::aruco::getPredefinedDictionary(
+			                 static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(nh_priv_.param("dictionary", 2)));
+		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
+
+		std::string type, map;
+		type = nh_priv_.param<std::string>("type", "map");
+		transform_.child_frame_id = nh_priv_.param<std::string>("frame_id", "aruco_map");
+		known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
+		auto_flip_ = nh_priv_.param("auto_flip", false);
+		image_width_ = nh_priv_.param("image_width" , 2000);
+		image_height_ = nh_priv_.param("image_height", 2000);
+		image_margin_ = nh_priv_.param("image_margin", 200);
+		image_axis_ = nh_priv_.param("image_axis", true);
+		markers_parent_frame_ = nh_priv_.param<std::string>("markers/frame_id", transform_.child_frame_id);
+		markers_frame_ = nh_priv_.param<std::string>("markers/child_frame_id_prefix", "");
+
+		// createStripLine();
+
+		if (type == "map") {
+			param(nh_priv_, "map", map);
+			loadMap(map);
+		} else if (type == "gridboard") {
+			createGridBoard(nh_priv_);
+		} else {
+			NODELET_FATAL("unknown type: %s", type.c_str());
+			ros::shutdown();
+		}
+
+		pose_pub_ = nh_priv_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose", 1);
+		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1, true);
+		debug_pub_ = it_priv.advertise("debug", 1);
+
+		image_sub_.subscribe(nh_, "image_raw", 1);
+		info_sub_.subscribe(nh_, "camera_info", 1);
+		markers_sub_.subscribe(nh_, "markers", 1);
+
+		sync_.reset(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(10), image_sub_, info_sub_, markers_sub_));
+		sync_->registerCallback(boost::bind(&ArucoMap::callback, this, _1, _2, _3));
+
+		publishMarkersFrames();
+		publishMarkers();
+		publishMapImage();
+		vis_markers_pub_.publish(vis_array_);
+
+		NODELET_INFO("ready");
+	}
+
+	void callback(const sensor_msgs::ImageConstPtr& image,
+	              const sensor_msgs::CameraInfoConstPtr& cinfo,
+	              const aruco_pose::MarkerArrayConstPtr& markers)
+	{
+		int valid = 0;
+		int count = markers->markers.size();
+		std::vector<int> ids;
+		std::vector<std::vector<cv::Point2f>> corners;
+		cv::Vec3d rvec, tvec;
+
+		parseCameraInfo(cinfo, camera_matrix_, dist_coeffs_);
+		if (markers->markers.empty()) goto publish_debug;
+
+		ids.reserve(count);
+		corners.reserve(count);
+
+		for(auto const &marker : markers->markers) {
+			ids.push_back(marker.id);
+			std::vector<cv::Point2f> marker_corners = {
+				cv::Point2f(marker.c1.x, marker.c1.y),
+				cv::Point2f(marker.c2.x, marker.c2.y),
+				cv::Point2f(marker.c3.x, marker.c3.y),
+				cv::Point2f(marker.c4.x, marker.c4.y)
+			};
+			corners.push_back(marker_corners);
+		}
+
+		if (known_tilt_.empty()) {
+			// simple estimation
+			valid = cv::aruco::estimatePoseBoard(corners, ids, board_, camera_matrix_, dist_coeffs_,
+			                                     rvec, tvec, false);
+			if (!valid) goto publish_debug;
+
+			transform_.header.stamp = markers->header.stamp;
+			transform_.header.frame_id = markers->header.frame_id;
+			pose_.header = transform_.header;
+			fillPose(pose_.pose.pose, rvec, tvec);
+			fillTransform(transform_.transform, rvec, tvec);
+
+		} else {
+			Mat obj_points, img_points;
+			// estimation with "snapping"
+			cv::aruco::getBoardObjectAndImagePoints(board_, corners, ids, obj_points, img_points);
+			if (obj_points.empty()) goto publish_debug;
+
+			double center_x = 0, center_y = 0, center_z = 0;
+			alignObjPointsToCenter(obj_points, center_x, center_y, center_z);
+
+			valid = solvePnP(obj_points, img_points, camera_matrix_, dist_coeffs_, rvec, tvec, false);
+			if (!valid) goto publish_debug;
+
+			fillTransform(transform_.transform, rvec, tvec);
+			try {
+				geometry_msgs::TransformStamped snap_to = tf_buffer_.lookupTransform(markers->header.frame_id,
+				                                          known_tilt_, markers->header.stamp, ros::Duration(0.02));
+				snapOrientation(transform_.transform.rotation, snap_to.transform.rotation, auto_flip_);
+			} catch (const tf2::TransformException& e) {
+				NODELET_WARN_THROTTLE(1, "can't snap: %s", e.what());
+			}
+
+			geometry_msgs::TransformStamped shift;
+			shift.transform.translation.x = -center_x;
+			shift.transform.translation.y = -center_y;
+			shift.transform.translation.z = -center_z;
+			shift.transform.rotation.w = 1;
+			tf2::doTransform(shift, transform_, transform_);
+
+			// for debug topic
+			tvec[0] = transform_.transform.translation.x;
+			tvec[1] = transform_.transform.translation.y;
+			tvec[2] = transform_.transform.translation.z;
+
+			transform_.header.stamp = markers->header.stamp;
+			transform_.header.frame_id = markers->header.frame_id;
+			pose_.header = transform_.header;
+			transformToPose(transform_.transform, pose_.pose.pose);
+		}
+
+		if (!transform_.child_frame_id.empty()) {
+			br_.sendTransform(transform_);
+		}
+		pose_pub_.publish(pose_);
+
+publish_debug:
+		// publish debug image (even if no map detected)
+		if (debug_pub_.getNumSubscribers() > 0) {
+			Mat mat = cv_bridge::toCvCopy(image, "bgr8")->image; // copy image as we're planning to modify it
+			cv::aruco::drawDetectedMarkers(mat, corners, ids); // draw detected markers
+			if (valid) {
+				_drawAxis(mat, camera_matrix_, dist_coeffs_, rvec, tvec, 1.0); // draw board axis
+			}
+			cv_bridge::CvImage out_msg;
+			out_msg.header.frame_id = image->header.frame_id;
+			out_msg.header.stamp = image->header.stamp;
+			out_msg.encoding = sensor_msgs::image_encodings::BGR8;
+			out_msg.image = mat;
+			debug_pub_.publish(out_msg.toImageMsg());
+		}
+	}
+
+	void alignObjPointsToCenter(Mat &obj_points, double &center_x, double &center_y, double &center_z) const
+	{
+		// Align object points to the center of mass
+		double sum_x = 0;
+		double sum_y = 0;
+		double sum_z = 0;
+
+		for (int i = 0; i < obj_points.rows; i++) {
+			sum_x += obj_points.at<float>(i, 0);
+			sum_y += obj_points.at<float>(i, 1);
+			sum_z += obj_points.at<float>(i, 2);
+		}
+
+		center_x = sum_x / obj_points.rows;
+		center_y = sum_y / obj_points.rows;
+		center_z = sum_z / obj_points.rows;
+
+		for (int i = 0; i < obj_points.rows; i++) {
+			obj_points.at<float>(i, 0) -= center_x;
+			obj_points.at<float>(i, 1) -= center_y;
+			obj_points.at<float>(i, 2) -= center_z;
+		}
+	}
+
+	void loadMap(std::string filename)
+	{
+		std::ifstream f(filename);
+		std::string line;
+
+		if (!f.good()) {
+			NODELET_FATAL("%s - %s", strerror(errno), filename.c_str());
+			ros::shutdown();
+		}
+
+		while (std::getline(f, line)) {
+			int id;
+			double length, x, y, z, yaw, pitch, roll;
+
+			std::istringstream s(line);
+
+			// Read first character to see whether it's a comment
+			char first = 0;
+			if (!(s >> first)) {
+				// No non-whitespace characters, must be a blank line
+				continue;
+			}
+
+			if (first == '#') {
+				NODELET_DEBUG("Skipping line as a comment: %s", line.c_str());
+				continue;
+			} else if (isdigit(first)) {
+				// Put the digit back into the stream
+				// Note that this is a non-modifying putback, so this should work with istreams
+				// (see https://en.cppreference.com/w/cpp/io/basic_istream/putback)
+				s.putback(first);
+			} else {
+				// Probably garbage data; inform user and throw an exception, possibly killing nodelet
+				NODELET_FATAL("Malformed input: %s", line.c_str());
+				ros::shutdown();
+				throw std::runtime_error("Malformed input");
+			}
+
+			if (!(s >> id >> length >> x >> y)) {
+				NODELET_ERROR("Not enough data in line: %s; "
+				          "Each marker must have at least id, length, x, y fields", line.c_str());
+				continue;
+			}
+			// Be less strict about z, yaw, pitch roll
+			if (!(s >> z)) {
+				NODELET_DEBUG("No z coordinate provided for marker %d, assuming 0", id);
+				z = 0;
+			}
+			if (!(s >> yaw)) {
+				NODELET_DEBUG("No yaw provided for marker %d, assuming 0", id);
+				yaw = 0;
+			}
+			if (!(s >> pitch)) {
+				NODELET_DEBUG("No pitch provided for marker %d, assuming 0", id);
+				pitch = 0;
+			}
+			if (!(s >> roll)) {
+				NODELET_DEBUG("No roll provided for marker %d, assuming 0", id);
+				roll = 0;
+			}
+			addMarker(id, length, x, y, z, yaw, pitch, roll);
+		}
+
+		NODELET_INFO("loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
+	}
+
+	void createGridBoard(ros::NodeHandle& nh)
+	{
+		NODELET_INFO("generate gridboard");
+		NODELET_WARN("gridboard maps are deprecated");
+
+		int markers_x, markers_y, first_marker;
+		double markers_side, markers_sep_x, markers_sep_y;
+		std::vector<int> marker_ids;
+		markers_x = nh.param("markers_x", 10);
+		markers_y = nh.param("markers_y", 10);
+		first_marker = nh.param("first_marker", 0);
+
+		param(nh, "markers_side", markers_side);
+		param(nh, "markers_sep_x", markers_sep_x);
+		param(nh, "markers_sep_y", markers_sep_y);
+
+		if (nh.getParam("marker_ids", marker_ids)) {
+			if ((unsigned int)(markers_x * markers_y) != marker_ids.size()) {
+				NODELET_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y");
+				ros::shutdown();
+			}
+		} else {
+			// Fill marker_ids automatically
+			marker_ids.resize(markers_x * markers_y);
+			for (int i = 0; i < markers_x * markers_y; i++)
+			{
+				marker_ids.at(i) = first_marker++;
+			}
+		}
+
+		double max_y = markers_y * markers_side + (markers_y - 1) * markers_sep_y;
+		for(int y = 0; y < markers_y; y++) {
+			for(int x = 0; x < markers_x; x++) {
+				double x_pos = x * (markers_side + markers_sep_x);
+				double y_pos = max_y - y * (markers_side + markers_sep_y) - markers_side;
+				NODELET_INFO("add marker %d %g %g", marker_ids[y * markers_y + x], x_pos, y_pos);
+				addMarker(marker_ids[y * markers_y + x], markers_side, x_pos, y_pos, 0, 0, 0, 0);
+			}
+		}
+	}
+
+	// void createStripLine()
+	// {
+	// 	visualization_msgs::Marker marker;
+	// 	marker.header.frame_id = transform_.child_frame_id;
+	// 	marker.action = visualization_msgs::Marker::ADD;
+	// 	marker.ns = "aruco_map_link";
+	// 	marker.type = visualization_msgs::Marker::LINE_STRIP;
+	// 	marker.scale.x = 0.02;
+	// 	marker.color.g = 1;
+	// 	marker.color.a = 0.8;
+	// 	marker.frame_locked = true;
+	// 	marker.pose.orientation.w = 1;
+	// 	vis_array_.markers.push_back(marker);
+	// }
+
+	void addMarker(int id, double length, double x, double y, double z,
+				   double yaw, double pitch, double roll)
+	{
+		// Check whether the id is in range for current dictionary
+		int num_markers = board_->dictionary->bytesList.rows;
+		if (num_markers <= id) {
+			NODELET_ERROR("Marker id %d is not in dictionary; current dictionary contains %d markers. "
+			              "Please see https://github.com/CopterExpress/clover/blob/master/aruco_pose/README.md#parameters for details",
+					  id, num_markers);
+			return;
+		}
+		// Check if marker is already in the board
+		if (std::count(board_->ids.begin(), board_->ids.end(), id) > 0) {
+			NODELET_ERROR("Marker id %d is already in the map", id);
+			return;
+		}
+		// Create transform
+		tf::Quaternion q;
+		q.setRPY(roll, pitch, yaw);
+		tf::Transform transform(q, tf::Vector3(x, y, z));
+
+		/* marker's corners:
+			0    1
+			3    2
+		*/
+		double halflen = length / 2;
+		tf::Point p0(-halflen, halflen, 0);
+		tf::Point p1(halflen, halflen, 0);
+		tf::Point p2(halflen, -halflen, 0);
+		tf::Point p3(-halflen, -halflen, 0);
+		p0 = transform * p0;
+		p1 = transform * p1;
+		p2 = transform * p2;
+		p3 = transform * p3;
+
+		vector<cv::Point3f> obj_points = {
+			cv::Point3f(p0.x(), p0.y(), p0.z()),
+			cv::Point3f(p1.x(), p1.y(), p1.z()),
+			cv::Point3f(p2.x(), p2.y(), p2.z()),
+			cv::Point3f(p3.x(), p3.y(), p3.z())
+		};
+
+		board_->ids.push_back(id);
+		board_->objPoints.push_back(obj_points);
+
+		// Add marker's static transform
+		if (!markers_frame_.empty()) {
+			geometry_msgs::TransformStamped marker_transform;
+			marker_transform.header.frame_id = markers_parent_frame_;
+			marker_transform.child_frame_id = markers_frame_ + std::to_string(id);
+			tf::transformTFToMsg(transform, marker_transform.transform);
+			markers_transforms_.push_back(marker_transform);
+		}
+
+		// Add marker to array
+		aruco_pose::Marker marker;
+		marker.id = id;
+		marker.length = length;
+		marker.pose.position.x = x;
+		marker.pose.position.y = y;
+		marker.pose.position.z = z;
+		tf::quaternionTFToMsg(q, marker.pose.orientation);
+		markers_.markers.push_back(marker);
+
+		// Add visualization marker
+		visualization_msgs::Marker vis_marker;
+		vis_marker.header.frame_id = transform_.child_frame_id;
+		vis_marker.action = visualization_msgs::Marker::ADD;
+		vis_marker.id = vis_array_.markers.size();
+		vis_marker.ns = "aruco_map_marker";
+		vis_marker.type = visualization_msgs::Marker::CUBE;
+		vis_marker.scale.x = length;
+		vis_marker.scale.y = length;
+		vis_marker.scale.z = 0.001;
+		vis_marker.color.r = 1;
+		vis_marker.color.g = 0.5;
+		vis_marker.color.b = 0.5;
+		vis_marker.color.a = 0.8;
+		vis_marker.pose.position.x = x;
+		vis_marker.pose.position.y = y;
+		vis_marker.pose.position.z = z;
+		tf::quaternionTFToMsg(q, marker.pose.orientation);
+		vis_marker.frame_locked = true;
+		vis_array_.markers.push_back(vis_marker);
+
+		// Add linking line
+		// geometry_msgs::Point p;
+		// p.x = x;
+		// p.y = y;
+		// p.z = z;
+		// vis_array_.markers.at(0).points.push_back(p);
+	}
+
+	void publishMarkersFrames()
+	{
+		if (!markers_transforms_.empty()) {
+			static_br_.sendTransform(markers_transforms_);
+		}
+	}
+
+	void publishMarkers()
+	{
+		markers_pub_.publish(markers_);
+	}
+
+	void publishMapImage()
+	{
+		cv::Size size(image_width_, image_height_);
+		cv::Mat image;
+		cv_bridge::CvImage msg;
+
+		if (!board_->ids.empty()) {
+			_drawPlanarBoard(board_, size, image, image_margin_, 1, image_axis_);
+			msg.encoding = image_axis_ ? sensor_msgs::image_encodings::RGB8 : sensor_msgs::image_encodings::MONO8;
+		} else {
+			// empty map
+			image.create(size, CV_8UC1);
+			image.setTo(cv::Scalar::all(255));
+			msg.encoding = sensor_msgs::image_encodings::MONO8;
+		}
+
+		msg.image = image;
+		img_pub_.publish(msg.toImageMsg());
+	}
+};
+
+PLUGINLIB_EXPORT_CLASS(ArucoMap, nodelet::Nodelet)

aruco_pose/src/aruco_pose.cpp

@@ -1,350 +0,0 @@
-#include <algorithm>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <pluginlib/class_list_macros.h>
-#include <geometry_msgs/TransformStamped.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/PoseWithCovarianceStamped.h>
-#include <visualization_msgs/MarkerArray.h>
-#include <tf/transform_datatypes.h>
-#include <opencv2/calib3d/calib3d.hpp>
-#include <opencv2/opencv.hpp>
-#include <opencv2/aruco.hpp>
-#include <opencv2/aruco/dictionary.hpp>
-#include <stdio.h>
-#include <tf/transform_broadcaster.h>
-
-#include "util.h"
-
-using std::vector;
-using std::string;
-
-namespace aruco_pose {
-
-class ArucoPose : public nodelet::Nodelet {
-    tf::TransformBroadcaster br;
-    cv::Ptr<cv::aruco::Dictionary> dictionary;
-    cv::Ptr<cv::aruco::DetectorParameters> parameters;
-    cv::Ptr<cv::aruco::Board> board;
-    std::string frame_id_;
-    image_transport::CameraSubscriber img_sub;
-    image_transport::Publisher img_pub;
-    ros::Publisher marker_pub;
-    ros::Publisher pose_pub;
-    ros::NodeHandle nh_, nh_priv_;
-
-    virtual void onInit();
-    void createBoard();
-    cv::Point3f getObjPointsCenter(cv::Mat objPoints);
-    void detect(const sensor_msgs::ImageConstPtr&, const sensor_msgs::CameraInfoConstPtr&);
-    void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr&, cv::Mat&, cv::Mat&);
-    tf::Transform aruco2tf(cv::Mat rvec, cv::Mat tvec);
-};
-
-void ArucoPose::onInit() {
-    ROS_INFO("Initializing aruco_pose");
-    nh_ = getNodeHandle();
-    nh_priv_ = getPrivateNodeHandle();
-
-    nh_priv_.param("frame_id", frame_id_, std::string("aruco_map"));
-
-    dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_1000);
-    parameters = cv::aruco::DetectorParameters::create();
-
-    try
-    {
-        createBoard();
-    }
-    catch (const std::exception &exc)
-    {
-        std::cerr << exc.what();
-        exit(0);
-    }
-
-    image_transport::ImageTransport it(nh_);
-    img_sub = it.subscribeCamera("image", 1, &ArucoPose::detect, this);
-
-    image_transport::ImageTransport it_priv(nh_priv_);
-    img_pub = it_priv.advertise("debug", 1);
-
-    pose_pub = nh_priv_.advertise<geometry_msgs::PoseStamped>("pose", 1);
-
-    ROS_INFO("aruco_pose nodelet inited");
-}
-
-cv::Ptr<cv::aruco::Board> createCustomGridBoard(int markersX, int markersY, float markerLength, float markerSeparationX, float markerSeparationY,
-                            const cv::Ptr<cv::aruco::Dictionary> &dictionary, std::vector<int> ids) {
-
-    CV_Assert(markersX > 0 && markersY > 0 && markerLength > 0 && markerSeparationX > 0 && markerSeparationY > 0);
-
-    cv::Ptr<cv::aruco::Board> res = cv::makePtr<cv::aruco::Board>();
-
-    res->dictionary = dictionary;
-
-    size_t totalMarkers = (size_t) markersX * markersY;
-    res->ids = ids;
-    res->objPoints.reserve(totalMarkers);
-
-    // calculate Board objPoints
-    float maxY = (float)markersY * markerLength + (markersY - 1) * markerSeparationY;
-    for(int y = 0; y < markersY; y++) {
-        for(int x = 0; x < markersX; x++) {
-            std::vector< cv::Point3f > corners;
-            corners.resize(4);
-            corners[0] = cv::Point3f(x * (markerLength + markerSeparationX),
-                                 maxY - y * (markerLength + markerSeparationY), 0);
-            corners[1] = corners[0] + cv::Point3f(markerLength, 0, 0);
-            corners[2] = corners[0] + cv::Point3f(markerLength, -markerLength, 0);
-            corners[3] = corners[0] + cv::Point3f(0, -markerLength, 0);
-            res->objPoints.push_back(corners);
-        }
-    }
-
-    return res;
-}
-
-cv::Ptr<cv::aruco::Board> createCustomBoard(std::map<string, string> markers, const cv::Ptr<cv::aruco::Dictionary> &dictionary) {
-    cv::Ptr<cv::aruco::Board> res = cv::makePtr<cv::aruco::Board>();
-
-    res->dictionary = dictionary;
-
-    size_t total_markers = markers.size();
-    res->ids.reserve(total_markers);
-    res->objPoints.reserve(total_markers);
-
-    // Generate ids and objPoints
-    for(auto const &marker : markers) {
-        res->ids.push_back(std::stoi(marker.first));
-
-        vector<string> parts;
-        parts = strSplit(marker.second, " ");
-
-        float size = std::stof(parts.at(0));
-        float x = std::stof(parts.at(1));
-        float y = std::stof(parts.at(2));
-        float z = std::stof(parts.at(3));
-        float yaw = std::stof(parts.at(4));
-        float pitch = std::stof(parts.at(5));
-        float roll = std::stof(parts.at(6));
-
-        vector<cv::Point3f> corners;
-        corners.resize(4);
-        corners[0] = cv::Point3f(x - size / 2, y + size / 2, 0);
-        corners[1] = corners[0] + cv::Point3f(size, 0, 0);
-        corners[2] = corners[0] + cv::Point3f(size, -size, 0);
-        corners[3] = corners[0] + cv::Point3f(0, -size, 0);
-
-        // TODO: process yaw, pitch, roll
-
-        res->objPoints.push_back(corners);
-    }
-
-    return res;
-}
-
-#include "fix.cpp"
-
-void ArucoPose::createBoard()
-{
-    static auto map_image_pub = nh_priv_.advertise<sensor_msgs::Image>("map_image", 1, true);
-    cv_bridge::CvImage map_image_msg;
-    cv::Mat map_image;
-
-    std::string type;
-
-    nh_priv_.param<std::string>("type", type, "gridboard");
-    if (type == "gridboard")
-    {
-        ROS_INFO("Initialize gridboard");
-
-        int markers_x, markers_y, first_marker;
-        float markers_side, markers_sep_x, markers_sep_y;
-        std::vector<int> marker_ids;
-        nh_priv_.param<int>("markers_x", markers_x, 10);
-        nh_priv_.param<int>("markers_y", markers_y, 10);
-        nh_priv_.param<int>("first_marker", first_marker, 0);
-
-        if (!nh_priv_.getParam("markers_side", markers_side))
-        {
-            ROS_ERROR("gridboard: required parameter ~markers_side is not set.");
-            exit(1);
-        }
-
-        if (!nh_priv_.getParam("markers_sep_x", markers_sep_x))
-        {
-            if (!nh_priv_.getParam("markers_sep", markers_sep_x))
-            {
-               ROS_ERROR("gridboard: ~markers_sep_x or ~markers_sep parameters are required");
-               exit(1);
-            }
-        }
-
-        if (!nh_priv_.getParam("markers_sep_y", markers_sep_y))
-        {
-            if (!nh_priv_.getParam("markers_sep", markers_sep_y))
-            {
-               ROS_ERROR("gridboard: ~markers_sep_y or ~markers_sep parameters are required");
-               exit(1);
-            }
-        }
-
-        if (nh_priv_.getParam("marker_ids", marker_ids))
-        {
-            if (markers_x * markers_y != marker_ids.size())
-            {
-                ROS_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y");
-                exit(1);
-            }
-        }
-        else
-        {
-            // Fill marker_ids automatically
-            marker_ids.resize(markers_x * markers_y);
-            for(int i = 0; i < markers_x * markers_y; i++)
-            {
-                marker_ids.at(i) = first_marker++;
-            }
-        }
-
-        // Create grid board
-        board = createCustomGridBoard(markers_x, markers_y, markers_side, markers_sep_x, markers_sep_y, dictionary, marker_ids);
-
-        // Publish map image for debugging
-        _drawPlanarBoard(board,  cv::Size(2000, 2000), map_image, 50, 1);
-
-        cv::cvtColor(map_image, map_image, CV_GRAY2BGR);
-
-        map_image_msg.encoding = sensor_msgs::image_encodings::BGR8;
-        map_image_msg.image = map_image;
-        map_image_pub.publish(map_image_msg.toImageMsg());
-    }
-    else if (type == "custom")
-    {
-        ROS_INFO("Initialize a custom board");
-
-        std::map<string, string> markers;
-        nh_priv_.getParam("markers", markers);
-
-        board = createCustomBoard(markers, dictionary);
-
-        ROS_INFO("Draw a custom board");
-        // Publish map image for debugging
-        _drawPlanarBoard(board,  cv::Size(2000, 2000), map_image, 50, 1);
-
-        cv::cvtColor(map_image, map_image, CV_GRAY2BGR);
-
-        map_image_msg.encoding = sensor_msgs::image_encodings::BGR8;
-        map_image_msg.image = map_image;
-        map_image_pub.publish(map_image_msg.toImageMsg());
-    }
-    else
-    {
-        ROS_ERROR("Incorrect map type '%s'", type.c_str());
-    }
-}
-
-cv::Point3f ArucoPose::getObjPointsCenter(cv::Mat objPoints) {
-    float min_x = std::numeric_limits<float>::max();
-    float max_x = std::numeric_limits<float>::min();
-    float min_y = min_x, max_y = max_x;
-    for (int i = 0; i < objPoints.rows; i++) {
-        max_x = std::max(max_x, objPoints.at<float>(i, 0));
-        max_y = std::max(max_y, objPoints.at<float>(i, 1));
-        min_x = std::min(min_x, objPoints.at<float>(i, 0));
-        min_y = std::min(min_y, objPoints.at<float>(i, 1));
-    }
-    cv::Point3f res((min_x + max_x) / 2, (min_y + max_y) / 2, 0);
-    return res;
-}
-
-void ArucoPose::detect(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo) {
-    cv::Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
-
-    std::vector<int> markerIds;
-    std::vector<std::vector<cv::Point2f>> markerCorners;
-    std::vector<std::vector<cv::Point2f>> rejectedCandidates;
-
-    cv::aruco::detectMarkers(image, dictionary, markerCorners, markerIds, parameters, rejectedCandidates);
-
-    cv::Mat cameraMatrix(3, 3, CV_64F);
-    cv::Mat distCoeffs(8, 1, CV_64F);
-    parseCameraInfo(cinfo, cameraMatrix, distCoeffs);
-
-    int valid = 0;
-    cv::Mat rvec, tvec, objPoints;
-
-    if (markerIds.size() > 0) {
-
-        valid = _estimatePoseBoard(markerCorners, markerIds, board, cameraMatrix, distCoeffs,
-                                       rvec, tvec, false, objPoints);
-
-        if (valid) {
-            // Send map transform
-            tf::StampedTransform transform(aruco2tf(rvec, tvec), msg->header.stamp, cinfo->header.frame_id, frame_id_);
-            br.sendTransform(transform);
-
-            // Publish map pose
-            static geometry_msgs::PoseStamped ps;
-            ps.header.frame_id = frame_id_;
-            ps.header.stamp = msg->header.stamp;
-            ps.pose.orientation.w = 1;
-            pose_pub.publish(ps);
-
-            // Send reference point
-            cv::Point3f ref = getObjPointsCenter(objPoints);
-            tf::Vector3 ref_vector3 = tf::Vector3(ref.x, ref.y, ref.z);
-            tf::Quaternion q(0, 0, 0);
-            static tf::StampedTransform ref_transform;
-            ref_transform.stamp_ = msg->header.stamp;
-            ref_transform.frame_id_ = frame_id_;
-            ref_transform.child_frame_id_ = "aruco_map_reference";
-            ref_transform.setOrigin(ref_vector3);
-            ref_transform.setRotation(q);
-            br.sendTransform(ref_transform);
-        }
-    }
-
-    if (img_pub.getNumSubscribers() > 0)
-    {
-        cv::aruco::drawDetectedMarkers(image, markerCorners, markerIds); // draw markers
-        if (valid)
-        {
-            cv::aruco::drawAxis(image, cameraMatrix, distCoeffs, rvec, tvec, 0.3); // draw board axis
-        }
-        cv_bridge::CvImage out_msg;
-        out_msg.header.frame_id = msg->header.frame_id;
-        out_msg.header.stamp = msg->header.stamp;
-        out_msg.encoding = sensor_msgs::image_encodings::BGR8;
-        out_msg.image = image;
-        img_pub.publish(out_msg.toImageMsg());
-    }
-}
-
-void ArucoPose::parseCameraInfo(const sensor_msgs::CameraInfoConstPtr &cinfo, cv::Mat &cameraMat, cv::Mat &distCoeffs) {
-    for (int i = 0; i < 3; ++i) {
-        for (int j = 0; j < 3; ++j) {
-            cameraMat.at<double>(i, j) = cinfo->K[3 * i + j];
-        }
-    }
-    for (int k = 0; k < cinfo->D.size(); k++) {
-        distCoeffs.at<double>(k) = cinfo->D[k];
-    }
-}
-
-tf::Transform ArucoPose::aruco2tf(cv::Mat rvec, cv::Mat tvec) {
-
-    cv::Mat rot;
-    cv::Rodrigues(rvec, rot);
-
-    tf::Matrix3x3 tf_rot(rot.at<double>(0,0), rot.at<double>(0,1), rot.at<double>(0,2),
-                         rot.at<double>(1,0), rot.at<double>(1,1), rot.at<double>(1,2),
-                         rot.at<double>(2,0), rot.at<double>(2,1), rot.at<double>(2,2));
-    tf::Vector3 tf_orig(tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0));
-    return tf::Transform(tf_rot, tf_orig);
-}
-
-PLUGINLIB_EXPORT_CLASS(ArucoPose, nodelet::Nodelet)
-
-}

aruco_pose/src/draw.cpp

@@ -0,0 +1,832 @@
+// This code is basically taken from https://github.com/opencv/opencv_contrib/blob/master/modules/aruco/src/aruco.cpp
+// with some improvements and fixes
+
+#include "draw.h"
+#include <math.h>
+
+using namespace cv;
+using namespace cv::aruco;
+
+static void _cvProjectPoints2( const CvMat* object_points, const CvMat* rotation_vector,
+                       const CvMat* translation_vector, const CvMat* camera_matrix,
+                       const CvMat* distortion_coeffs, CvMat* image_points,
+                       CvMat* dpdrot CV_DEFAULT(NULL), CvMat* dpdt CV_DEFAULT(NULL),
+                       CvMat* dpdf CV_DEFAULT(NULL), CvMat* dpdc CV_DEFAULT(NULL),
+                       CvMat* dpddist CV_DEFAULT(NULL),
+                       double aspect_ratio CV_DEFAULT(0));
+
+static void _projectPoints( InputArray objectPoints,
+                    InputArray rvec, InputArray tvec,
+                    InputArray cameraMatrix, InputArray distCoeffs,
+                    OutputArray imagePoints,
+                    OutputArray jacobian = noArray(),
+                    double aspectRatio = 0 );
+
+
+void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
+                      int borderBits, bool drawAxis) {
+
+	CV_Assert(outSize.area() > 0);
+	CV_Assert(marginSize >= 0);
+
+	_img.create(outSize, drawAxis ? CV_8UC3 : CV_8UC1);
+	Mat out = _img.getMat();
+	out.setTo(Scalar::all(255));
+	out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
+
+	// calculate max and min values in XY plane
+	CV_Assert(_board->objPoints.size() > 0);
+	float minX, maxX, minY, maxY;
+	minX = maxX = _board->objPoints[0][0].x;
+	minY = maxY = _board->objPoints[0][0].y;
+
+	for(unsigned int i = 0; i < _board->objPoints.size(); i++) {
+		for(int j = 0; j < 4; j++) {
+			minX = min(minX, _board->objPoints[i][j].x);
+			maxX = max(maxX, _board->objPoints[i][j].x);
+			minY = min(minY, _board->objPoints[i][j].y);
+			maxY = max(maxY, _board->objPoints[i][j].y);
+		}
+	}
+
+	float sizeX = maxX - minX;
+	float sizeY = maxY - minY;
+
+	// proportion transformations
+	float xReduction = sizeX / float(out.cols);
+	float yReduction = sizeY / float(out.rows);
+
+	// determine the zone where the markers are placed
+	if(xReduction > yReduction) {
+		int nRows = int(sizeY / xReduction);
+		int rowsMargins = (out.rows - nRows) / 2;
+		out.adjustROI(-rowsMargins, -rowsMargins, 0, 0);
+	} else {
+		int nCols = int(sizeX / yReduction);
+		int colsMargins = (out.cols - nCols) / 2;
+		out.adjustROI(0, 0, -colsMargins, -colsMargins);
+	}
+
+	// now paint each marker
+	Dictionary &dictionary = *(_board->dictionary);
+	Mat marker;
+	Point2f outCorners[3];
+	Point2f inCorners[3];
+	for(unsigned int m = 0; m < _board->objPoints.size(); m++) {
+		// transform corners to markerZone coordinates
+		for(int j = 0; j < 3; j++) {
+			Point2f pf = Point2f(_board->objPoints[m][j].x, _board->objPoints[m][j].y);
+			// move top left to 0, 0
+			pf -= Point2f(minX, minY);
+			pf.x = pf.x / sizeX * float(out.cols);
+			pf.y = (1.0f - pf.y / sizeY) * float(out.rows);
+			outCorners[j] = pf;
+		}
+
+		// get marker
+		Size dst_sz(outCorners[2] - outCorners[0]); // assuming CCW order
+		// dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
+		double diag = std::round(std::hypot(dst_sz.width, dst_sz.height));
+		int side = std::round(diag / std::sqrt(2));
+		side = std::max(side, 10);
+
+		dictionary.drawMarker(_board->ids[m], side, marker, borderBits);
+		if (drawAxis) {
+			cvtColor(marker, marker, COLOR_GRAY2RGB);
+		}
+
+		// interpolate tiny marker to marker position in markerZone
+		inCorners[0] = Point2f(-0.5f, -0.5f);
+		inCorners[1] = Point2f(marker.cols - 0.5f, -0.5f);
+		inCorners[2] = Point2f(marker.cols - 0.5f, marker.rows - 0.5f);
+
+		// remove perspective
+		Mat transformation = getAffineTransform(inCorners, outCorners);
+		warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
+						BORDER_TRANSPARENT);
+	}
+
+	// draw axis
+	if (drawAxis) {
+		Size wholeSize; Point ofs;
+		out.locateROI(wholeSize, ofs);
+		auto out_copy = _img.getMat();
+
+		cv::Point center(ofs.x - minX / sizeX * float(out.cols), ofs.y + out.rows + minY / sizeY * float(out.rows));
+
+		int axis_points[3][2] = {{300, 0}, {0, -300}, {-150, 150}};
+		Point axis_names[3] = {Point(270, 50), Point(25, -270), Point(-160, 115)};
+		Scalar colors[] = {Scalar(255, 0, 0), Scalar(0, 255, 0), Scalar(0, 0, 255)};
+		String names[] = {"X", "Y", "Z"};
+
+		int r_half = 14;
+		int height = 55;
+
+		for(int poly = 2; poly >= 0; poly--){
+			double alpha = atan2(0 - axis_points[poly][0], 0 - axis_points[poly][1]);
+			float x_delta = r_half * cos(alpha);
+			float y_delta = r_half * sin(alpha);
+
+			Point polygon_vertices[1][3];
+			polygon_vertices[0][0] = center + Point(axis_points[poly][0] + x_delta, axis_points[poly][1] - y_delta);
+			polygon_vertices[0][1] = center + Point(axis_points[poly][0] - x_delta, axis_points[poly][1] + y_delta);
+			polygon_vertices[0][2] = center + Point(axis_points[poly][0] - sin(alpha) * height, axis_points[poly][1] - cos(alpha) * height);
+
+			const Point* ppt[1] = {polygon_vertices[0]};
+			int npt[] = {3};
+
+			fillPoly(out_copy, ppt, npt, 1, colors[poly]);
+			putText(out_copy, names[poly], center + axis_names[poly], FONT_HERSHEY_SIMPLEX, 1.2, colors[poly], 7);
+			line(out_copy, center, center + Point(axis_points[poly][0], axis_points[poly][1]), colors[poly], 10);
+		}
+	}
+}
+
+/* Draw a (potentially partially visible) line. */
+static void linePartial(InputOutputArray image, Point3f p1, Point3f p2, const Scalar& color,
+                        int thickness = 1, int lineType = LINE_8, int shift = 0)
+{
+	// If both points are behind the screen, don't draw anything
+	if (p1.z <= 0 && p2.z <= 0) {
+		return;
+	}
+	Point2f p1p{p1.x, p1.y};
+	Point2f p2p{p2.x, p2.y};
+	// If points are on the different sides of the plane, compute intersection point
+	if (p1.z * p2.z < 0) {
+		// Compute intersection point with the screen
+		// We denote alpha as such:
+		// xi = (1 - alpha) * x1 + alpha * x2
+		// yi = (1 - alpha) * y1 + alpha * y2
+		// zi = (1 - alpha) * z1 + alpha * z2 = 0
+		// Thus, alpha can be expressed as
+		// alpha = z1 / (z1 - z2)
+		float alpha = p1.z / (p1.z - p2.z);
+		Point2f pi{(1 - alpha) * p1.x + alpha * p2.x, (1 - alpha) * p1.y + alpha * p2.y};
+		// Now, if z1 is negative, we draw the line from (xi, yi) to (x2, y2), else we draw from (x1, y1) to (xi, yi)
+		if (p1.z < 0) {
+			p1p = pi;
+		} else {
+			p2p = pi;
+		}
+	}
+	line(image, p1p, p2p, color, thickness, lineType, shift);
+}
+
+void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs,
+              InputArray _rvec, InputArray _tvec, float length) {
+
+	CV_Assert(_image.getMat().total() != 0 &&
+			  (_image.getMat().channels() == 1 || _image.getMat().channels() == 3));
+	CV_Assert(length > 0);
+
+	// project axis points
+	std::vector<Point3f> axisPoints;
+	axisPoints.push_back(Point3f(0, 0, 0));
+	axisPoints.push_back(Point3f(length, 0, 0));
+	axisPoints.push_back(Point3f(0, length, 0));
+	axisPoints.push_back(Point3f(0, 0, length));
+	std::vector<Point3f> imagePointsZ;
+	_projectPoints(axisPoints, _rvec, _tvec, _cameraMatrix, _distCoeffs, imagePointsZ);
+
+	// draw axis lines
+	linePartial(_image, imagePointsZ[0], imagePointsZ[1], Scalar(0, 0, 255), 3);
+	linePartial(_image, imagePointsZ[0], imagePointsZ[2], Scalar(0, 255, 0), 3);
+	linePartial(_image, imagePointsZ[0], imagePointsZ[3], Scalar(255, 0, 0), 3);
+}
+
+static CvMat _cvMat(const cv::Mat& m)
+{
+	CvMat self;
+	CV_DbgAssert(m.dims <= 2);
+	self = cvMat(m.rows, m.dims == 1 ? 1 : m.cols, m.type(), m.data);
+	self.step = (int)m.step[0];
+	self.type = (self.type & ~cv::Mat::CONTINUOUS_FLAG) | (m.flags & cv::Mat::CONTINUOUS_FLAG);
+	return self;
+}
+
+static void _projectPoints( InputArray _opoints,
+                        InputArray _rvec,
+                        InputArray _tvec,
+                        InputArray _cameraMatrix,
+                        InputArray _distCoeffs,
+                        OutputArray _ipoints,
+                        OutputArray _jacobian,
+                        double aspectRatio )
+{
+	Mat opoints = _opoints.getMat();
+	int npoints = opoints.checkVector(3), depth = opoints.depth();
+	CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_64F));
+
+	CvMat dpdrot, dpdt, dpdf, dpdc, dpddist;
+	CvMat *pdpdrot = 0, *pdpdt = 0, *pdpdf = 0, *pdpdc = 0, *pdpddist = 0;
+
+	CV_Assert(_ipoints.needed());
+
+	_ipoints.create(npoints, 1, CV_MAKETYPE(depth, 3), -1, true);
+	Mat imagePoints = _ipoints.getMat();
+	CvMat c_imagePoints = _cvMat(imagePoints);
+	CvMat c_objectPoints = _cvMat(opoints);
+	Mat cameraMatrix = _cameraMatrix.getMat();
+
+	Mat rvec = _rvec.getMat(), tvec = _tvec.getMat();
+	CvMat c_cameraMatrix = _cvMat(cameraMatrix);
+	CvMat c_rvec = _cvMat(rvec), c_tvec = _cvMat(tvec);
+
+	double dc0buf[5] = {0};
+	Mat dc0(5, 1, CV_64F, dc0buf);
+	Mat distCoeffs = _distCoeffs.getMat();
+	if (distCoeffs.empty())
+		distCoeffs = dc0;
+	CvMat c_distCoeffs = _cvMat(distCoeffs);
+	int ndistCoeffs = distCoeffs.rows + distCoeffs.cols - 1;
+
+	Mat jacobian;
+	if (_jacobian.needed())
+	{
+		_jacobian.create(npoints * 2, 3 + 3 + 2 + 2 + ndistCoeffs, CV_64F);
+		jacobian = _jacobian.getMat();
+		pdpdrot = &(dpdrot = _cvMat(jacobian.colRange(0, 3)));
+		pdpdt = &(dpdt = _cvMat(jacobian.colRange(3, 6)));
+		pdpdf = &(dpdf = _cvMat(jacobian.colRange(6, 8)));
+		pdpdc = &(dpdc = _cvMat(jacobian.colRange(8, 10)));
+		pdpddist = &(dpddist = _cvMat(jacobian.colRange(10, 10 + ndistCoeffs)));
+	}
+
+	_cvProjectPoints2(&c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs,
+					  &c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio);
+}
+
+namespace _detail
+{
+    template <typename FLOAT>
+    void computeTiltProjectionMatrix(FLOAT tauX,
+                                     FLOAT tauY,
+                                     Matx<FLOAT, 3, 3>* matTilt = 0,
+                                     Matx<FLOAT, 3, 3>* dMatTiltdTauX = 0,
+                                     Matx<FLOAT, 3, 3>* dMatTiltdTauY = 0,
+                                     Matx<FLOAT, 3, 3>* invMatTilt = 0)
+    {
+        FLOAT cTauX = cos(tauX);
+        FLOAT sTauX = sin(tauX);
+        FLOAT cTauY = cos(tauY);
+        FLOAT sTauY = sin(tauY);
+        Matx<FLOAT, 3, 3> matRotX = Matx<FLOAT, 3, 3>(1,0,0,0,cTauX,sTauX,0,-sTauX,cTauX);
+        Matx<FLOAT, 3, 3> matRotY = Matx<FLOAT, 3, 3>(cTauY,0,-sTauY,0,1,0,sTauY,0,cTauY);
+        Matx<FLOAT, 3, 3> matRotXY = matRotY * matRotX;
+        Matx<FLOAT, 3, 3> matProjZ = Matx<FLOAT, 3, 3>(matRotXY(2,2),0,-matRotXY(0,2),0,matRotXY(2,2),-matRotXY(1,2),0,0,1);
+        if (matTilt)
+        {
+            // Matrix for trapezoidal distortion of tilted image sensor
+            *matTilt = matProjZ * matRotXY;
+        }
+        if (dMatTiltdTauX)
+        {
+            // Derivative with respect to tauX
+            Matx<FLOAT, 3, 3> dMatRotXYdTauX = matRotY * Matx<FLOAT, 3, 3>(0,0,0,0,-sTauX,cTauX,0,-cTauX,-sTauX);
+            Matx<FLOAT, 3, 3> dMatProjZdTauX = Matx<FLOAT, 3, 3>(dMatRotXYdTauX(2,2),0,-dMatRotXYdTauX(0,2),
+                                                                 0,dMatRotXYdTauX(2,2),-dMatRotXYdTauX(1,2),0,0,0);
+            *dMatTiltdTauX = (matProjZ * dMatRotXYdTauX) + (dMatProjZdTauX * matRotXY);
+        }
+        if (dMatTiltdTauY)
+        {
+            // Derivative with respect to tauY
+            Matx<FLOAT, 3, 3> dMatRotXYdTauY = Matx<FLOAT, 3, 3>(-sTauY,0,-cTauY,0,0,0,cTauY,0,-sTauY) * matRotX;
+            Matx<FLOAT, 3, 3> dMatProjZdTauY = Matx<FLOAT, 3, 3>(dMatRotXYdTauY(2,2),0,-dMatRotXYdTauY(0,2),
+                                                                 0,dMatRotXYdTauY(2,2),-dMatRotXYdTauY(1,2),0,0,0);
+            *dMatTiltdTauY = (matProjZ * dMatRotXYdTauY) + (dMatProjZdTauY * matRotXY);
+        }
+        if (invMatTilt)
+        {
+            FLOAT inv = 1./matRotXY(2,2);
+            Matx<FLOAT, 3, 3> invMatProjZ = Matx<FLOAT, 3, 3>(inv,0,inv*matRotXY(0,2),0,inv,inv*matRotXY(1,2),0,0,1);
+            *invMatTilt = matRotXY.t()*invMatProjZ;
+        }
+    }
+}
+
+static const char* cvDistCoeffErr = "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector";
+
+static void _cvProjectPoints2Internal( const CvMat* objectPoints,
+                                      const CvMat* r_vec,
+                                      const CvMat* t_vec,
+                                      const CvMat* A,
+                                      const CvMat* distCoeffs,
+                                      CvMat* imagePoints, CvMat* dpdr CV_DEFAULT(NULL),
+                                      CvMat* dpdt CV_DEFAULT(NULL), CvMat* dpdf CV_DEFAULT(NULL),
+                                      CvMat* dpdc CV_DEFAULT(NULL), CvMat* dpdk CV_DEFAULT(NULL),
+                                      CvMat* dpdo CV_DEFAULT(NULL),
+                                      double aspectRatio CV_DEFAULT(0) )
+{
+    Ptr<CvMat> matM, _m;
+    Ptr<CvMat> _dpdr, _dpdt, _dpdc, _dpdf, _dpdk;
+    Ptr<CvMat> _dpdo;
+
+    int i, j, count;
+    int calc_derivatives;
+    const CvPoint3D64f* M;
+    CvPoint3D64f* m;
+    double r[3], R[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy;
+    Matx33d matTilt = Matx33d::eye();
+    Matx33d dMatTiltdTauX(0,0,0,0,0,0,0,-1,0);
+    Matx33d dMatTiltdTauY(0,0,0,0,0,0,1,0,0);
+    CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
+    CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );
+    double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0;
+    double* dpdo_p = 0;
+    int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0;
+    int dpdo_step = 0;
+    bool fixedAspectRatio = aspectRatio > FLT_EPSILON;
+
+    if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) ||
+        !CV_IS_MAT(t_vec) || !CV_IS_MAT(A) ||
+        /*!CV_IS_MAT(distCoeffs) ||*/ !CV_IS_MAT(imagePoints) )
+        CV_Error( CV_StsBadArg, "One of required arguments is not a valid matrix" );
+
+    int total = objectPoints->rows * objectPoints->cols * CV_MAT_CN(objectPoints->type);
+    if(total % 3 != 0)
+    {
+        //we have stopped support of homogeneous coordinates because it cause ambiguity in interpretation of the input data
+        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
+    }
+    count = total / 3;
+
+    if( CV_IS_CONT_MAT(objectPoints->type) &&
+        (CV_MAT_DEPTH(objectPoints->type) == CV_32F || CV_MAT_DEPTH(objectPoints->type) == CV_64F)&&
+        ((objectPoints->rows == 1 && CV_MAT_CN(objectPoints->type) == 3) ||
+         (objectPoints->rows == count && CV_MAT_CN(objectPoints->type)*objectPoints->cols == 3) ||
+         (objectPoints->rows == 3 && CV_MAT_CN(objectPoints->type) == 1 && objectPoints->cols == count)))
+    {
+        matM.reset(cvCreateMat( objectPoints->rows, objectPoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(objectPoints->type)) ));
+        cvConvert(objectPoints, matM);
+    }
+    else
+    {
+//        matM = cvCreateMat( 1, count, CV_64FC3 );
+//        cvConvertPointsHomogeneous( objectPoints, matM );
+        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
+    }
+
+    if( CV_IS_CONT_MAT(imagePoints->type) &&
+        (CV_MAT_DEPTH(imagePoints->type) == CV_32F || CV_MAT_DEPTH(imagePoints->type) == CV_64F) &&
+        ((imagePoints->rows == 1 && CV_MAT_CN(imagePoints->type) == 3) ||
+         (imagePoints->rows == count && CV_MAT_CN(imagePoints->type)*imagePoints->cols == 3) ||
+         (imagePoints->rows == 3 && CV_MAT_CN(imagePoints->type) == 1 && imagePoints->cols == count)))
+    {
+        _m.reset(cvCreateMat( imagePoints->rows, imagePoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(imagePoints->type)) ));
+        cvConvert(imagePoints, _m);
+    }
+    else
+    {
+//        _m = cvCreateMat( 1, count, CV_64FC2 );
+        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
+    }
+
+    M = (CvPoint3D64f*)matM->data.db;
+    m = (CvPoint3D64f*)_m->data.db;
+
+    if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) ||
+        (((r_vec->rows != 1 && r_vec->cols != 1) ||
+          r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) &&
+         ((r_vec->rows != 3 && r_vec->cols != 3) || CV_MAT_CN(r_vec->type) != 1)))
+        CV_Error( CV_StsBadArg, "Rotation must be represented by 1x3 or 3x1 "
+                                "floating-point rotation vector, or 3x3 rotation matrix" );
+
+    if( r_vec->rows == 3 && r_vec->cols == 3 )
+    {
+        _r = cvMat( 3, 1, CV_64FC1, r );
+        cvRodrigues2( r_vec, &_r );
+        cvRodrigues2( &_r, &matR, &_dRdr );
+        cvCopy( r_vec, &matR );
+    }
+    else
+    {
+        _r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r );
+        cvConvert( r_vec, &_r );
+        cvRodrigues2( &_r, &matR, &_dRdr );
+    }
+
+    if( (CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F) ||
+        (t_vec->rows != 1 && t_vec->cols != 1) ||
+        t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
+        CV_Error( CV_StsBadArg,
+                  "Translation vector must be 1x3 or 3x1 floating-point vector" );
+
+    _t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t );
+    cvConvert( t_vec, &_t );
+
+    if( (CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1) ||
+        A->rows != 3 || A->cols != 3 )
+        CV_Error( CV_StsBadArg, "Instrinsic parameters must be 3x3 floating-point matrix" );
+
+    cvConvert( A, &_a );
+    fx = a[0]; fy = a[4];
+    cx = a[2]; cy = a[5];
+
+    if( fixedAspectRatio )
+        fx = fy*aspectRatio;
+
+    if( distCoeffs )
+    {
+        if( !CV_IS_MAT(distCoeffs) ||
+            (CV_MAT_DEPTH(distCoeffs->type) != CV_64F &&
+             CV_MAT_DEPTH(distCoeffs->type) != CV_32F) ||
+            (distCoeffs->rows != 1 && distCoeffs->cols != 1) ||
+            (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 &&
+             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5 &&
+             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 8 &&
+             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 12 &&
+             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 14) )
+            CV_Error( CV_StsBadArg, cvDistCoeffErr );
+
+        _k = cvMat( distCoeffs->rows, distCoeffs->cols,
+                    CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k );
+        cvConvert( distCoeffs, &_k );
+        if(k[12] != 0 || k[13] != 0)
+        {
+            _detail::computeTiltProjectionMatrix(k[12], k[13],
+                                                &matTilt, &dMatTiltdTauX, &dMatTiltdTauY);
+        }
+    }
+
+    if( dpdr )
+    {
+        if( !CV_IS_MAT(dpdr) ||
+            (CV_MAT_TYPE(dpdr->type) != CV_32FC1 &&
+             CV_MAT_TYPE(dpdr->type) != CV_64FC1) ||
+            dpdr->rows != count*2 || dpdr->cols != 3 )
+            CV_Error( CV_StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" );
+
+        if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 )
+        {
+            _dpdr.reset(cvCloneMat(dpdr));
+        }
+        else
+            _dpdr.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
+        dpdr_p = _dpdr->data.db;
+        dpdr_step = _dpdr->step/sizeof(dpdr_p[0]);
+    }
+
+    if( dpdt )
+    {
+        if( !CV_IS_MAT(dpdt) ||
+            (CV_MAT_TYPE(dpdt->type) != CV_32FC1 &&
+             CV_MAT_TYPE(dpdt->type) != CV_64FC1) ||
+            dpdt->rows != count*2 || dpdt->cols != 3 )
+            CV_Error( CV_StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" );
+
+        if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 )
+        {
+            _dpdt.reset(cvCloneMat(dpdt));
+        }
+        else
+            _dpdt.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
+        dpdt_p = _dpdt->data.db;
+        dpdt_step = _dpdt->step/sizeof(dpdt_p[0]);
+    }
+
+    if( dpdf )
+    {
+        if( !CV_IS_MAT(dpdf) ||
+            (CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1) ||
+            dpdf->rows != count*2 || dpdf->cols != 2 )
+            CV_Error( CV_StsBadArg, "dp/df must be 2Nx2 floating-point matrix" );
+
+        if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 )
+        {
+            _dpdf.reset(cvCloneMat(dpdf));
+        }
+        else
+            _dpdf.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
+        dpdf_p = _dpdf->data.db;
+        dpdf_step = _dpdf->step/sizeof(dpdf_p[0]);
+    }
+
+    if( dpdc )
+    {
+        if( !CV_IS_MAT(dpdc) ||
+            (CV_MAT_TYPE(dpdc->type) != CV_32FC1 && CV_MAT_TYPE(dpdc->type) != CV_64FC1) ||
+            dpdc->rows != count*2 || dpdc->cols != 2 )
+            CV_Error( CV_StsBadArg, "dp/dc must be 2Nx2 floating-point matrix" );
+
+        if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 )
+        {
+            _dpdc.reset(cvCloneMat(dpdc));
+        }
+        else
+            _dpdc.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
+        dpdc_p = _dpdc->data.db;
+        dpdc_step = _dpdc->step/sizeof(dpdc_p[0]);
+    }
+
+    if( dpdk )
+    {
+        if( !CV_IS_MAT(dpdk) ||
+            (CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) ||
+            dpdk->rows != count*2 || (dpdk->cols != 14 && dpdk->cols != 12 && dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) )
+            CV_Error( CV_StsBadArg, "dp/df must be 2Nx14, 2Nx12, 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" );
+
+        if( !distCoeffs )
+            CV_Error( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" );
+
+        if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 )
+        {
+            _dpdk.reset(cvCloneMat(dpdk));
+        }
+        else
+            _dpdk.reset(cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 ));
+        dpdk_p = _dpdk->data.db;
+        dpdk_step = _dpdk->step/sizeof(dpdk_p[0]);
+    }
+
+    if( dpdo )
+    {
+        if( !CV_IS_MAT( dpdo ) || ( CV_MAT_TYPE( dpdo->type ) != CV_32FC1
+                                    && CV_MAT_TYPE( dpdo->type ) != CV_64FC1 )
+            || dpdo->rows != count * 2 || dpdo->cols != count * 3 )
+            CV_Error( CV_StsBadArg, "dp/do must be 2Nx3N floating-point matrix" );
+
+        if( CV_MAT_TYPE( dpdo->type ) == CV_64FC1 )
+        {
+            _dpdo.reset( cvCloneMat( dpdo ) );
+        }
+        else
+            _dpdo.reset( cvCreateMat( 2 * count, 3 * count, CV_64FC1 ) );
+        cvZero(_dpdo);
+        dpdo_p = _dpdo->data.db;
+        dpdo_step = _dpdo->step / sizeof( dpdo_p[0] );
+    }
+
+    calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk || dpdo;
+
+    for( i = 0; i < count; i++ )
+    {
+        double X = M[i].x, Y = M[i].y, Z = M[i].z;
+        double x = R[0]*X + R[1]*Y + R[2]*Z + t[0];
+        double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
+        double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
+        double r2, r4, r6, a1, a2, a3, cdist, icdist2;
+        double xd, yd, xd0, yd0, invProj;
+        Vec3d vecTilt;
+        Vec3d dVecTilt;
+        Matx22d dMatTilt;
+        Vec2d dXdYd;
+
+        double z0 = z;
+        z = z ? 1./z : 1;
+        x *= z; y *= z;
+
+        r2 = x*x + y*y;
+        r4 = r2*r2;
+        r6 = r4*r2;
+        a1 = 2*x*y;
+        a2 = r2 + 2*x*x;
+        a3 = r2 + 2*y*y;
+        cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
+        icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
+        xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
+        yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
+
+        // additional distortion by projecting onto a tilt plane
+        vecTilt = matTilt*Vec3d(xd0, yd0, 1);
+        invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
+        xd = invProj * vecTilt(0);
+        yd = invProj * vecTilt(1);
+
+        m[i].x = xd*fx + cx;
+        m[i].y = yd*fy + cy;
+        m[i].z = z; // Just put the projected Z coordinate here, we mainly care about the sign
+
+        if( calc_derivatives )
+        {
+            if( dpdc_p )
+            {
+                dpdc_p[0] = 1; dpdc_p[1] = 0; // dp_xdc_x; dp_xdc_y
+                dpdc_p[dpdc_step] = 0;
+                dpdc_p[dpdc_step+1] = 1;
+                dpdc_p += dpdc_step*2;
+            }
+
+            if( dpdf_p )
+            {
+                if( fixedAspectRatio )
+                {
+                    dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio; // dp_xdf_x; dp_xdf_y
+                    dpdf_p[dpdf_step] = 0;
+                    dpdf_p[dpdf_step+1] = yd;
+                }
+                else
+                {
+                    dpdf_p[0] = xd; dpdf_p[1] = 0;
+                    dpdf_p[dpdf_step] = 0;
+                    dpdf_p[dpdf_step+1] = yd;
+                }
+                dpdf_p += dpdf_step*2;
+            }
+            for (int row = 0; row < 2; ++row)
+                for (int col = 0; col < 2; ++col)
+                    dMatTilt(row,col) = matTilt(row,col)*vecTilt(2)
+                                        - matTilt(2,col)*vecTilt(row);
+            double invProjSquare = (invProj*invProj);
+            dMatTilt *= invProjSquare;
+            if( dpdk_p )
+            {
+                dXdYd = dMatTilt*Vec2d(x*icdist2*r2, y*icdist2*r2);
+                dpdk_p[0] = fx*dXdYd(0);
+                dpdk_p[dpdk_step] = fy*dXdYd(1);
+                dXdYd = dMatTilt*Vec2d(x*icdist2*r4, y*icdist2*r4);
+                dpdk_p[1] = fx*dXdYd(0);
+                dpdk_p[dpdk_step+1] = fy*dXdYd(1);
+                if( _dpdk->cols > 2 )
+                {
+                    dXdYd = dMatTilt*Vec2d(a1, a3);
+                    dpdk_p[2] = fx*dXdYd(0);
+                    dpdk_p[dpdk_step+2] = fy*dXdYd(1);
+                    dXdYd = dMatTilt*Vec2d(a2, a1);
+                    dpdk_p[3] = fx*dXdYd(0);
+                    dpdk_p[dpdk_step+3] = fy*dXdYd(1);
+                    if( _dpdk->cols > 4 )
+                    {
+                        dXdYd = dMatTilt*Vec2d(x*icdist2*r6, y*icdist2*r6);
+                        dpdk_p[4] = fx*dXdYd(0);
+                        dpdk_p[dpdk_step+4] = fy*dXdYd(1);
+
+                        if( _dpdk->cols > 5 )
+                        {
+                            dXdYd = dMatTilt*Vec2d(
+                                    x*cdist*(-icdist2)*icdist2*r2, y*cdist*(-icdist2)*icdist2*r2);
+                            dpdk_p[5] = fx*dXdYd(0);
+                            dpdk_p[dpdk_step+5] = fy*dXdYd(1);
+                            dXdYd = dMatTilt*Vec2d(
+                                    x*cdist*(-icdist2)*icdist2*r4, y*cdist*(-icdist2)*icdist2*r4);
+                            dpdk_p[6] = fx*dXdYd(0);
+                            dpdk_p[dpdk_step+6] = fy*dXdYd(1);
+                            dXdYd = dMatTilt*Vec2d(
+                                    x*cdist*(-icdist2)*icdist2*r6, y*cdist*(-icdist2)*icdist2*r6);
+                            dpdk_p[7] = fx*dXdYd(0);
+                            dpdk_p[dpdk_step+7] = fy*dXdYd(1);
+                            if( _dpdk->cols > 8 )
+                            {
+                                dXdYd = dMatTilt*Vec2d(r2, 0);
+                                dpdk_p[8] = fx*dXdYd(0); //s1
+                                dpdk_p[dpdk_step+8] = fy*dXdYd(1); //s1
+                                dXdYd = dMatTilt*Vec2d(r4, 0);
+                                dpdk_p[9] = fx*dXdYd(0); //s2
+                                dpdk_p[dpdk_step+9] = fy*dXdYd(1); //s2
+                                dXdYd = dMatTilt*Vec2d(0, r2);
+                                dpdk_p[10] = fx*dXdYd(0);//s3
+                                dpdk_p[dpdk_step+10] = fy*dXdYd(1); //s3
+                                dXdYd = dMatTilt*Vec2d(0, r4);
+                                dpdk_p[11] = fx*dXdYd(0);//s4
+                                dpdk_p[dpdk_step+11] = fy*dXdYd(1); //s4
+                                if( _dpdk->cols > 12 )
+                                {
+                                    dVecTilt = dMatTiltdTauX * Vec3d(xd0, yd0, 1);
+                                    dpdk_p[12] = fx * invProjSquare * (
+                                            dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
+                                    dpdk_p[dpdk_step+12] = fy*invProjSquare * (
+                                            dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
+                                    dVecTilt = dMatTiltdTauY * Vec3d(xd0, yd0, 1);
+                                    dpdk_p[13] = fx * invProjSquare * (
+                                            dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
+                                    dpdk_p[dpdk_step+13] = fy * invProjSquare * (
+                                            dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
+                                }
+                            }
+                        }
+                    }
+                }
+                dpdk_p += dpdk_step*2;
+            }
+
+            if( dpdt_p )
+            {
+                double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };
+                for( j = 0; j < 3; j++ )
+                {
+                    double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j];
+                    double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt;
+                    double dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt);
+                    double da1dt = 2*(x*dydt[j] + y*dxdt[j]);
+                    double dmxdt = (dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt +
+                                    k[2]*da1dt + k[3]*(dr2dt + 4*x*dxdt[j]) + k[8]*dr2dt + 2*r2*k[9]*dr2dt);
+                    double dmydt = (dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt +
+                                    k[2]*(dr2dt + 4*y*dydt[j]) + k[3]*da1dt + k[10]*dr2dt + 2*r2*k[11]*dr2dt);
+                    dXdYd = dMatTilt*Vec2d(dmxdt, dmydt);
+                    dpdt_p[j] = fx*dXdYd(0);
+                    dpdt_p[dpdt_step+j] = fy*dXdYd(1);
+                }
+                dpdt_p += dpdt_step*2;
+            }
+
+            if( dpdr_p )
+            {
+                double dx0dr[] =
+                        {
+                                X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2],
+                                X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11],
+                                X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20]
+                        };
+                double dy0dr[] =
+                        {
+                                X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5],
+                                X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14],
+                                X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23]
+                        };
+                double dz0dr[] =
+                        {
+                                X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8],
+                                X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17],
+                                X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26]
+                        };
+                for( j = 0; j < 3; j++ )
+                {
+                    double dxdr = z*(dx0dr[j] - x*dz0dr[j]);
+                    double dydr = z*(dy0dr[j] - y*dz0dr[j]);
+                    double dr2dr = 2*x*dxdr + 2*y*dydr;
+                    double dcdist_dr = (k[0] + 2*k[1]*r2 + 3*k[4]*r4)*dr2dr;
+                    double dicdist2_dr = -icdist2*icdist2*(k[5] + 2*k[6]*r2 + 3*k[7]*r4)*dr2dr;
+                    double da1dr = 2*(x*dydr + y*dxdr);
+                    double dmxdr = (dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr +
+                                    k[2]*da1dr + k[3]*(dr2dr + 4*x*dxdr) + (k[8] + 2*r2*k[9])*dr2dr);
+                    double dmydr = (dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr +
+                                    k[2]*(dr2dr + 4*y*dydr) + k[3]*da1dr + (k[10] + 2*r2*k[11])*dr2dr);
+                    dXdYd = dMatTilt*Vec2d(dmxdr, dmydr);
+                    dpdr_p[j] = fx*dXdYd(0);
+                    dpdr_p[dpdr_step+j] = fy*dXdYd(1);
+                }
+                dpdr_p += dpdr_step*2;
+            }
+
+            if( dpdo_p )
+            {
+                double dxdo[] = { z * ( R[0] - x * z * z0 * R[6] ),
+                                  z * ( R[1] - x * z * z0 * R[7] ),
+                                  z * ( R[2] - x * z * z0 * R[8] ) };
+                double dydo[] = { z * ( R[3] - y * z * z0 * R[6] ),
+                                  z * ( R[4] - y * z * z0 * R[7] ),
+                                  z * ( R[5] - y * z * z0 * R[8] ) };
+                for( j = 0; j < 3; j++ )
+                {
+                    double dr2do = 2 * x * dxdo[j] + 2 * y * dydo[j];
+                    double dr4do = 2 * r2 * dr2do;
+                    double dr6do = 3 * r4 * dr2do;
+                    double da1do = 2 * y * dxdo[j] + 2 * x * dydo[j];
+                    double da2do = dr2do + 4 * x * dxdo[j];
+                    double da3do = dr2do + 4 * y * dydo[j];
+                    double dcdist_do
+                            = k[0] * dr2do + k[1] * dr4do + k[4] * dr6do;
+                    double dicdist2_do = -icdist2 * icdist2
+                                         * ( k[5] * dr2do + k[6] * dr4do + k[7] * dr6do );
+                    double dxd0_do = cdist * icdist2 * dxdo[j]
+                                     + x * icdist2 * dcdist_do + x * cdist * dicdist2_do
+                                     + k[2] * da1do + k[3] * da2do + k[8] * dr2do
+                                     + k[9] * dr4do;
+                    double dyd0_do = cdist * icdist2 * dydo[j]
+                                     + y * icdist2 * dcdist_do + y * cdist * dicdist2_do
+                                     + k[2] * da3do + k[3] * da1do + k[10] * dr2do
+                                     + k[11] * dr4do;
+                    dXdYd = dMatTilt * Vec2d( dxd0_do, dyd0_do );
+                    dpdo_p[i * 3 + j] = fx * dXdYd( 0 );
+                    dpdo_p[dpdo_step + i * 3 + j] = fy * dXdYd( 1 );
+                }
+                dpdo_p += dpdo_step * 2;
+            }
+        }
+    }
+
+    if( _m != imagePoints )
+        cvConvert( _m, imagePoints );
+
+    if( _dpdr != dpdr )
+        cvConvert( _dpdr, dpdr );
+
+    if( _dpdt != dpdt )
+        cvConvert( _dpdt, dpdt );
+
+    if( _dpdf != dpdf )
+        cvConvert( _dpdf, dpdf );
+
+    if( _dpdc != dpdc )
+        cvConvert( _dpdc, dpdc );
+
+    if( _dpdk != dpdk )
+        cvConvert( _dpdk, dpdk );
+
+    if( _dpdo != dpdo )
+        cvConvert( _dpdo, dpdo );
+}
+
+static void _cvProjectPoints2( const CvMat* objectPoints,
+                        const CvMat* r_vec,
+                        const CvMat* t_vec,
+                        const CvMat* A,
+                        const CvMat* distCoeffs,
+                        CvMat* imagePoints, CvMat* dpdr,
+                        CvMat* dpdt, CvMat* dpdf,
+                        CvMat* dpdc, CvMat* dpdk,
+                        double aspectRatio )
+{
+    _cvProjectPoints2Internal( objectPoints, r_vec, t_vec, A, distCoeffs, imagePoints, dpdr, dpdt,
+                               dpdf, dpdc, dpdk, NULL, aspectRatio );
+}

aruco_pose/src/draw.h

@@ -0,0 +1,9 @@
+#include <cmath>
+#include <ros/ros.h>
+#include <opencv2/opencv.hpp>
+#include <opencv2/aruco.hpp>
+
+void _drawPlanarBoard(cv::aruco::Board *_board, cv::Size outSize, cv::OutputArray _img,
+                      int marginSize, int borderBits, bool drawAxis); // editorconfig-checker-disable-line
+void _drawAxis(cv::InputOutputArray image, cv::InputArray cameraMatrix, cv::InputArray distCoeffs,
+               cv::InputArray rvec, cv::InputArray tvec, float length); // editorconfig-checker-disable-line

aruco_pose/src/fix.cpp

@@ -1,145 +0,0 @@
-using namespace cv;
-using namespace cv::aruco;
-
-// Temporal fix!
-// TODO: remove
-// fix strange bug in our OpenCV version
-
-void _getBoardObjectAndImagePoints(const Ptr<aruco::Board> &board, InputArrayOfArrays detectedCorners,
-    InputArray detectedIds, OutputArray objPoints, OutputArray imgPoints) {
-
-    CV_Assert(board->ids.size() == board->objPoints.size());
-    CV_Assert(detectedIds.total() == detectedCorners.total());
-
-    size_t nDetectedMarkers = detectedIds.total();
-
-    std::vector< Point3f > objPnts;
-    objPnts.reserve(nDetectedMarkers);
-
-    std::vector< Point2f > imgPnts;
-    imgPnts.reserve(nDetectedMarkers);
-
-    // look for detected markers that belong to the board and get their information
-    for(unsigned int i = 0; i < nDetectedMarkers; i++) {
-        int currentId = detectedIds.getMat().ptr< int >(0)[i];
-        for(unsigned int j = 0; j < board->ids.size(); j++) {
-            if(currentId == board->ids[j]) {
-                for(int p = 0; p < 4; p++) {
-                    objPnts.push_back(board->objPoints[j][p]);
-                    imgPnts.push_back(detectedCorners.getMat(i).ptr< Point2f >(0)[p]);
-                }
-            }
-        }
-    }
-
-    // create output
-    Mat(objPnts).copyTo(objPoints);
-    Mat(imgPnts).copyTo(imgPoints);
-}
-
-int _estimatePoseBoard(InputArrayOfArrays _corners, InputArray _ids, const Ptr<aruco::Board> &board,
-                      InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec,
-                      OutputArray _tvec, bool useExtrinsicGuess, Mat &objPoints) {
-
-    CV_Assert(_corners.total() == _ids.total());
-
-    // get object and image points for the solvePnP function
-    Mat /*objPoints, */imgPoints;
-    _getBoardObjectAndImagePoints(board, _corners, _ids, objPoints, imgPoints);
-
-    CV_Assert(imgPoints.total() == objPoints.total());
-
-    if(objPoints.total() == 0) // 0 of the detected markers in board
-        return 0;
-
-//    std::cout << "objPoints: " << objPoints << std::endl;
-//    std::cout << "imgPoints: " << imgPoints << std::endl;
-
-    solvePnP(objPoints, imgPoints, _cameraMatrix, _distCoeffs, _rvec, _tvec, useExtrinsicGuess);
-
-    // divide by four since all the four corners are concatenated in the array for each marker
-    return (int)objPoints.total() / 4;
-}
-
-void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
-                     int borderBits) {
-
-    CV_Assert(outSize.area() > 0);
-    CV_Assert(marginSize >= 0);
-
-    _img.create(outSize, CV_8UC1);
-    Mat out = _img.getMat();
-    out.setTo(Scalar::all(255));
-    out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
-
-    // calculate max and min values in XY plane
-    CV_Assert(_board->objPoints.size() > 0);
-    float minX, maxX, minY, maxY;
-    minX = maxX = _board->objPoints[0][0].x;
-    minY = maxY = _board->objPoints[0][0].y;
-
-    for(unsigned int i = 0; i < _board->objPoints.size(); i++) {
-        for(int j = 0; j < 4; j++) {
-            minX = min(minX, _board->objPoints[i][j].x);
-            maxX = max(maxX, _board->objPoints[i][j].x);
-            minY = min(minY, _board->objPoints[i][j].y);
-            maxY = max(maxY, _board->objPoints[i][j].y);
-        }
-    }
-
-    float sizeX = maxX - minX;
-    float sizeY = maxY - minY;
-
-    // proportion transformations
-    float xReduction = sizeX / float(out.cols);
-    float yReduction = sizeY / float(out.rows);
-
-    // determine the zone where the markers are placed
-    if(xReduction > yReduction) {
-        int nRows = int(sizeY / xReduction);
-        int rowsMargins = (out.rows - nRows) / 2;
-        out.adjustROI(-rowsMargins, -rowsMargins, 0, 0);
-    } else {
-        int nCols = int(sizeX / yReduction);
-        int colsMargins = (out.cols - nCols) / 2;
-        out.adjustROI(0, 0, -colsMargins, -colsMargins);
-    }
-
-    // now paint each marker
-    Dictionary &dictionary = *(_board->dictionary);
-    Mat marker;
-    Point2f outCorners[3];
-    Point2f inCorners[3];
-    for(unsigned int m = 0; m < _board->objPoints.size(); m++) {
-        // transform corners to markerZone coordinates
-        for(int j = 0; j < 3; j++) {
-            Point2f pf = Point2f(_board->objPoints[m][j].x, _board->objPoints[m][j].y);
-            // move top left to 0, 0
-            pf -= Point2f(minX, minY);
-            pf.x = pf.x / sizeX * float(out.cols);
-            pf.y = (1.0f - pf.y / sizeY) * float(out.rows);
-            outCorners[j] = pf;
-        }
-
-        // get marker
-        Size dst_sz(outCorners[2] - outCorners[0]); // assuming CCW order
-        dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
-        dictionary.drawMarker(_board->ids[m], dst_sz.width, marker, borderBits);
-
-        if((outCorners[0].y == outCorners[1].y) && (outCorners[1].x == outCorners[2].x)) {
-            // marker is aligned to image axes
-            marker.copyTo(out(Rect(outCorners[0], dst_sz)));
-            continue;
-        }
-
-        // interpolate tiny marker to marker position in markerZone
-        inCorners[0] = Point2f(-0.5f, -0.5f);
-        inCorners[1] = Point2f(marker.cols - 0.5f, -0.5f);
-        inCorners[2] = Point2f(marker.cols - 0.5f, marker.rows - 0.5f);
-
-        // remove perspective
-        Mat transformation = getAffineTransform(inCorners, outCorners);
-        warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
-                        BORDER_TRANSPARENT);
-    }
-}

aruco_pose/src/genmap.py

@@ -0,0 +1,58 @@
+#!/usr/bin/env python
+
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Oleg Kalachev <okalachev@gmail.com>
+#
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+
+"""Markers map generator
+
+Generate map file for aruco_map nodelet.
+
+Usage:
+  genmap.py <length> <x> <y> <dist_x> <dist_y> [<first>] [--top-left | --bottom-left]
+  genmap.py (-h | --help)
+
+Options:
+  <length>       Marker side length
+  <x>            Marker count along X axis
+  <y>            Marker count along Y axis
+  <dist_x>       Distance between markers along X axis
+  <dist_y>       Distance between markers along Y axis
+  <first>        First marker ID [default: 0]
+  --top-left     First marker is on top-left (default)
+  --bottom-left  First marker is on bottom-left
+
+Example:
+  rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 > $(catkin_find aruco_pose map)/test_map.txt
+"""
+
+from __future__ import print_function
+
+from docopt import docopt
+
+
+arguments = docopt(__doc__)
+
+length = float(arguments['<length>'])
+first = int(arguments['<first>'] if arguments['<first>'] is not None else 0)
+markers_x = int(arguments['<x>'])
+markers_y = int(arguments['<y>'])
+dist_x = float(arguments['<dist_x>'])
+dist_y = float(arguments['<dist_y>'])
+bottom_left = arguments['--bottom-left']
+
+max_y = (markers_y - 1) * dist_y
+
+print('# id\tlength\tx\ty\tz\trot_z\trot_y\trot_x')
+for y in range(markers_y):
+    for x in range(markers_x):
+        pos_x = x * dist_x
+        pos_y = y * dist_y
+        if not bottom_left:
+            pos_y = max_y - pos_y
+        print('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\t'.format(first, length, pos_x, pos_y, 0, 0, 0, 0))
+        first += 1

aruco_pose/src/util.h

@@ -1,20 +0,0 @@
-#pragma once
-
-#include <vector>
-#include <string>
-
-std::vector<std::string> strSplit(const std::string& str, const std::string& delim)
-{
-    std::vector<std::string> tokens;
-    size_t prev = 0, pos = 0;
-    do
-    {
-        pos = str.find(delim, prev);
-        if (pos == std::string::npos) pos = str.length();
-        std::string token = str.substr(prev, pos-prev);
-        if (!token.empty()) tokens.push_back(token);
-        prev = pos + delim.length();
-    }
-    while (pos < str.length() && prev < str.length());
-    return tokens;
-}

aruco_pose/src/utils.h

@@ -0,0 +1,137 @@
+/*
+ * Utility functions
+ * Copyright (C) 2018 Copter Express Technologies
+ *
+ * Author: Oleg Kalachev <okalachev@gmail.com>
+ *
+ * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ */
+
+#pragma once
+
+#include <cmath>
+#include <ros/ros.h>
+#include <tf/transform_datatypes.h>
+#include <geometry_msgs/Quaternion.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/Vector3.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <opencv2/opencv.hpp>
+
+// Read required param or shutdown the node
+template<typename T>
+static void param(ros::NodeHandle nh, const std::string& param_name, T& param_val)
+{
+	if (!nh.getParam(param_name, param_val)) {
+		ROS_FATAL("Required param %s is not defined", param_name.c_str());
+		ros::shutdown();
+	}
+}
+
+static void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr& cinfo, cv::Mat& matrix, cv::Mat& dist)
+{
+	for (unsigned int i = 0; i < 3; ++i)
+		for (unsigned int j = 0; j < 3; ++j)
+			matrix.at<double>(i, j) = cinfo->K[3 * i + j];
+	dist = cv::Mat(cinfo->D, true);
+}
+
+inline void rotatePoint(cv::Point3f& p, cv::Point3f origin, float angle)
+{
+	float s = sin(angle);
+	float c = cos(angle);
+
+	// translate point back to origin:
+	p.x -= origin.x;
+	p.y -= origin.y;
+
+	// rotate point
+	float xnew = p.x * c - p.y * s;
+	float ynew = p.x * s + p.y * c;
+
+	// translate point back:
+	p.x = xnew + origin.x;
+	p.y = ynew + origin.y;
+}
+
+inline void fillPose(geometry_msgs::Pose& pose, const cv::Vec3d& rvec, const cv::Vec3d& tvec)
+{
+	pose.position.x = tvec[0];
+	pose.position.y = tvec[1];
+	pose.position.z = tvec[2];
+
+	double angle = norm(rvec);
+	cv::Vec3d axis = rvec / angle;
+
+	tf2::Quaternion q;
+	q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle);
+
+	pose.orientation.w = q.w();
+	pose.orientation.x = q.x();
+	pose.orientation.y = q.y();
+	pose.orientation.z = q.z();
+}
+
+inline void fillTransform(geometry_msgs::Transform& transform, const cv::Vec3d& rvec, const cv::Vec3d& tvec)
+{
+	transform.translation.x = tvec[0];
+	transform.translation.y = tvec[1];
+	transform.translation.z = tvec[2];
+
+	double angle = norm(rvec);
+	cv::Vec3d axis = rvec / angle;
+
+	tf2::Quaternion q;
+	q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle);
+
+	transform.rotation.w = q.w();
+	transform.rotation.x = q.x();
+	transform.rotation.y = q.y();
+	transform.rotation.z = q.z();
+}
+
+inline void fillTranslation(geometry_msgs::Vector3& translation, const cv::Vec3d& tvec)
+{
+	translation.x = tvec[0];
+	translation.y = tvec[1];
+	translation.z = tvec[2];
+}
+
+inline bool isFlipped(tf::Quaternion& q)
+{
+	double yaw, pitch, roll;
+	tf::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
+	return (abs(pitch) > M_PI / 2) || (abs(roll) > M_PI / 2);
+}
+
+/* Set roll and pitch from "from" to "to", keeping yaw */
+inline void snapOrientation(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from, bool auto_flip = false)
+{
+	tf::Quaternion _from, _to;
+	tf::quaternionMsgToTF(from, _from);
+	tf::quaternionMsgToTF(to, _to);
+
+	if (auto_flip) {
+		if (!isFlipped(_from)) {
+			static const tf::Quaternion flip = tf::createQuaternionFromRPY(M_PI, 0, 0);
+			_from *= flip; // flip "from"
+		}
+	}
+
+	auto diff = tf::Matrix3x3(_to).transposeTimes(tf::Matrix3x3(_from));
+	double _, yaw;
+	diff.getRPY(_, _, yaw);
+	auto q = tf::createQuaternionFromRPY(0, 0, -yaw);
+	_from = _from * q; // set yaw from "to" to "from"
+	tf::quaternionTFToMsg(_from, to); // set "from" to "to"
+}
+
+inline void transformToPose(const geometry_msgs::Transform& transform, geometry_msgs::Pose& pose)
+{
+	pose.position.x = transform.translation.x;
+	pose.position.y = transform.translation.y;
+	pose.position.z = transform.translation.z;
+	pose.orientation = transform.rotation;
+}

aruco_pose/test/basic.py

@@ -0,0 +1,204 @@
+import rospy
+import pytest
+
+import tf2_ros
+import tf2_geometry_msgs
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from sensor_msgs.msg import Image
+from aruco_pose.msg import MarkerArray
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+@pytest.fixture
+def node():
+    return rospy.init_node('aruco_pose_test', anonymous=True)
+
+@pytest.fixture
+def tf_buffer():
+    buf = tf2_ros.Buffer()
+    tf2_ros.TransformListener(buf)
+    return buf
+
+def approx(expected):
+    return pytest.approx(expected, abs=1e-4) # compare floats more roughly
+
+def test_markers(node):
+    markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=5)
+    assert len(markers.markers) == 5
+    assert markers.header.frame_id == 'main_camera_optical'
+
+    assert markers.markers[0].id == 2
+    assert markers.markers[0].length == approx(0.33)
+    assert markers.markers[0].pose.position.x == approx(0.36706567854)
+    assert markers.markers[0].pose.position.y == approx(0.290484516644)
+    assert markers.markers[0].pose.position.z == approx(2.18787602301)
+    assert markers.markers[0].pose.orientation.x == approx(0.993997406299)
+    assert markers.markers[0].pose.orientation.y == approx(-0.00532003481626)
+    assert markers.markers[0].pose.orientation.z == approx(-0.107390951553)
+    assert markers.markers[0].pose.orientation.w == approx(0.0201999263402)
+    assert markers.markers[0].c1.x == approx(415.557739258)
+    assert markers.markers[0].c1.y == approx(335.557739258)
+    assert markers.markers[0].c2.x == approx(509.442260742)
+    assert markers.markers[0].c2.y == approx(335.557739258)
+    assert markers.markers[0].c3.x == approx(509.442260742)
+    assert markers.markers[0].c3.y == approx(429.442260742)
+    assert markers.markers[0].c4.x == approx(415.557739258)
+    assert markers.markers[0].c4.y == approx(429.442260742)
+
+    assert markers.markers[4].id == 3
+    assert markers.markers[4].length == approx(0.1)
+    assert markers.markers[4].pose.position.x == approx(-0.1805169666)
+    assert markers.markers[4].pose.position.y == approx(-0.200697302327)
+    assert markers.markers[4].pose.position.z == approx(0.585767514823)
+    assert markers.markers[4].pose.orientation.x == approx(-0.961738074009)
+    assert markers.markers[4].pose.orientation.y == approx(-0.0375180244707)
+    assert markers.markers[4].pose.orientation.z == approx(-0.0115387773672)
+    assert markers.markers[4].pose.orientation.w == approx(0.271144115664)
+    assert markers.markers[4].c1.x == approx(129.557723999)
+    assert markers.markers[4].c1.y == approx(49.557723999)
+    assert markers.markers[4].c2.x == approx(223.442276001)
+    assert markers.markers[4].c2.y == approx(49.557723999)
+    assert markers.markers[4].c3.x == approx(223.442276001)
+    assert markers.markers[4].c3.y == approx(143.442276001)
+    assert markers.markers[4].c4.x == approx(129.557723999)
+    assert markers.markers[4].c4.y == approx(143.442276001)
+
+    assert markers.markers[1].id == 1
+    assert markers.markers[1].length == approx(0.33)
+    assert markers.markers[3].id == 4
+    assert markers.markers[3].length == approx(0.33)
+
+    assert markers.markers[2].id == 100
+    assert markers.markers[2].length == approx(0.33)
+    assert markers.markers[2].pose.position.x == approx(-1.37600105389)
+    assert markers.markers[2].pose.position.y == approx(-0.323028530991)
+    assert markers.markers[2].pose.position.z == approx(2.94611272668)
+    assert markers.markers[2].pose.orientation.x == approx(-0.955543925678)
+    assert markers.markers[2].pose.orientation.y == approx(0.0458801909197)
+    assert markers.markers[2].pose.orientation.z == approx(-0.249604946264)
+    assert markers.markers[2].pose.orientation.w == approx(-0.150093920537)
+    assert markers.markers[2].c1.x == approx(52.557723999)
+    assert markers.markers[2].c1.y == approx(205.557723999)
+    assert markers.markers[2].c2.x == approx(113.442276001)
+    assert markers.markers[2].c2.y == approx(205.557723999)
+    assert markers.markers[2].c3.x == approx(113.442276001)
+    assert markers.markers[2].c3.y == approx(265.442260742)
+    assert markers.markers[2].c4.x == approx(52.557723999)
+    assert markers.markers[2].c4.y == approx(265.442260742)
+
+def test_markers_frames(node, tf_buffer):
+    marker_2 = tf_buffer.lookup_transform('main_camera_optical', 'aruco_2', rospy.Time(), rospy.Duration(5))
+    assert marker_2.transform.translation.x == approx(0.36706567854)
+    assert marker_2.transform.translation.y == approx(0.290484516644)
+    assert marker_2.transform.translation.z == approx(2.18787602301)
+    assert marker_2.transform.rotation.x == approx(0.993997406299)
+    assert marker_2.transform.rotation.y == approx(-0.00532003481626)
+    assert marker_2.transform.rotation.z == approx(-0.107390951553)
+    assert marker_2.transform.rotation.w == approx(0.0201999263402)
+
+def test_map_markers_frames(node, tf_buffer):
+    stamp = rospy.get_rostime()
+    timeout = rospy.Duration(5)
+
+    marker_1 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_1', stamp, timeout)
+    assert marker_1.transform.translation.x == approx(0)
+    assert marker_1.transform.translation.y == approx(0)
+    assert marker_1.transform.translation.z == approx(0)
+
+    marker_4 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_4', stamp, timeout)
+    assert marker_4.transform.translation.x == approx(1)
+    assert marker_4.transform.translation.y == approx(1)
+    assert marker_4.transform.translation.z == approx(0)
+
+    marker_12 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_12', stamp, timeout)
+    assert marker_12.transform.translation.x == approx(0.2)
+    assert marker_12.transform.translation.y == approx(0.5)
+    assert marker_12.transform.translation.z == approx(0)
+
+def test_visualization(node):
+    vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)
+    assert len(vis.markers) == 11
+
+def test_debug(node):
+    img = rospy.wait_for_message('aruco_detect/debug', Image, timeout=5)
+    assert img.width == 640
+    assert img.height == 480
+    assert img.header.frame_id == 'main_camera_optical'
+
+def test_map(node):
+    pose = rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=5)
+    assert pose.header.frame_id == 'main_camera_optical'
+    assert pose.pose.pose.position.x == approx(-0.629167753342)
+    assert pose.pose.pose.position.y == approx(0.293822650809)
+    assert pose.pose.pose.position.z == approx(2.12641343155)
+    assert pose.pose.pose.orientation.x == approx(-0.998383794799)
+    assert pose.pose.pose.orientation.y == approx(-5.20919098575e-06)
+    assert pose.pose.pose.orientation.z == approx(-0.0300861070302)
+    assert pose.pose.pose.orientation.w == approx(0.0482143590507)
+
+def test_map_image(node):
+    img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
+    assert img.width == 2000
+    assert img.height == 2000
+    assert img.encoding in ('mono8', 'rgb8')
+
+def test_map_markers(node):
+    markers = rospy.wait_for_message('aruco_map/markers', MarkerArray, timeout=5)
+    assert markers.markers[0].id == 1
+    assert markers.markers[1].id == 2
+    assert markers.markers[2].id == 3
+    assert markers.markers[3].id == 4
+    assert markers.markers[4].id == 10
+    assert markers.markers[5].id == 11
+    assert markers.markers[6].id == 12
+
+    assert markers.markers[0].pose.position.x == 0
+    assert markers.markers[0].pose.position.y == 0
+    assert markers.markers[0].pose.position.z == 0
+    assert markers.markers[0].pose.orientation.x == 0
+    assert markers.markers[0].pose.orientation.y == 0
+    assert markers.markers[0].pose.orientation.z == 0
+    assert markers.markers[0].pose.orientation.w == 1
+    assert markers.markers[0].length == approx(0.33)
+
+    assert markers.markers[1].pose.position.x == 1
+    assert markers.markers[1].pose.position.y == 0
+    assert markers.markers[1].pose.position.z == 0
+    assert markers.markers[1].pose.orientation.x == 0
+    assert markers.markers[1].pose.orientation.y == 0
+    assert markers.markers[1].pose.orientation.z == 0
+    assert markers.markers[1].pose.orientation.w == 1
+    assert markers.markers[1].length == approx(0.33)
+
+    assert markers.markers[2].pose.position.x == 0
+    assert markers.markers[2].pose.position.y == 1
+    assert markers.markers[2].pose.position.z == 0
+    assert markers.markers[2].pose.orientation.x == 0
+    assert markers.markers[2].pose.orientation.y == 0
+    assert markers.markers[2].pose.orientation.z == 0
+    assert markers.markers[2].pose.orientation.w == 1
+    assert markers.markers[2].length == approx(0.33)
+
+    assert markers.markers[3].pose.position.x == 1
+    assert markers.markers[3].pose.position.y == 1
+    assert markers.markers[3].pose.position.z == 0
+    assert markers.markers[3].pose.orientation.x == 0
+    assert markers.markers[3].pose.orientation.y == 0
+    assert markers.markers[3].pose.orientation.z == 0
+    assert markers.markers[3].pose.orientation.w == 1
+    assert markers.markers[3].length == approx(0.33)
+
+    assert markers.markers[4].pose.position.x == approx(0.5)
+    assert markers.markers[4].pose.position.y == 2
+    assert markers.markers[4].pose.position.z == 0
+    assert markers.markers[4].pose.orientation.x == 0
+    assert markers.markers[4].pose.orientation.y == 0
+    assert markers.markers[4].pose.orientation.z == approx(0.5646424733950354)
+    assert markers.markers[4].pose.orientation.w == approx(0.8253356149096783)
+    assert markers.markers[4].length == approx(0.5)
+
+def test_map_visualization(node):
+    vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+
+def test_map_debug(node):
+    img = rospy.wait_for_message('aruco_map/debug', Image, timeout=5)

aruco_pose/test/basic.test

@@ -0,0 +1,32 @@
+<launch>
+    <node pkg="image_publisher" type="image_publisher" name="main_camera" args="$(find aruco_pose)/test/map.png">
+        <param name="frame_id" value="main_camera_optical"/>
+        <param name="publish_rate" value="10"/>
+        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
+    </node>
+
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" required="true"/>
+
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect" args="load aruco_pose/aruco_detect nodelet_manager" required="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <param name="length" value="0.33"/>
+        <param name="length_override/3" value="0.1"/>
+        <param name="estimate_poses" value="true"/>
+        <param name="send_tf" value="true"/>
+        <param name="cornerRefinementMethod" value="1"/>
+    </node>
+
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true" required="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="type" value="map"/>
+        <param name="map" value="$(find aruco_pose)/test/basic.txt"/>
+        <param name="markers/frame_id" value="aruco_map"/>
+        <param name="markers/child_frame_id_prefix" value="aruco_in_map_"/>
+    </node>
+
+    <param name="test_module" value="$(find aruco_pose)/test/basic.py"/>
+    <test test-name="aruco_pose_test" pkg="ros_pytest" type="ros_pytest_runner"/>
+</launch>

aruco_pose/test/basic.txt

@@ -0,0 +1,11 @@
+# Default markers
+1	0.33	0	0	0	0	0	0
+2	0.33	1	0	0	0	0	0
+3	0.33	0	1	0	0	0	0
+4	0.33	1	1	0	0	0	0
+# Marker with non-zero yaw rotation
+10	0.5	0.5	2	0	1.2	0	0
+# Marker with non-zero pitch and roll rotation
+11	0.2	0.5	0.5	0	0.0	-1	1
+# Marker with yaw, pitch and roll rotation
+12	0.4	0.2	0.5	0	0.1	-1.2	-0.5

aruco_pose/test/camera_info.yaml

@@ -0,0 +1,21 @@
+# some random camera calibration for testing
+image_width: 640
+image_height: 480
+camera_name: test_camera
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [643.229809, 0.000000, 356.811289, 0.000000, 644.318982, 299.150067, 0.000000, 0.000000, 1.000000]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.422907, 0.202567, 0.000781, 0.000447, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [567.010193, 0.000000, 366.677428, 0.000000, 0.000000, 594.591980, 307.043423, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

aruco_pose/test/crash_opencv.py

@@ -0,0 +1,18 @@
+import rospy
+import pytest
+
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+@pytest.fixture
+def node():
+    return rospy.init_node('aruco_pose_opencv_crash', anonymous=True)
+
+def test_opencv_crashes_img01(node):
+    rospy.wait_for_message('aruco_detect_01/visualization', VisMarkerArray, timeout=5)
+
+def test_opencv_crashes_img02(node):
+    rospy.wait_for_message('aruco_detect_02/visualization', VisMarkerArray, timeout=5)
+
+def test_opencv_crashes_img03(node):
+    rospy.wait_for_message('aruco_detect_03/visualization', VisMarkerArray, timeout=5)

aruco_pose/test/crash_opencv.test

@@ -0,0 +1,51 @@
+<launch>
+    <arg name="corner_method" default="2"/>
+
+    <node pkg="image_publisher" type="image_publisher" name="imgpub_01" args="$(find aruco_pose)/test/crash_image_01.png">
+        <param name="frame_id" value="main_camera_optical"/>
+        <param name="publish_rate" value="10"/>
+        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
+    </node>
+
+    <node pkg="image_publisher" type="image_publisher" name="imgpub_02" args="$(find aruco_pose)/test/crash_image_02.png">
+        <param name="frame_id" value="main_camera_optical"/>
+        <param name="publish_rate" value="10"/>
+        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
+    </node>
+
+    <node pkg="image_publisher" type="image_publisher" name="imgpub_03" args="$(find aruco_pose)/test/crash_image_03.png">
+        <param name="frame_id" value="main_camera_optical"/>
+        <param name="publish_rate" value="10"/>
+        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
+    </node>
+
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager_01" args="manager"/>
+
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect_01" args="load aruco_pose/aruco_detect nodelet_manager_01">
+        <remap from="image_raw" to="imgpub_01/image_raw"/>
+        <remap from="camera_info" to="imgpub_01/camera_info"/>
+        <param name="length" value="0.33"/>
+        <param name="cornerRefinementMethod" value="$(arg corner_method)"/>
+    </node>
+
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager_02" args="manager"/>
+
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect_02" args="load aruco_pose/aruco_detect nodelet_manager_02">
+        <remap from="image_raw" to="imgpub_02/image_raw"/>
+        <remap from="camera_info" to="imgpub_02/camera_info"/>
+        <param name="length" value="0.33"/>
+        <param name="cornerRefinementMethod" value="$(arg corner_method)"/>
+    </node>
+
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager_03" args="manager"/>
+
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect_03" args="load aruco_pose/aruco_detect nodelet_manager_03">
+        <remap from="image_raw" to="imgpub_03/image_raw"/>
+        <remap from="camera_info" to="imgpub_03/camera_info"/>
+        <param name="length" value="0.33"/>
+        <param name="cornerRefinementMethod" value="$(arg corner_method)"/>
+    </node>
+
+    <param name="test_module" value="$(find aruco_pose)/test/crash_opencv.py"/>
+    <test test-name="crash_opencv" pkg="ros_pytest" type="ros_pytest_runner"/>
+</launch>

aruco_pose/test/largemap.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+import sys
+import unittest
+import json
+import rospy
+import rostest
+
+from sensor_msgs.msg import Image
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+class TestArucoPose(unittest.TestCase):
+    def setUp(self):
+        rospy.init_node('test_aruco_largemap', anonymous=True)
+
+    def test_map_image(self):
+        img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
+        self.assertEqual(img.width, 2000)
+        self.assertEqual(img.height, 2000)
+        self.assertIn(img.encoding, ('mono8', 'rgb8'))
+
+    def test_map_visualization(self):
+        vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+
+
+rostest.rosrun('aruco_pose', 'test_aruco_largemap', TestArucoPose, sys.argv)

aruco_pose/test/largemap.test

@@ -0,0 +1,13 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" required="true"/>
+
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true" required="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="type" value="map"/>
+        <param name="map" value="$(find aruco_pose)/test/largemap.txt"/>
+    </node>
+
+    <test test-name="test_aruco_pose_largemap" pkg="aruco_pose" type="largemap.py"/>
+</launch>

aruco_pose/test/largemap.txt

@@ -0,0 +1,11 @@
+0 0.2 0 0 0 0 0 0
+1 0.2 10 0 0 0 0 0
+2 0.2 20 0 0 0 0 0
+3 0.2 30 0 0 0 0 0
+4 0.2 40 0 0 0 0 0
+5 0.2 50 0 0 0 0 0
+6 0.2 60 0 0 0 0 0
+7 0.2 70 0 0 0 0 0
+8 0.2 80 0 0 0 0 0
+9 0.2 90 0 0 0 0 0
+10 0.2 100 0 0 0 0 0

aruco_pose/test/test_node_failure.py

@@ -0,0 +1,13 @@
+import rospy
+import pytest
+
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+@pytest.fixture
+def node():
+    return rospy.init_node('aruco_pose_test', anonymous=True)
+
+def test_node_failure(node):
+    with pytest.raises(rospy.exceptions.ROSException):
+        rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)

aruco_pose/test/test_node_failure.test

@@ -0,0 +1,14 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
+
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="type" value="map"/>
+        <param name="map" value="$(find aruco_pose)/test/test_node_failure.txt"/>
+    </node>
+
+    <param name="test_module" value="$(find aruco_pose)/test/test_node_failure.py"/>
+    <test test-name="test_node_failure" pkg="ros_pytest" type="ros_pytest_runner"/>
+</launch>