diff --git a/.clang-format b/.clang-format
new file mode 100644
index 00000000..8b3d0c00
--- /dev/null
+++ b/.clang-format
@@ -0,0 +1,68 @@
+---
+BasedOnStyle: Google
+AccessModifierOffset: -2
+AlignEscapedNewlinesLeft: false
+AlignTrailingComments: true
+AlignAfterOpenBracket: Align
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortFunctionsOnASingleLine: true
+AllowShortIfStatementsOnASingleLine: false
+AllowShortLoopsOnASingleLine: false
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakBeforeMultilineStrings: false
+AlwaysBreakTemplateDeclarations: true
+BinPackArguments: false
+BinPackParameters: false
+BreakBeforeBinaryOperators: false
+BreakBeforeTernaryOperators: false
+BreakConstructorInitializersBeforeComma: true
+ColumnLimit: 120
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+ConstructorInitializerIndentWidth: 2
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: false
+DerivePointerBinding: false
+ExperimentalAutoDetectBinPacking: false
+IndentCaseLabels: true
+IndentFunctionDeclarationAfterType: false
+IndentWidth: 2
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakBeforeFirstCallParameter: 19
+PenaltyBreakComment: 60
+PenaltyBreakFirstLessLess: 1000
+PenaltyBreakString: 1
+PenaltyExcessCharacter: 1000
+PenaltyReturnTypeOnItsOwnLine: 90
+PointerBindsToType: true
+SortIncludes: false
+SpaceAfterControlStatementKeyword: true
+SpaceAfterCStyleCast: false
+SpaceBeforeAssignmentOperators: true
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 2
+SpacesInAngles: false
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+Standard: Auto
+TabWidth: 2
+UseTab: Never
+
+# Configure each individual brace in BraceWrapping
+BreakBeforeBraces: Custom
+
+# Control of individual brace wrapping cases
+BraceWrapping: {
+    AfterClass: 'true'
+    AfterControlStatement: 'true'
+    AfterEnum : 'true'
+    AfterFunction : 'true'
+    AfterNamespace : 'true'
+    AfterStruct : 'true'
+    AfterUnion : 'true'
+    BeforeCatch : 'true'
+    BeforeElse : 'true'
+    IndentBraces : 'false'
+}
+...
diff --git a/.cmake-format b/.cmake-format
new file mode 100644
index 00000000..ab202667
--- /dev/null
+++ b/.cmake-format
@@ -0,0 +1,334 @@
+#!/bin/python
+# ----------------------------------
+# Options affecting listfile parsing
+# ----------------------------------
+with section("parse"):
+
+  # Specify structure for custom cmake functions
+  additional_commands = {
+    'target_clang_tidy': {
+      'pargs': {'nargs': 1},
+      'kwargs': {
+        'ARGUMENTS': '*',
+        'ENABLE': 1,
+      }
+    },
+    'target_include_what_you_use': {
+      'pargs': {'nargs': 1},
+      'kwargs': {
+        'ARGUMENTS': '*',
+        'ENABLE': 1,
+      }
+    },
+    'include_what_you_use': {
+      'pargs': {'nargs': 0},
+      'kwargs': {
+        'ARGUMENTS': '*',
+        'ENABLE': 1,
+      }
+    },
+    'target_cppcheck': {
+      'pargs': {'nargs': 1},
+      'kwargs': {
+        'ARGUMENTS': '*',
+        'ENABLE': 1,
+      }
+    },
+    'cppcheck': {
+      'pargs': {'nargs': 0},
+      'kwargs': {
+        'ARGUMENTS': '*',
+        'ENABLE': 1,
+      }
+    },
+    'configure_package': {
+      'pargs': {'nargs': 0},
+      'kwargs': {
+        'NAMESPACE': '?',
+        'TARGETS': '*',
+      }
+    },
+    'add_gtest_discover_tests': {
+      'pargs': {'nargs': 1},
+    },
+    'add_run_tests_target': {
+      'pargs': {'nargs': 0},
+      'kwargs': {
+        'ENABLE': 1,
+      }
+    },
+    'add_run_benchmark_target': {
+      'pargs': {'nargs': 0},
+      'kwargs': {
+        'ENABLE': 1,
+      }
+    },
+    'target_cxx_version': {
+      'pargs': {'nargs': 1},
+      'kwargs': {
+        'INTERFACE': '+',
+        'PUBLIC': '+',
+        'PRIVATE': '+',
+        'VERSION': 1,
+      }
+    },
+    'target_code_coverage': {
+      'pargs': {'nargs': 1},
+      'kwargs': {
+        'AUTO': '+',
+        'ALL': '+',
+        'EXTERNAL': '+',
+        'PRIVATE': '+',
+        'PUBLIC': '+',
+        'INTERFACE': '+',
+        'ENABLE': 1,
+        'EXCLUDE': '*',
+      }
+    },
+    'add_code_coverage': {
+      'pargs': {'nargs': 0},
+      'kwargs': {
+        'ENABLE': 1,
+      }
+    },
+    'add_code_coverage_all_targets': {
+      'pargs': {'nargs': 0},
+      'kwargs': {
+        'ENABLE': 1,
+        'EXCLUDE': '*',
+      }
+    },
+  }
+
+  # Override configurations per-command where available
+  override_spec = {}
+
+  # Specify variable tags.
+  vartags = []
+
+  # Specify property tags.
+  proptags = []
+
+# -----------------------------
+# Options affecting formatting.
+# -----------------------------
+with section("format"):
+
+  # Disable formatting entirely, making cmake-format a no-op
+  disable = False
+
+  # How wide to allow formatted cmake files
+  line_width = 120
+
+  # How many spaces to tab for indent
+  tab_size = 2
+
+  # If true, lines are indented using tab characters (utf-8 0x09) instead of
+  # <tab_size> space characters (utf-8 0x20). In cases where the layout would
+  # require a fractional tab character, the behavior of the  fractional
+  # indentation is governed by <fractional_tab_policy>
+  use_tabchars = False
+
+  # If <use_tabchars> is True, then the value of this variable indicates how
+  # fractional indentions are handled during whitespace replacement. If set to
+  # 'use-space', fractional indentation is left as spaces (utf-8 0x20). If set
+  # to `round-up` fractional indentation is replaced with a single tab character
+  # (utf-8 0x09) effectively shifting the column to the next tabstop
+  fractional_tab_policy = 'use-space'
+
+  # If an argument group contains more than this many sub-groups (parg or kwarg
+  # groups) then force it to a vertical layout.
+  max_subgroups_hwrap = 3
+
+  # If a positional argument group contains more than this many arguments, then
+  # force it to a vertical layout.
+  max_pargs_hwrap = 3
+
+  # If a cmdline positional group consumes more than this many lines without
+  # nesting, then invalidate the layout (and nest)
+  max_rows_cmdline = 2
+
+  # If true, separate flow control names from their parentheses with a space
+  separate_ctrl_name_with_space = False
+
+  # If true, separate function names from parentheses with a space
+  separate_fn_name_with_space = False
+
+  # If a statement is wrapped to more than one line, than dangle the closing
+  # parenthesis on its own line.
+  dangle_parens = False
+
+  # If the trailing parenthesis must be 'dangled' on its on line, then align it
+  # to this reference: `prefix`: the start of the statement,  `prefix-indent`:
+  # the start of the statement, plus one indentation  level, `child`: align to
+  # the column of the arguments
+  dangle_align = 'prefix'
+
+  # If the statement spelling length (including space and parenthesis) is
+  # smaller than this amount, then force reject nested layouts.
+  min_prefix_chars = 4
+
+  # If the statement spelling length (including space and parenthesis) is larger
+  # than the tab width by more than this amount, then force reject un-nested
+  # layouts.
+  max_prefix_chars = 10
+
+  # If a candidate layout is wrapped horizontally but it exceeds this many
+  # lines, then reject the layout.
+  max_lines_hwrap = 2
+
+  # What style line endings to use in the output.
+  line_ending = 'unix'
+
+  # Format command names consistently as 'lower' or 'upper' case
+  command_case = 'canonical'
+
+  # Format keywords consistently as 'lower' or 'upper' case
+  keyword_case = 'unchanged'
+
+  # A list of command names which should always be wrapped
+  always_wrap = []
+
+  # If true, the argument lists which are known to be sortable will be sorted
+  # lexicographicall
+  enable_sort = True
+
+  # If true, the parsers may infer whether or not an argument list is sortable
+  # (without annotation).
+  autosort = False
+
+  # By default, if cmake-format cannot successfully fit everything into the
+  # desired linewidth it will apply the last, most agressive attempt that it
+  # made. If this flag is True, however, cmake-format will print error, exit
+  # with non-zero status code, and write-out nothing
+  require_valid_layout = False
+
+  # A dictionary mapping layout nodes to a list of wrap decisions. See the
+  # documentation for more information.
+  layout_passes = {}
+
+# ------------------------------------------------
+# Options affecting comment reflow and formatting.
+# ------------------------------------------------
+with section("markup"):
+
+  # What character to use for bulleted lists
+  bullet_char = '*'
+
+  # What character to use as punctuation after numerals in an enumerated list
+  enum_char = '.'
+
+  # If comment markup is enabled, don't reflow the first comment block in each
+  # listfile. Use this to preserve formatting of your copyright/license
+  # statements.
+  first_comment_is_literal = False
+
+  # If comment markup is enabled, don't reflow any comment block which matches
+  # this (regex) pattern. Default is `None` (disabled).
+  literal_comment_pattern = None
+
+  # Regular expression to match preformat fences in comments default=
+  # ``r'^\s*([`~]{3}[`~]*)(.*)$'``
+  fence_pattern = '^\\s*([`~]{3}[`~]*)(.*)$'
+
+  # Regular expression to match rulers in comments default=
+  # ``r'^\s*[^\w\s]{3}.*[^\w\s]{3}$'``
+  ruler_pattern = '^\\s*[^\\w\\s]{3}.*[^\\w\\s]{3}$'
+
+  # If a comment line matches starts with this pattern then it is explicitly a
+  # trailing comment for the preceeding argument. Default is '#<'
+  explicit_trailing_pattern = '#<'
+
+  # If a comment line starts with at least this many consecutive hash
+  # characters, then don't lstrip() them off. This allows for lazy hash rulers
+  # where the first hash char is not separated by space
+  hashruler_min_length = 10
+
+  # If true, then insert a space between the first hash char and remaining hash
+  # chars in a hash ruler, and normalize its length to fill the column
+  canonicalize_hashrulers = True
+
+  # enable comment markup parsing and reflow
+  enable_markup = True
+
+# ----------------------------
+# Options affecting the linter
+# ----------------------------
+with section("lint"):
+
+  # a list of lint codes to disable
+  disabled_codes = []
+
+  # regular expression pattern describing valid function names
+  function_pattern = '[0-9a-z_]+'
+
+  # regular expression pattern describing valid macro names
+  macro_pattern = '[0-9A-Z_]+'
+
+  # regular expression pattern describing valid names for variables with global
+  # (cache) scope
+  global_var_pattern = '[A-Z][0-9A-Z_]+'
+
+  # regular expression pattern describing valid names for variables with global
+  # scope (but internal semantic)
+  internal_var_pattern = '_[A-Z][0-9A-Z_]+'
+
+  # regular expression pattern describing valid names for variables with local
+  # scope
+  local_var_pattern = '[a-z][a-z0-9_]+'
+
+  # regular expression pattern describing valid names for privatedirectory
+  # variables
+  private_var_pattern = '_[0-9a-z_]+'
+
+  # regular expression pattern describing valid names for public directory
+  # variables
+  public_var_pattern = '[A-Z][0-9A-Z_]+'
+
+  # regular expression pattern describing valid names for function/macro
+  # arguments and loop variables.
+  argument_var_pattern = '[a-z][a-z0-9_]+'
+
+  # regular expression pattern describing valid names for keywords used in
+  # functions or macros
+  keyword_pattern = '[A-Z][0-9A-Z_]+'
+
+  # In the heuristic for C0201, how many conditionals to match within a loop in
+  # before considering the loop a parser.
+  max_conditionals_custom_parser = 2
+
+  # Require at least this many newlines between statements
+  min_statement_spacing = 1
+
+  # Require no more than this many newlines between statements
+  max_statement_spacing = 2
+  max_returns = 6
+  max_branches = 12
+  max_arguments = 5
+  max_localvars = 15
+  max_statements = 50
+
+# -------------------------------
+# Options affecting file encoding
+# -------------------------------
+with section("encode"):
+
+  # If true, emit the unicode byte-order mark (BOM) at the start of the file
+  emit_byteorder_mark = False
+
+  # Specify the encoding of the input file. Defaults to utf-8
+  input_encoding = 'utf-8'
+
+  # Specify the encoding of the output file. Defaults to utf-8. Note that cmake
+  # only claims to support utf-8 so be careful when using anything else
+  output_encoding = 'utf-8'
+
+# -------------------------------------
+# Miscellaneous configurations options.
+# -------------------------------------
+with section("misc"):
+
+  # A dictionary containing any per-command configuration overrides. Currently
+  # only `command_case` is supported.
+  per_command = {}
+
diff --git a/.github/workflows/clang_format.yml b/.github/workflows/clang_format.yml
new file mode 100644
index 00000000..ea46111a
--- /dev/null
+++ b/.github/workflows/clang_format.yml
@@ -0,0 +1,50 @@
+name: Clang-Format
+
+on:
+  push:
+    branches:
+      - master
+  pull_request:
+  schedule:
+    - cron: '0 5 * * *'
+
+jobs:
+  industrial_ci:
+    name: Format
+    runs-on: ubuntu-latest
+    env:
+      CI_NAME: Clang-Format
+      OS_NAME: ubuntu
+      OS_CODE_NAME: bionic
+      ROS_DISTRO: melodic
+      ROS_REPO: main
+      CLANG_FORMAT_CHECK: file
+      CLANG_FORMAT_VERSION: 8
+    steps:
+      - uses: actions/checkout@v2
+
+      - name: Free Disk Space
+        run: |
+          sudo swapoff -a
+          sudo rm -f /swapfile
+          sudo apt clean
+          docker rmi $(docker image ls -aq)
+          df -h
+
+      - name: Prepare ccache timestamp
+        id: ccache_cache_timestamp
+        shell: cmake -P {0}
+        run: |
+          string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC)
+          message("::set-output name=timestamp::${current_date}")
+
+      - name: ccache cache files
+        uses: actions/cache@v1.1.0
+        with:
+          path: ${{ env.CI_NAME }}/.ccache
+          key: ${{ env.CI_NAME }}-ccache-${{ steps.ccache_cache_timestamp.outputs.timestamp }}
+          restore-keys: |
+            ${{ env.CI_NAME }}-ccache-
+
+      - uses: 'ros-industrial/industrial_ci@master'
+        env: ${{env}}
diff --git a/.github/workflows/cmake_format.yml b/.github/workflows/cmake_format.yml
new file mode 100644
index 00000000..ca114ef6
--- /dev/null
+++ b/.github/workflows/cmake_format.yml
@@ -0,0 +1,27 @@
+name: CMake-Format
+
+on:
+  push:
+    branches:
+      - master
+  pull_request:
+  schedule:
+    - cron: '0 5 * * *'
+
+jobs:
+  cmake_lang:
+    name: Format
+    runs-on: ubuntu-20.04
+    steps:
+      - uses: actions/checkout@v2
+
+      - name: Run CMake Lang Format Check
+        run: |
+          sudo pip3 install cmakelang
+          RED='\033[0;31m'
+          NC='\033[0m' # No Color
+          ./.run-cmake-format
+          output=$(git diff)
+          if [ -n "$output" ]; then printf "${RED}CMake format error: run script './.run-cmake-formate'${NC}\n"; fi
+          if [ -n "$output" ]; then printf "${RED}${output}${NC}\n"; fi
+          if [ -n "$output" ]; then exit 1; else exit 0; fi
diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml
index 50df4041..65e32efe 100644
--- a/.github/workflows/main.yml
+++ b/.github/workflows/main.yml
@@ -28,16 +28,6 @@ jobs:
              TARGET_CMAKE_ARGS: "-DRCT_BUILD_TESTS=ON",
              CTEST_OUTPUT_ON_FAILURE: true,
              AFTER_SCRIPT: "cd $target_ws/build/rct_optimizations && ctest -V && cd $target_ws/build/rct_image_tools && ctest -V"}
-          - {BADGE: xenial,
-             OS_NAME: ubuntu,
-             OS_CODE_NAME: xenial,
-             ROS_DISTRO: kinetic,
-             ROS_REPO: main,
-             ADDITIONAL_DEBS: git,
-             VERBOSE_TESTS: true,
-             TARGET_CMAKE_ARGS: "-DRCT_BUILD_TESTS=ON",
-             CTEST_OUTPUT_ON_FAILURE: true,
-             AFTER_SCRIPT: 'cd $target_ws/build/rct_optimizations && ctest -V && cd $target_ws/build/rct_image_tools && ctest -V'}
     runs-on: ubuntu-latest
     steps:
       - uses: actions/checkout@v1
diff --git a/.github_clang_format.yml b/.github_clang_format.yml
new file mode 120000
index 00000000..93fa9ccf
--- /dev/null
+++ b/.github_clang_format.yml
@@ -0,0 +1 @@
+.github/workflows/clang_format.yml
\ No newline at end of file
diff --git a/.github_cmake_format.yml b/.github_cmake_format.yml
new file mode 120000
index 00000000..95702a6e
--- /dev/null
+++ b/.github_cmake_format.yml
@@ -0,0 +1 @@
+.github/workflows/cmake_format.yml
\ No newline at end of file
diff --git a/.run-clang-format b/.run-clang-format
new file mode 100755
index 00000000..478405ef
--- /dev/null
+++ b/.run-clang-format
@@ -0,0 +1,2 @@
+#!/bin/bash
+find . -type d \( -name gtest_ros \) -prune -o -type f -regex '.*\.\(cpp\|hpp\|cc\|cxx\|h\|hxx\)' -exec clang-format-8 -style=file -i {} \;
diff --git a/.run-cmake-format b/.run-cmake-format
new file mode 100755
index 00000000..fe66f6d7
--- /dev/null
+++ b/.run-cmake-format
@@ -0,0 +1,2 @@
+#!/bin/bash
+find . \( -name CMakeLists.txt -o -name \*.cmake \) -exec cmake-format -i {} \;
diff --git a/README.md b/README.md
index 57a0dccc..48e41059 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,12 @@
 # Robot Calibration Tools
 ![Static Camera Workcell Setup](rct_examples/docs/static_camera_cell.png)
 
+Platform               | CI Status
+---------------------- |:---------
+Linux (Focal & Bionic) | [![Build Status](https://github.com/Jmeyer1292/robot_cal_tools/workflows/CI/badge.svg)](https://github.com/Jmeyer1292/robot_cal_tools/actions)
+Lint  (Clang-Format)   | [![Build Status](https://github.com/Jmeyer1292/robot_cal_tools/workflows/Clang-Format/badge.svg)](https://github.com/Jmeyer1292/robot_cal_tools/actions)
+Lint  (CMake-Format)   | [![Build Status](https://github.com/Jmeyer1292/robot_cal_tools/workflows/CMake-Format/badge.svg)](https://github.com/Jmeyer1292/robot_cal_tools/actions)
+
 ## Description
 A loosely connected bundle of tools for calibrating cameras to (industrial) robots and vice-versa with a focus on being easy to *integrate*  into more complex applications.
 
diff --git a/rct_common/CMakeLists.txt b/rct_common/CMakeLists.txt
index 649d148e..f4624747 100644
--- a/rct_common/CMakeLists.txt
+++ b/rct_common/CMakeLists.txt
@@ -3,6 +3,17 @@ project(rct_common VERSION 0.1.0)
 
 include(cmake/rct_macros.cmake)
 
+find_package(Eigen3 REQUIRED)
+if(NOT EIGEN3_INCLUDE_DIRS)
+  set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
+endif()
+if(NOT TARGET Eigen3::Eigen)
+    find_package(Threads REQUIRED)
+    add_library(Eigen3::Eigen IMPORTED INTERFACE)
+    set_property(TARGET Eigen3::Eigen PROPERTY INTERFACE_COMPILE_DEFINITIONS ${EIGEN3_DEFINITIONS})
+    set_property(TARGET Eigen3::Eigen PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${EIGEN3_INCLUDE_DIRS})
+endif()
+
 if(RCT_BUILD_TESTS)
   find_package(GTest QUIET)
   if ( NOT GTest_FOUND )
@@ -45,9 +56,18 @@ if(RCT_BUILD_TESTS)
 endif()
 
 add_library(${PROJECT_NAME} INTERFACE)
+target_compile_options(${PROJECT_NAME} INTERFACE -std=c++11)
+target_include_directories(${PROJECT_NAME} INTERFACE
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include>")
+target_link_libraries(${PROJECT_NAME} INTERFACE Eigen3::Eigen)
 
 rct_configure_package(${PROJECT_NAME})
 
+install(DIRECTORY include/${PROJECT_NAME}
+  DESTINATION include
+)
+
 install(FILES
   "${CMAKE_CURRENT_LIST_DIR}/cmake/rct_macros.cmake"
   DESTINATION lib/cmake/${PROJECT_NAME})
diff --git a/rct_common/cmake/rct_common-config.cmake.in b/rct_common/cmake/rct_common-config.cmake.in
index 095dc817..4da87252 100644
--- a/rct_common/cmake/rct_common-config.cmake.in
+++ b/rct_common/cmake/rct_common-config.cmake.in
@@ -3,6 +3,9 @@
 set(@PROJECT_NAME@_FOUND ON)
 set_and_check(@PROJECT_NAME@_LIBRARY_DIRS "${PACKAGE_PREFIX_DIR}/lib")
 
+include(CMakeFindDependencyMacro)
+find_dependency(Eigen3)
+
 include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@-targets.cmake")
 include("${CMAKE_CURRENT_LIST_DIR}/rct_macros.cmake")
 
diff --git a/rct_common/include/rct_common/extrinsic_data_set.h b/rct_common/include/rct_common/extrinsic_data_set.h
new file mode 100644
index 00000000..d8f39908
--- /dev/null
+++ b/rct_common/include/rct_common/extrinsic_data_set.h
@@ -0,0 +1,4 @@
+#ifndef EXTRINSIC_DATA_SET_H
+#define EXTRINSIC_DATA_SET_H
+
+#endif // EXTRINSIC_DATA_SET_H
diff --git a/rct_ros_tools/include/rct_ros_tools/print_utils.h b/rct_common/include/rct_common/print_utils.h
similarity index 97%
rename from rct_ros_tools/include/rct_ros_tools/print_utils.h
rename to rct_common/include/rct_common/print_utils.h
index daa70a57..c269b95d 100644
--- a/rct_ros_tools/include/rct_ros_tools/print_utils.h
+++ b/rct_common/include/rct_common/print_utils.h
@@ -1,11 +1,11 @@
-#ifndef RCT_PRINT_UTILS_H
-#define RCT_PRINT_UTILS_H
+#ifndef RCT_COMMON_PRINT_UTILS_H
+#define RCT_COMMON_PRINT_UTILS_H
+
 #include <Eigen/Dense>
 #include <iostream>
 
-namespace rct_ros_tools
+namespace rct_common
 {
-
 inline
 std::string getStringRPY(const Eigen::Vector3d& rpy)
 {
@@ -139,6 +139,5 @@ void printCameraDistortion(const std::array<double, 5> &values, const std::strin
   std::cout << description << ":" << std::endl;
   std::cout << getStringDistortion(values) << std::endl;
 }
-
 }
-#endif // RCT_PRINT_UTILS_H
+#endif // PRINT_UTILS_H
diff --git a/rct_common/package.xml b/rct_common/package.xml
index 1a60829b..e2dc6dc8 100644
--- a/rct_common/package.xml
+++ b/rct_common/package.xml
@@ -6,6 +6,7 @@
   <maintainer email="joseph.schornak@swri.org">Joseph Schornak</maintainer>
   <author email="joseph.schornak@swri.org">Joseph Schornak</author>
   <license>Apache 2.0</license>
+  <depend>eigen</depend>
   <export>
     <build_type>cmake</build_type>
   </export>
diff --git a/rct_examples/CMakeLists.txt b/rct_examples/CMakeLists.txt
index 3ce4a84e..250b2b15 100644
--- a/rct_examples/CMakeLists.txt
+++ b/rct_examples/CMakeLists.txt
@@ -5,6 +5,7 @@ add_compile_options(-std=c++11 -Wall -Wextra)
 
 find_package(rct_optimizations REQUIRED)
 find_package(rct_image_tools REQUIRED)
+find_package(rct_common REQUIRED)
 
 find_package(catkin REQUIRED COMPONENTS
   rct_ros_tools
@@ -45,7 +46,7 @@ set_target_properties(${PROJECT_NAME}_wrist_example PROPERTIES OUTPUT_NAME examp
 
 add_dependencies(${PROJECT_NAME}_wrist_example ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
-target_link_libraries(${PROJECT_NAME}_wrist_example ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools)
+target_link_libraries(${PROJECT_NAME}_wrist_example ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools rct::rct_common)
 
 
 #Executable for kinematic calibration
@@ -55,7 +56,7 @@ set_target_properties(${PROJECT_NAME}_kinematic_calibration PROPERTIES OUTPUT_NA
 
 add_dependencies(${PROJECT_NAME}_kinematic_calibration ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
-target_link_libraries(${PROJECT_NAME}_kinematic_calibration ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools)
+target_link_libraries(${PROJECT_NAME}_kinematic_calibration ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools rct::rct_common)
 
 ###############################
 ## Offline Calibration Tools ##
@@ -64,19 +65,19 @@ target_link_libraries(${PROJECT_NAME}_kinematic_calibration ${catkin_LIBRARIES}
 add_executable(${PROJECT_NAME}_moving_camera src/tools/camera_on_wrist_extrinsic.cpp)
 set_target_properties(${PROJECT_NAME}_moving_camera PROPERTIES OUTPUT_NAME moving_cam_extr_cal_ex PREFIX "")
 add_dependencies(${PROJECT_NAME}_moving_camera ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(${PROJECT_NAME}_moving_camera ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools)
+target_link_libraries(${PROJECT_NAME}_moving_camera ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools rct::rct_common)
 
 # Executable for demonstrating extrinsic cal of static camera, moving target functionality
 add_executable(${PROJECT_NAME}_static_camera src/tools/static_camera_extrinsic.cpp)
 set_target_properties(${PROJECT_NAME}_static_camera PROPERTIES OUTPUT_NAME static_cam_extr_cal_ex PREFIX "")
 add_dependencies(${PROJECT_NAME}_static_camera ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(${PROJECT_NAME}_static_camera ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools)
+target_link_libraries(${PROJECT_NAME}_static_camera ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools rct::rct_common)
 
 # Executable for demonstrating extrinsic cal of multiple static camera, moving target functionality
 add_executable(${PROJECT_NAME}_multi_static_camera src/tools/multi_static_camera_extrinsic.cpp)
 set_target_properties(${PROJECT_NAME}_multi_static_camera PROPERTIES OUTPUT_NAME multi_static_cam_extr_cal_ex PREFIX "")
 add_dependencies(${PROJECT_NAME}_multi_static_camera ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(${PROJECT_NAME}_multi_static_camera ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools)
+target_link_libraries(${PROJECT_NAME}_multi_static_camera ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools rct::rct_common)
 
 # Executable for demonstrating extrinsic cal of multiple static camera multi step, moving target functionality
 # First it calibrates the cameras to each other then it calibrates the set of camera where there relationship
@@ -84,37 +85,37 @@ target_link_libraries(${PROJECT_NAME}_multi_static_camera ${catkin_LIBRARIES} rc
 add_executable(${PROJECT_NAME}_multi_static_camera_multi_step src/tools/multi_static_camera_multi_step_extrinsic.cpp)
 set_target_properties(${PROJECT_NAME}_multi_static_camera_multi_step PROPERTIES OUTPUT_NAME multi_static_cam_multi_step_extr_cal_ex PREFIX "")
 add_dependencies(${PROJECT_NAME}_multi_static_camera_multi_step ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(${PROJECT_NAME}_multi_static_camera_multi_step ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools)
+target_link_libraries(${PROJECT_NAME}_multi_static_camera_multi_step ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools rct::rct_common)
 
 #Executable for demonstrating intrinsic calibration of a camera
 add_executable(${PROJECT_NAME}_intr src/tools/intrinsic_calibration.cpp)
 set_target_properties(${PROJECT_NAME}_intr PROPERTIES OUTPUT_NAME intr_camera_cal_ex PREFIX "")
 add_dependencies(${PROJECT_NAME}_intr ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(${PROJECT_NAME}_intr ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools)
+target_link_libraries(${PROJECT_NAME}_intr ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools rct::rct_common)
 
 # Executable demonstrating solving for the pose of a target given camera properties
 add_executable(${PROJECT_NAME}_pnp src/tools/solve_pnp.cpp)
 set_target_properties(${PROJECT_NAME}_pnp PROPERTIES OUTPUT_NAME solve_pnp_ex PREFIX "")
 add_dependencies(${PROJECT_NAME}_pnp ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(${PROJECT_NAME}_pnp ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools)
+target_link_libraries(${PROJECT_NAME}_pnp ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools rct::rct_common)
 
 # Executable demonstrating solving for the pose of a target given multiple camera properties
 add_executable(${PROJECT_NAME}_multi_camera_pnp src/tools/solve_multi_camera_pnp.cpp)
 set_target_properties(${PROJECT_NAME}_multi_camera_pnp PROPERTIES OUTPUT_NAME solve_multi_camera_pnp_ex PREFIX "")
 add_dependencies(${PROJECT_NAME}_multi_camera_pnp ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(${PROJECT_NAME}_multi_camera_pnp ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools)
+target_link_libraries(${PROJECT_NAME}_multi_camera_pnp ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools rct::rct_common)
 
 # Executable demonstrating camera intrinsic calibration validation
 add_executable(${PROJECT_NAME}_camera_intrinsic_calibration_validation src/tools/camera_intrinsic_calibration_validation.cpp)
 set_target_properties(${PROJECT_NAME}_camera_intrinsic_calibration_validation PROPERTIES OUTPUT_NAME camera_intrinsic_calibration_validation PREFIX "")
 add_dependencies(${PROJECT_NAME}_camera_intrinsic_calibration_validation ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(${PROJECT_NAME}_camera_intrinsic_calibration_validation ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools)
+target_link_libraries(${PROJECT_NAME}_camera_intrinsic_calibration_validation ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools rct::rct_common)
 
 #Executable for testing camera noise
 add_executable(${PROJECT_NAME}_noise_qualification_2d src/tools/noise_qualification_2d.cpp)
 set_target_properties(${PROJECT_NAME}_noise_qualification_2d PROPERTIES OUTPUT_NAME noise_qualification_2d PREFIX "")
 add_dependencies(${PROJECT_NAME}_noise_qualification_2d ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(${PROJECT_NAME}_noise_qualification_2d ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools)
+target_link_libraries(${PROJECT_NAME}_noise_qualification_2d ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools rct::rct_common)
 
 #############
 ## Testing ##
@@ -124,7 +125,7 @@ if(CATKIN_ENABLE_TESTING AND RCT_BUILD_TESTS)
   # Tests extrinsic wrist calibration example
   catkin_add_gtest(${PROJECT_NAME}_wrist_test src/examples/camera_on_wrist.cpp)
   target_compile_definitions(${PROJECT_NAME}_wrist_test PRIVATE -DRCT_ENABLE_TESTING)
-  target_link_libraries(${PROJECT_NAME}_wrist_test ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools GTest::GTest GTest::Main)
+  target_link_libraries(${PROJECT_NAME}_wrist_test ${catkin_LIBRARIES} rct::rct_optimizations rct::rct_image_tools rct::rct_common GTest::GTest GTest::Main)
 endif()
 
 #############
diff --git a/rct_examples/src/examples/camera_on_wrist.cpp b/rct_examples/src/examples/camera_on_wrist.cpp
index a39273b4..94d7972e 100644
--- a/rct_examples/src/examples/camera_on_wrist.cpp
+++ b/rct_examples/src/examples/camera_on_wrist.cpp
@@ -14,7 +14,7 @@
 // of this package. It's my only concession to the "self-contained rule".
 #include <rct_ros_tools/data_set.h>
 // This include provide useful print functions for outputing results to terminal
-#include <rct_ros_tools/print_utils.h>
+#include <rct_common/print_utils.h>
 // This header brings in a tool for finding the target in a given image
 #include <rct_image_tools/modified_circle_grid_finder.h>
 // This header brings in he calibration function for 'moving camera' on robot wrist - what we
@@ -26,6 +26,8 @@
 // For std::cout
 #include <iostream>
 
+using namespace rct_common;
+
 int extrinsicWristCameraCalibration()
 {
   // The general goal is to fill out the 'Extrinsic Camera on Wrist' problem definition
@@ -134,18 +136,18 @@ int extrinsicWristCameraCalibration()
   // Step 5: Do something with your results. Here I just print the results, but you might want to
   // update a data structure, save to a file, push to a mutable joint or mutable state publisher in
   // ROS. The options are many, and it's up to you. We just try to help solve the problem.
-  rct_ros_tools::printOptResults(opt_result.converged, opt_result.initial_cost_per_obs, opt_result.final_cost_per_obs);
-  rct_ros_tools::printNewLine();
+  printOptResults(opt_result.converged, opt_result.initial_cost_per_obs, opt_result.final_cost_per_obs);
+  printNewLine();
 
   // Note: Convergence and low cost does not mean a good calibration. See the calibration primer
   // readme on the main page of this repo.
   Eigen::Isometry3d c = opt_result.camera_mount_to_camera;
-  rct_ros_tools::printTransform(c, "Wrist", "Camera", "WRIST TO CAMERA");
-  rct_ros_tools::printNewLine();
+  printTransform(c, "Wrist", "Camera", "WRIST TO CAMERA");
+  printNewLine();
 
   Eigen::Isometry3d t = opt_result.target_mount_to_target;
-  rct_ros_tools::printTransform(t, "Base", "Target", "BASE TO TARGET");
-  rct_ros_tools::printNewLine();
+  printTransform(t, "Base", "Target", "BASE TO TARGET");
+  printNewLine();
 
   return 0;
 }
diff --git a/rct_examples/src/tools/camera_intrinsic_calibration_validation.cpp b/rct_examples/src/tools/camera_intrinsic_calibration_validation.cpp
index 6e9401a6..5d53e914 100644
--- a/rct_examples/src/tools/camera_intrinsic_calibration_validation.cpp
+++ b/rct_examples/src/tools/camera_intrinsic_calibration_validation.cpp
@@ -1,7 +1,7 @@
 #include <rct_optimizations/validation/camera_intrinsic_calibration_validation.h>
+#include <rct_common/print_utils.h>
 #include <rct_ros_tools/parameter_loaders.h>
 #include <rct_ros_tools/target_finder_plugin.h>
-#include <rct_ros_tools/print_utils.h>
 #include <rct_ros_tools/data_set.h>
 #include <rct_ros_tools/loader_utils.h>
 
@@ -17,6 +17,7 @@
 using namespace rct_optimizations;
 using namespace rct_image_tools;
 using namespace rct_ros_tools;
+using namespace rct_common;
 
 std::string WINDOW = "window";
 
diff --git a/rct_examples/src/tools/camera_on_wrist_extrinsic.cpp b/rct_examples/src/tools/camera_on_wrist_extrinsic.cpp
index 7c8766b3..64fa7e4f 100644
--- a/rct_examples/src/tools/camera_on_wrist_extrinsic.cpp
+++ b/rct_examples/src/tools/camera_on_wrist_extrinsic.cpp
@@ -1,8 +1,8 @@
 // Utilities for loading data sets and calib parameters from YAML files via ROS
+#include <rct_common/print_utils.h>
 #include <rct_ros_tools/data_set.h>
 #include <rct_ros_tools/parameter_loaders.h>
 #include <rct_ros_tools/target_finder_plugin.h>
-#include <rct_ros_tools/print_utils.h>
 #include <rct_ros_tools/loader_utils.h>
 // The calibration function for 'moving camera' on robot wrist
 #include <rct_optimizations/extrinsic_hand_eye.h>
@@ -20,6 +20,7 @@
 using namespace rct_optimizations;
 using namespace rct_image_tools;
 using namespace rct_ros_tools;
+using namespace rct_common;
 
 const std::string WINDOW = "window";
 
diff --git a/rct_examples/src/tools/intrinsic_calibration.cpp b/rct_examples/src/tools/intrinsic_calibration.cpp
index e59400a7..d9471431 100644
--- a/rct_examples/src/tools/intrinsic_calibration.cpp
+++ b/rct_examples/src/tools/intrinsic_calibration.cpp
@@ -1,10 +1,10 @@
 #include <rct_image_tools/image_utils.h>
 #include <rct_optimizations/eigen_conversions.h>
 #include <rct_optimizations/experimental/camera_intrinsic.h>
+#include <rct_common/print_utils.h>
 #include <rct_ros_tools/data_set.h>
 #include <rct_ros_tools/parameter_loaders.h>
 #include <rct_ros_tools/target_finder_plugin.h>
-#include <rct_ros_tools/print_utils.h>
 #include <rct_ros_tools/loader_utils.h>
 
 #include <opencv2/calib3d.hpp>
@@ -15,6 +15,7 @@
 using namespace rct_optimizations;
 using namespace rct_image_tools;
 using namespace rct_ros_tools;
+using namespace rct_common;
 
 const std::string WINDOW = "window";
 
diff --git a/rct_examples/src/tools/multi_static_camera_extrinsic.cpp b/rct_examples/src/tools/multi_static_camera_extrinsic.cpp
index 684f812d..ee9a958e 100644
--- a/rct_examples/src/tools/multi_static_camera_extrinsic.cpp
+++ b/rct_examples/src/tools/multi_static_camera_extrinsic.cpp
@@ -1,8 +1,8 @@
 // Utilities for loading data sets and calib parameters from YAML files via ROS
+#include <rct_common/print_utils.h>
 #include <rct_ros_tools/data_set.h>
 #include <rct_ros_tools/parameter_loaders.h>
 #include <rct_ros_tools/target_finder_plugin.h>
-#include <rct_ros_tools/print_utils.h>
 #include <rct_ros_tools/loader_utils.h>
 // To find 2D  observations from images
 #include <rct_image_tools/image_utils.h>
@@ -19,6 +19,7 @@
 using namespace rct_optimizations;
 using namespace rct_image_tools;
 using namespace rct_ros_tools;
+using namespace rct_common;
 
 static void reproject(const Eigen::Isometry3d& base_to_target, const std::vector<Eigen::Isometry3d>& base_to_camera,
                       const std::vector<CameraIntrinsics>& intr, const cv::Mat& image,
diff --git a/rct_examples/src/tools/multi_static_camera_multi_step_extrinsic.cpp b/rct_examples/src/tools/multi_static_camera_multi_step_extrinsic.cpp
index 9a0d1b71..b289d5d6 100644
--- a/rct_examples/src/tools/multi_static_camera_multi_step_extrinsic.cpp
+++ b/rct_examples/src/tools/multi_static_camera_multi_step_extrinsic.cpp
@@ -1,8 +1,8 @@
 // Utilities for loading data sets and calib parameters from YAML files via ROS
+#include <rct_common/print_utils.h>
 #include <rct_ros_tools/data_set.h>
 #include <rct_ros_tools/parameter_loaders.h>
 #include <rct_ros_tools/target_finder_plugin.h>
-#include <rct_ros_tools/print_utils.h>
 #include <rct_ros_tools/loader_utils.h>
 // To find 2D  observations from images
 #include <rct_image_tools/image_utils.h>
@@ -20,6 +20,7 @@
 using namespace rct_optimizations;
 using namespace rct_image_tools;
 using namespace rct_ros_tools;
+using namespace rct_common;
 
 static void reproject(const Eigen::Isometry3d& base_to_target,
                       const std::vector<Eigen::Isometry3d>& base_to_camera,
diff --git a/rct_examples/src/tools/solve_multi_camera_pnp.cpp b/rct_examples/src/tools/solve_multi_camera_pnp.cpp
index 03d54347..db3b722a 100644
--- a/rct_examples/src/tools/solve_multi_camera_pnp.cpp
+++ b/rct_examples/src/tools/solve_multi_camera_pnp.cpp
@@ -6,10 +6,10 @@
 #include <rct_image_tools/image_utils.h>
 #include <rct_optimizations/experimental/multi_camera_pnp.h>
 #include <rct_optimizations/ceres_math_utilities.h>
+#include <rct_common/print_utils.h>
 #include <rct_ros_tools/parameter_loaders.h>
 #include <rct_ros_tools/target_finder_plugin.h>
 #include <rct_ros_tools/data_set.h>
-#include <rct_ros_tools/print_utils.h>
 #include <rct_ros_tools/loader_utils.h>
 
 #include <opencv2/highgui.hpp>
@@ -21,6 +21,7 @@
 using namespace rct_optimizations;
 using namespace rct_image_tools;
 using namespace rct_ros_tools;
+using namespace rct_common;
 
 static void reproject(const Eigen::Isometry3d& base_to_target,
                       const std::vector<Eigen::Isometry3d>& base_to_camera,
diff --git a/rct_examples/src/tools/solve_pnp.cpp b/rct_examples/src/tools/solve_pnp.cpp
index e05e61ec..cbd3e804 100644
--- a/rct_examples/src/tools/solve_pnp.cpp
+++ b/rct_examples/src/tools/solve_pnp.cpp
@@ -5,9 +5,9 @@
  */
 #include <rct_image_tools/image_utils.h>
 #include <rct_optimizations/pnp.h>
+#include <rct_common/print_utils.h>
 #include <rct_ros_tools/parameter_loaders.h>
 #include <rct_ros_tools/target_finder_plugin.h>
-#include <rct_ros_tools/print_utils.h>
 #include <rct_ros_tools/loader_utils.h>
 
 #include <opencv2/highgui.hpp>
@@ -18,6 +18,7 @@
 using namespace rct_optimizations;
 using namespace rct_image_tools;
 using namespace rct_ros_tools;
+using namespace rct_common;
 
 std::string WINDOW = "window";
 
diff --git a/rct_examples/src/tools/static_camera_extrinsic.cpp b/rct_examples/src/tools/static_camera_extrinsic.cpp
index 891f66a1..34933279 100644
--- a/rct_examples/src/tools/static_camera_extrinsic.cpp
+++ b/rct_examples/src/tools/static_camera_extrinsic.cpp
@@ -1,8 +1,8 @@
 // Utilities for loading data sets and calib parameters from YAML files via ROS
+#include <rct_common/print_utils.h>
 #include <rct_ros_tools/data_set.h>
 #include <rct_ros_tools/parameter_loaders.h>
 #include <rct_ros_tools/target_finder_plugin.h>
-#include <rct_ros_tools/print_utils.h>
 #include <rct_ros_tools/loader_utils.h>
 // The calibration function for 'static camera' on robot wrist
 #include <rct_optimizations/extrinsic_hand_eye.h>
@@ -18,6 +18,7 @@
 using namespace rct_optimizations;
 using namespace rct_image_tools;
 using namespace rct_ros_tools;
+using namespace rct_common;
 
 std::string WINDOW = "window";
 
diff --git a/rct_image_tools/cmake/rct_image_tools-config.cmake.in b/rct_image_tools/cmake/rct_image_tools-config.cmake.in
index 264bd77b..029bb561 100644
--- a/rct_image_tools/cmake/rct_image_tools-config.cmake.in
+++ b/rct_image_tools/cmake/rct_image_tools-config.cmake.in
@@ -5,8 +5,11 @@ set_and_check(@PROJECT_NAME@_INCLUDE_DIRS "${PACKAGE_PREFIX_DIR}/include")
 set_and_check(@PROJECT_NAME@_LIBRARY_DIRS "${PACKAGE_PREFIX_DIR}/lib")
 
 include(CMakeFindDependencyMacro)
+find_dependency(Boost)
 find_dependency(Eigen3)
 find_dependency(OpenCV)
 find_dependency(yaml-cpp)
+find_dependency(rct_common)
+find_dependency(rct_optimizations)
 
 include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@-targets.cmake")
diff --git a/rct_image_tools/src/rct_image_tools/modified_circle_grid_target.cpp b/rct_image_tools/src/rct_image_tools/modified_circle_grid_target.cpp
index 33b30a4a..6d786327 100644
--- a/rct_image_tools/src/rct_image_tools/modified_circle_grid_target.cpp
+++ b/rct_image_tools/src/rct_image_tools/modified_circle_grid_target.cpp
@@ -30,7 +30,7 @@ rct_optimizations::Correspondence2D3D::Set ModifiedCircleGridTarget::createCorre
     rct_optimizations::Correspondence2D3D corr;
     corr.in_target = target_points.at(i);
     // Get the first (and only) element of the features at the current index; increment the index
-    corr.in_image = target_features.at(static_cast<const unsigned>(i)).at(0);
+    corr.in_image = target_features.at(i).at(0);
     correspondences.push_back(corr);
   }
 
diff --git a/rct_optimizations/cmake/rct_optimizations-config.cmake.in b/rct_optimizations/cmake/rct_optimizations-config.cmake.in
index f2bb8433..ffa4fec8 100644
--- a/rct_optimizations/cmake/rct_optimizations-config.cmake.in
+++ b/rct_optimizations/cmake/rct_optimizations-config.cmake.in
@@ -5,6 +5,9 @@ set_and_check(@PROJECT_NAME@_INCLUDE_DIRS "${PACKAGE_PREFIX_DIR}/include")
 set_and_check(@PROJECT_NAME@_LIBRARY_DIRS "${PACKAGE_PREFIX_DIR}/lib")
 
 include(CMakeFindDependencyMacro)
+find_dependency(rct_common)
+find_dependency(Boost)
 find_dependency(Ceres)
+find_dependency(yaml-cpp)
 
 include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@-targets.cmake")
diff --git a/rct_optimizations/include/rct_optimizations/covariance_analysis.h b/rct_optimizations/include/rct_optimizations/covariance_analysis.h
index 12acd944..0f361a51 100644
--- a/rct_optimizations/include/rct_optimizations/covariance_analysis.h
+++ b/rct_optimizations/include/rct_optimizations/covariance_analysis.h
@@ -20,6 +20,14 @@ struct DefaultCovarianceOptions : ceres::Covariance::Options
   }
 };
 
+/**
+ * @brief Get the covariance results provided covariance matrix and parameter names
+ * @param cov_matrix The covariance matrix
+ * @param parameter_names The parameter names
+ * @return The covariance results
+ */
+CovarianceResult computeCovarianceResults(const Eigen::MatrixXd& cov_matrix, const std::vector<std::string>& parameter_names);
+
 /**
  * @brief Given a covariance matrix, calculate the matrix of standard deviations and correlation coefficients.
  * @param Covariance matrix
@@ -30,40 +38,51 @@ Eigen::MatrixXd computeCorrelationsFromCovariance(const Eigen::MatrixXd& covaria
 
 /**
  * @brief Compute all covariance results for a Ceres optimization problem. Labels results with generic names.
+ * @details This excludes all parameters set to be constant during the solve
  * @param problem The Ceres problem (after optimization).
+ * @param param_masks Map of the parameter block pointer and the indices of the parameters within that block to be excluded from the covariance calculation
  * @param options ceres::Covariance::Options to use when calculating covariance.
  * @return CovarianceResult for the problem.
  */
 CovarianceResult computeCovariance(ceres::Problem &problem,
+                                   const std::map<const double *, std::vector<int> >& param_masks = std::map<const double *, std::vector<int>>(),
                                    const ceres::Covariance::Options& options = DefaultCovarianceOptions());
 
 /**
  * @brief Compute covariance results for the specified parameter blocks in a Ceres optimization problem. Labels results with generic names.
+ * @details This excludes all parameters set to be constant during the solve
  * @param problem The Ceres problem (after optimization).
  * @param parameter_blocks Specific parameter blocks to compute covariance between.
+ * @param param_masks Map of the parameter block pointer and the indices of the parameters within that block to be excluded from the covariance calculation
  * @param options ceres::Covariance::Options to use when calculating covariance.
  * @return CovarianceResult for the problem.
  */
 CovarianceResult computeCovariance(ceres::Problem &problem,
                                    const std::vector<const double *>& parameter_blocks,
+                                   const std::map<const double *, std::vector<int> >& param_masks = std::map<const double *, std::vector<int>>(),
                                    const ceres::Covariance::Options& options = DefaultCovarianceOptions());
 
 /**
  * @brief Compute all covariance results for a Ceres optimization problem and label them with the provided names.
+ * @details This excludes all parameters set to be constant during the solve
  * @param problem The Ceres problem (after optimization).
  * @param parameter_names Labels for all optimization parameters in the problem.
+ * @param param_masks Map of the parameter block pointer and the indices of the parameters within that block to be excluded from the covariance calculation
  * @param options ceres::Covariance::Options to use when calculating covariance.
  * @return CovarianceResult for the problem.
  */
 CovarianceResult computeCovariance(ceres::Problem &problem,
-                                   const std::vector<std::vector<std::string>>& parameter_names,
+                                   const std::map<const double *, std::vector<std::string> > &param_names,
+                                   const std::map<const double *, std::vector<int> >& param_masks = std::map<const double *, std::vector<int>>(),
                                    const ceres::Covariance::Options& options = DefaultCovarianceOptions());
 
 /**
  * @brief Compute covariance results for specified parameter blocks in a Ceres optimization problem and label them with the provided names.
+ * @details This excludes all parameters set to be constant during the solve
  * @param problem The Ceres problem (after optimization).
  * @param parameter_blocks Specific parameter blocks for which covariance will be calculated.
  * @param parameter_names Labels for optimization parameters in the specified blocks.
+ * @param param_masks Map of the parameter block pointer and the indices of the parameters within that block to be excluded from the covariance calculation
  * @param options ceres::Covariance::Options to use when calculating covariance.
  * @return CovarianceResult for the problem.
  * @throw CovarianceException if covariance.Compute fails.
@@ -73,6 +92,8 @@ CovarianceResult computeCovariance(ceres::Problem &problem,
  */
 CovarianceResult computeCovariance(ceres::Problem &problem,
                                    const std::vector<const double *>& parameter_blocks,
-                                   const std::vector<std::vector<std::string>>& parameter_names,
+                                   const std::map<const double *, std::vector<std::string> > &param_names,
+                                   const std::map<const double *, std::vector<int> >& param_masks = std::map<const double *, std::vector<int>>(),
                                    const ceres::Covariance::Options& options = DefaultCovarianceOptions());
+
 }  // namespace rct_optimizations
diff --git a/rct_optimizations/include/rct_optimizations/dh_chain.h b/rct_optimizations/include/rct_optimizations/dh_chain.h
index 0f2c16ff..bc3c30f6 100644
--- a/rct_optimizations/include/rct_optimizations/dh_chain.h
+++ b/rct_optimizations/include/rct_optimizations/dh_chain.h
@@ -216,6 +216,14 @@ class DHChain
    */
   Eigen::Isometry3d getBaseOffset() const;
 
+  /**
+   * @brief Get a joints relative joint transform
+   * @param joint_index The joint index
+   * @param value The joint value
+   * @return The joint relative transform
+   */
+  Eigen::Isometry3d getRelativeTransform(int joint_index, double value) const;
+
 protected:
   /** @brief The DH transforms that make up the chain */
   std::vector<DHTransform> transforms_;
diff --git a/rct_optimizations/include/rct_optimizations/dh_chain_kinematic_calibration.h b/rct_optimizations/include/rct_optimizations/dh_chain_kinematic_calibration.h
index e88a7c0f..a30126d8 100644
--- a/rct_optimizations/include/rct_optimizations/dh_chain_kinematic_calibration.h
+++ b/rct_optimizations/include/rct_optimizations/dh_chain_kinematic_calibration.h
@@ -183,39 +183,71 @@ class DualDHChainCost
     return parameters;
   }
 
-  static std::vector<std::vector<std::string>> constructParameterLabels(const std::vector<std::array<std::string, 4>>& camera_chain_labels,
-                                                                        const std::vector<std::array<std::string, 4>>& target_chain_labels,
-                                                                        const std::array<std::string, 3>& camera_mount_to_camera_position_labels,
-                                                                        const std::array<std::string, 3>& camera_mount_to_camera_angle_axis_labels,
-                                                                        const std::array<std::string, 3>& target_mount_to_target_position_labels,
-                                                                        const std::array<std::string, 3>& target_mount_to_target_angle_axis_labels,
-                                                                        const std::array<std::string, 3>& camera_chain_base_to_target_chain_base_position_labels,
-                                                                        const std::array<std::string, 3>& camera_chain_base_to_target_chain_base_angle_axis_labels)
+  static std::map<const double*, std::vector<std::string>>
+  constructParameterLabels(const std::vector<double *>& parameters,
+                           const std::vector<std::array<std::string, 4>>& camera_chain_labels,
+                           const std::vector<std::array<std::string, 4>>& target_chain_labels,
+                           const std::array<std::string, 3>& camera_mount_to_camera_position_labels,
+                           const std::array<std::string, 3>& camera_mount_to_camera_angle_axis_labels,
+                           const std::array<std::string, 3>& target_mount_to_target_position_labels,
+                           const std::array<std::string, 3>& target_mount_to_target_angle_axis_labels,
+                           const std::array<std::string, 3>& camera_chain_base_to_target_chain_base_position_labels,
+                           const std::array<std::string, 3>& camera_chain_base_to_target_chain_base_angle_axis_labels)
   {
-    std::vector<std::vector<std::string>> param_labels;
+    // Eigen is column major so need to store labels column major
+    std::map<const double*, std::vector<std::string>> param_labels;
     std::vector<std::string> cc_labels_concatenated;
-    for (auto cc_label : camera_chain_labels)
+    for (std::size_t c = 0; c < 4; ++c)
     {
-      cc_labels_concatenated.insert(cc_labels_concatenated.end(), cc_label.begin(), cc_label.end());
+      for (auto cc_label : camera_chain_labels)
+        cc_labels_concatenated.push_back(cc_label.at(c));
     }
-    param_labels.push_back(cc_labels_concatenated);
+    param_labels[parameters[0]] = cc_labels_concatenated;
 
+    // Eigen is column major so need to store labels column major
     std::vector<std::string> tc_labels_concatenated;
-    for (auto tc_label : target_chain_labels)
+    for (std::size_t c = 0; c < 4; ++c)
     {
-      tc_labels_concatenated.insert(tc_labels_concatenated.end(), tc_label.begin(), tc_label.end());
+      for (auto tc_label : target_chain_labels)
+        tc_labels_concatenated.push_back(tc_label.at(c));
     }
-    param_labels.push_back(tc_labels_concatenated);
-
-    param_labels.emplace_back(camera_mount_to_camera_position_labels.begin(), camera_mount_to_camera_position_labels.end());
-    param_labels.emplace_back(camera_mount_to_camera_angle_axis_labels.begin(), camera_mount_to_camera_angle_axis_labels.end());
-    param_labels.emplace_back(target_mount_to_target_position_labels.begin(), target_mount_to_target_position_labels.end());
-    param_labels.emplace_back(target_mount_to_target_angle_axis_labels.begin(), target_mount_to_target_angle_axis_labels.end());
-    param_labels.emplace_back(camera_chain_base_to_target_chain_base_position_labels.begin(), camera_chain_base_to_target_chain_base_position_labels.end());
-    param_labels.emplace_back(camera_chain_base_to_target_chain_base_angle_axis_labels.begin(), camera_chain_base_to_target_chain_base_angle_axis_labels.end());
+    param_labels[parameters[1]] = tc_labels_concatenated;
+
+    param_labels[parameters[2]] = std::vector<std::string>(camera_mount_to_camera_position_labels.begin(),camera_mount_to_camera_position_labels.end());
+    param_labels[parameters[3]] = std::vector<std::string>(camera_mount_to_camera_angle_axis_labels.begin(),camera_mount_to_camera_angle_axis_labels.end());
+    param_labels[parameters[4]] = std::vector<std::string>(target_mount_to_target_position_labels.begin(),target_mount_to_target_position_labels.end());
+    param_labels[parameters[5]] = std::vector<std::string>(target_mount_to_target_angle_axis_labels.begin(),target_mount_to_target_angle_axis_labels.end());
+    param_labels[parameters[6]] = std::vector<std::string>(camera_chain_base_to_target_chain_base_position_labels.begin(),camera_chain_base_to_target_chain_base_position_labels.end());
+    param_labels[parameters[7]] = std::vector<std::string>(camera_chain_base_to_target_chain_base_angle_axis_labels.begin(),camera_chain_base_to_target_chain_base_angle_axis_labels.end());
     return param_labels;
   }
 
+  static std::map<const double*, std::vector<int>>
+  constructParameterMasks(const std::vector<double *>& parameters, const std::array<std::vector<int>, 8>& masks)
+  {
+    assert(parameters.size() == masks.size());
+    std::map<const double*, std::vector<int>> param_masks;
+    for (std::size_t i = 0; i < parameters.size(); ++i)
+      param_masks[parameters[i]] = masks[i];
+
+    return param_masks;
+  }
+
+  static std::map<const double*, std::string> constructParameterNames(const std::vector<double *>& parameters)
+  {
+    std::map<const double*, std::string> names;
+    names[parameters[0]] = "Camera DH Parameters";
+    names[parameters[1]] = "Target DH Parameters";
+    names[parameters[2]] = "Camera Mount to Camera Position Parameters";
+    names[parameters[3]] = "Camera Mount to Camera Orientation Parameters";
+    names[parameters[4]] = "Target Mount to Target Position Parameters";
+    names[parameters[5]] = "Target Mount to Target Orientation Parameters";
+    names[parameters[6]] = "Camera Chain Base to Target Chain Base Position Parameters";
+    names[parameters[7]] = "Camera Chain Base to Target Chain Base Orientation Parameters";
+
+    return names;
+  }
+
 protected:
   const DHChain &camera_chain_;
   const DHChain &target_chain_;
diff --git a/rct_optimizations/include/rct_optimizations/local_parameterization.h b/rct_optimizations/include/rct_optimizations/local_parameterization.h
index cd402895..f7bc88c6 100644
--- a/rct_optimizations/include/rct_optimizations/local_parameterization.h
+++ b/rct_optimizations/include/rct_optimizations/local_parameterization.h
@@ -78,64 +78,55 @@ struct EigenQuaternionPlus
 /**
  * @brief Adds subset parameterization for all parameter blocks for a given problem
  * @param problem - Ceres optimization problem
- * @param masks - an array of masks indicating the index of the parameters that should be held constant.
- *   - The array must be the same size as the number of parameter blocks in the problem
- *   - Individual mask vectors cannot mask all parameters in a block
- * @param parameter_blocks - Ceres parameter blocks in the same order as the input masks
+ * @param param_masks - A map of parameter to an array of masks indicating the index of the parameters that should be held constant.
+ *   - The map must be the same size as the number of parameter blocks in the problem
+ *   - If all parameters are masked it sets that block to constant
  * @throws OptimizationException
  */
-template <std::size_t N_BLOCKS>
-void addSubsetParameterization(ceres::Problem& problem, const std::array<std::vector<int>, N_BLOCKS>& masks,
-                               const std::array<double*, N_BLOCKS>& parameter_blocks)
+void addSubsetParameterization(ceres::Problem& problem, const std::map<const double*, std::vector<int>>& param_masks)
 {
-  // Make sure the Ceres problem has the same number of blocks as the input
-  if (problem.NumParameterBlocks() != N_BLOCKS)
-  {
-    std::stringstream ss;
-    ss << "Input parameter block size does not match Ceres problem parameter block size (" << N_BLOCKS
-       << " != " << problem.NumParameterBlocks() << ")";
-    throw OptimizationException(ss.str());
-  }
+  if (param_masks.empty())
+    return;
 
-  if (parameter_blocks.size() != masks.size())
-  {
-    std::stringstream ss;
-    ss << "Parameter block count does not match number of masks (" << parameter_blocks.size() << " != " << masks.size();
-    throw OptimizationException(ss.str());
-  }
+  std::vector<double *> parameter_blocks;
+  problem.GetParameterBlocks(&parameter_blocks);
 
   // Set identity local parameterization on individual variables that we want to remain constant
-  for (std::size_t i = 0; i < parameter_blocks.size(); ++i)
+  for (const auto& p : parameter_blocks)
   {
-    std::size_t block_size = problem.ParameterBlockSize(parameter_blocks.at(i));
-    std::vector<int> mask = masks.at(i);
-
-    if (mask.size() > 0)
+    std::size_t block_size = problem.ParameterBlockSize(p);
+    auto it = param_masks.find(p);
+    if (it != param_masks.end())
     {
-      // Sort the array and remove duplicate indices
-      std::sort(mask.begin(), mask.end());
-      auto last = std::unique(mask.begin(), mask.end());
-      mask.erase(last, mask.end());
+      std::vector<int> mask = it->second;
 
-      // Check that the max index is not greater than the number of elements in the block
-      auto it = std::max_element(mask.begin(), mask.end());
-      if (static_cast<std::size_t>(*it) >= block_size)
+      if (mask.size() > 0)
       {
-        std::stringstream ss;
-        ss << "The largest mask index cannot be larger than or equal to the parameter block size (" << *it
-           << " >= " << block_size << ")";
-        throw OptimizationException(ss.str());
-      }
+        // Sort the array and remove duplicate indices
+        std::sort(mask.begin(), mask.end());
+        auto last = std::unique(mask.begin(), mask.end());
+        mask.erase(last, mask.end());
 
-      // Set local parameterization on the indices or set the entire block constant
-      if (mask.size() >= block_size)
-      {
-        problem.SetParameterBlockConstant(parameter_blocks.at(i));
-      }
-      else
-      {
-        ceres::LocalParameterization* lp = new ceres::SubsetParameterization(block_size, mask);
-        problem.SetParameterization(parameter_blocks[i], lp);
+        // Check that the max index is not greater than the number of elements in the block
+        auto it = std::max_element(mask.begin(), mask.end());
+        if (static_cast<std::size_t>(*it) >= block_size)
+        {
+          std::stringstream ss;
+          ss << "The largest mask index cannot be larger than or equal to the parameter block size (" << *it
+             << " >= " << block_size << ")";
+          throw OptimizationException(ss.str());
+        }
+
+        // Set local parameterization on the indices or set the entire block constant
+        if (mask.size() >= block_size)
+        {
+          problem.SetParameterBlockConstant(p);
+        }
+        else
+        {
+          ceres::LocalParameterization* lp = new ceres::SubsetParameterization(block_size, mask);
+          problem.SetParameterization(p, lp);
+        }
       }
     }
   }
diff --git a/rct_optimizations/include/rct_optimizations/types.h b/rct_optimizations/include/rct_optimizations/types.h
index 66cd70ff..ef450ba3 100644
--- a/rct_optimizations/include/rct_optimizations/types.h
+++ b/rct_optimizations/include/rct_optimizations/types.h
@@ -12,7 +12,7 @@ namespace rct_optimizations
  */
 struct CameraIntrinsics
 {
-  std::array<double, 4> values;
+  std::array<double, 4> values {0,0,0,0};
 
   double& fx() { return values[0]; }
   double& fy() { return values[1]; }
@@ -35,7 +35,7 @@ struct Pose6d
   Pose6d() = default;
   Pose6d(std::array<double, 6> l) : values(l) {}
 
-  std::array<double, 6> values;
+  std::array<double, 6> values{0,0,0,0,0,0};
 
   double& rx() { return values[0]; }
   double& ry() { return values[1]; }
diff --git a/rct_optimizations/src/rct_optimizations/camera_intrinsic.cpp b/rct_optimizations/src/rct_optimizations/camera_intrinsic.cpp
index 612864fa..c29df262 100644
--- a/rct_optimizations/src/rct_optimizations/camera_intrinsic.cpp
+++ b/rct_optimizations/src/rct_optimizations/camera_intrinsic.cpp
@@ -222,10 +222,10 @@ rct_optimizations::optimize(const rct_optimizations::IntrinsicEstimationProblem&
   }
 
   std::vector<const double*> param_blocks;
-  std::vector<std::vector<std::string>> param_labels;
+  std::map<const double*, std::vector<std::string>> param_labels;
 
   param_blocks.emplace_back(internal_intrinsics_data.data());
-  param_labels.emplace_back(params.labels_intrinsic_params.begin(), params.labels_intrinsic_params.end());
+  param_labels[internal_intrinsics_data.data()] = std::vector<std::string>(params.labels_intrinsic_params.begin(), params.labels_intrinsic_params.end());
 
   for (std::size_t i = 0; i < internal_poses.size(); i++)
   {
@@ -236,7 +236,7 @@ rct_optimizations::optimize(const rct_optimizations::IntrinsicEstimationProblem&
     {
       labels.push_back(params.label_extr + std::to_string(i) + "_" + label_extr);
     }
-    param_labels.push_back(labels);
+    param_labels[internal_poses[i].values.data()] = labels;
   }
 
   // Solve
diff --git a/rct_optimizations/src/rct_optimizations/circle_fit.cpp b/rct_optimizations/src/rct_optimizations/circle_fit.cpp
index a5970f67..cebd43c2 100644
--- a/rct_optimizations/src/rct_optimizations/circle_fit.cpp
+++ b/rct_optimizations/src/rct_optimizations/circle_fit.cpp
@@ -40,13 +40,16 @@ rct_optimizations::optimize(const rct_optimizations::CircleFitProblem& params)
 
   std::cout << summary.BriefReport() << std::endl;
 
+  std::map<const double*, std::vector<std::string>> param_block_labels;
+  param_block_labels[circle_params.data()] = params.labels;
+
   result.converged = summary.termination_type == ceres::TerminationType::CONVERGENCE;
   result.x_center = circle_params[0];
   result.y_center = circle_params[1];
   result.radius = pow(circle_params[2], 2);
   result.initial_cost_per_obs = summary.initial_cost / summary.num_residuals;
   result.final_cost_per_obs = summary.final_cost / summary.num_residuals;
-  result.covariance = rct_optimizations::computeCovariance(problem, std::vector<std::vector<std::string>>({params.labels}));
+  result.covariance = rct_optimizations::computeCovariance(problem, param_block_labels);
 
   return result;
 }
diff --git a/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp b/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp
index 08ab2ddd..15dd517d 100644
--- a/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp
+++ b/rct_optimizations/src/rct_optimizations/covariance_analysis.cpp
@@ -36,7 +36,57 @@ Eigen::MatrixXd computeCorrelationsFromCovariance(const Eigen::MatrixXd& covaria
   return out;
 }
 
+CovarianceResult computeCovarianceResults(const Eigen::MatrixXd& cov_matrix, const std::vector<std::string>& parameter_names)
+{
+  // 0. Save original covariance matrix output
+  // For parameter blocks [p1, p2], the structure of this matrix will be:
+  /*    |     p1    |     p2    |
+   * ---|-----------|-----------|
+   * p1 | C(p1, p1) | C(p1, p2) |
+   * p2 | C(p2, p1) | C(p2, p2) |
+   */
+  CovarianceResult res;
+  res.covariance_matrix = cov_matrix;
+
+  // 1. Compute matrix of standard deviations and correlation coefficients.
+  // The arrangement of elements in the correlation matrix matches the order in the covariance matrix.
+  Eigen::MatrixXd correlation_matrix = computeCorrelationsFromCovariance(cov_matrix);
+  res.correlation_matrix = correlation_matrix;
+
+  // 2. Create NamedParams for covariance and correlation results, which include labels and values for the parameters. Uses top-right triangular part of matrix.
+  Eigen::Index col_start = 0;
+  for (Eigen::Index row = 0; row < correlation_matrix.rows(); row++)
+  {
+    for (Eigen::Index col = col_start; col < correlation_matrix.rows(); col++)
+    {
+      NamedParam p_corr;
+      p_corr.value = correlation_matrix(row, col);
+
+      if (row == col)  // diagonal element, standard deviation
+      {
+        p_corr.names = std::make_pair(parameter_names[static_cast<std::size_t>(row)], "");
+        res.standard_deviations.push_back(p_corr);
+        continue;
+      }
+
+      // otherwise off-diagonal element, correlation coefficient
+      p_corr.names = std::make_pair(parameter_names[static_cast<std::size_t>(row)], parameter_names[static_cast<std::size_t>(col)]);
+      res.correlation_coeffs.push_back(p_corr);
+
+      // for off-diagonal elements also get covariance
+      NamedParam p_cov;
+      p_cov.value = cov_matrix(row, col);
+      p_cov.names = std::make_pair(parameter_names[static_cast<std::size_t>(row)], parameter_names[static_cast<std::size_t>(col)]);
+      res.covariances.push_back(p_cov);
+    }
+    col_start++;
+  }
+
+  return res;
+}
+
 CovarianceResult computeCovariance(ceres::Problem &problem,
+                                   const std::map<const double *, std::vector<int> > &param_masks,
                                    const ceres::Covariance::Options& options)
 {
   std::vector<double *> blocks;
@@ -44,14 +94,15 @@ CovarianceResult computeCovariance(ceres::Problem &problem,
 
   const std::vector<const double *> blocks_const(blocks.begin(), blocks.end());
 
-  return computeCovariance(problem, blocks_const, options);
+  return computeCovariance(problem, blocks_const, param_masks, options);
 }
 
 CovarianceResult computeCovariance(ceres::Problem &problem,
                                    const std::vector<const double *>& parameter_blocks,
+                                   const std::map<const double *, std::vector<int> > &param_masks,
                                    const ceres::Covariance::Options& options)
 {
-  std::vector<std::vector<std::string>> dummy_param_names;
+  std::map<const double *, std::vector<std::string>> dummy_param_names;
   for (std::size_t block_index = 0; block_index < parameter_blocks.size(); block_index++)
   {
     int size = problem.ParameterBlockSize(parameter_blocks[block_index]);
@@ -61,15 +112,16 @@ CovarianceResult computeCovariance(ceres::Problem &problem,
       // names follow format "blockN_elementM"
        block_names.emplace_back("block" + std::to_string(block_index) + "_element" + std::to_string(i));
     }
-    dummy_param_names.push_back(block_names);
+    dummy_param_names[parameter_blocks[block_index]] = block_names;
   }
 
-  return computeCovariance(problem, parameter_blocks, dummy_param_names, options);
+  return computeCovariance(problem, parameter_blocks, dummy_param_names, param_masks, options);
 
 }
 
 CovarianceResult computeCovariance(ceres::Problem &problem,
-                                   const std::vector<std::vector<std::string>>& parameter_names,
+                                   const std::map<const double *, std::vector<std::string> > &param_names,
+                                   const std::map<const double *, std::vector<int> > &param_masks,
                                    const ceres::Covariance::Options& options)
 {
   // Get all parameter blocks for the problem
@@ -78,29 +130,52 @@ CovarianceResult computeCovariance(ceres::Problem &problem,
 
   const std::vector<const double *> param_blocks_const(parameter_blocks.begin(), parameter_blocks.end());
 
-  return computeCovariance(problem, param_blocks_const, parameter_names, options);
+  return computeCovariance(problem, param_blocks_const, param_names, param_masks, options);
 }
 
 CovarianceResult computeCovariance(ceres::Problem &problem,
                                    const std::vector<const double *>& parameter_blocks,
-                                   const std::vector<std::vector<std::string>>& parameter_names,
+                                   const std::map<const double*, std::vector<std::string>>& param_names,
+                                   const std::map<const double*, std::vector<int>>& param_masks,
                                    const ceres::Covariance::Options& options)
-  {
+{
   // 0. Check user-specified arguments
-  if (parameter_blocks.size() != parameter_names.size())
+  if (parameter_blocks.size() != param_names.size())
     throw CovarianceException("Provided vector parameter_names is not same length as provided number of parameter blocks");
 
   Eigen::Index n_params_in_selected = 0;
-  for (std::size_t index_block = 0; index_block < parameter_blocks.size(); index_block++)
+  std::vector<Eigen::Index> tangent_space_indices;
+  std::vector<std::string> tangent_space_labels;
+  for (const double* b : parameter_blocks)
   {
-    int block_size = problem.ParameterBlockSize(parameter_blocks[index_block]);
-    if (static_cast<std::size_t>(block_size) != parameter_names[index_block].size())
+    int block_size = problem.ParameterBlockSize(b);
+    if (static_cast<std::size_t>(block_size) != param_names.at(b).size())
     {
       std::stringstream ss;
-      ss << "Number of parameter labels provided for block " << index_block << " does not match actual number of parameters in that block: " \
-         << "have " << parameter_names[index_block].size() << " labels and " << block_size << " parameters";
+      ss << "Number of parameter labels provided for block does not match actual number of parameters in that block: " \
+         << "have " << param_names.at(b).size() << " labels and " << block_size << " parameters";
       throw CovarianceException(ss.str());
     }
+
+    // Extract tangent space
+    if (!problem.IsParameterBlockConstant(const_cast<double*>(b)))
+    {
+      std::vector<int> masks;
+      auto it = param_masks.find(b);
+      if (it != param_masks.end())
+        masks = it->second;
+
+      const std::vector<std::string>& label = param_names.at(b);
+      for (std::size_t i = 0; i < static_cast<std::size_t>(block_size); ++i)
+      {
+        if (std::find(masks.begin(), masks.end(), i) == masks.end())
+        {
+          tangent_space_indices.push_back(n_params_in_selected + static_cast<Eigen::Index>(i));
+          tangent_space_labels.push_back(label.at(i));
+        }
+      }
+    }
+
     n_params_in_selected += block_size;
   }
 
@@ -116,57 +191,17 @@ CovarianceResult computeCovariance(ceres::Problem &problem,
     throw CovarianceException("GetCovarianceMatrix failed in computeCovariance()");
   }
 
-  // 2. Save original covariance matrix output
-  // For parameter blocks [p1, p2], the structure of this matrix will be:
-  /*    |     p1    |     p2    |
-   * ---|-----------|-----------|
-   * p1 | C(p1, p1) | C(p1, p2) |
-   * p2 | C(p2, p1) | C(p2, p2) |
-   */
-  CovarianceResult res;
-  res.covariance_matrix = cov_matrix;
-
-  // 3. Compute matrix of standard deviations and correlation coefficients.
-  // The arrangement of elements in the correlation matrix matches the order in the covariance matrix.
-  Eigen::MatrixXd correlation_matrix = computeCorrelationsFromCovariance(cov_matrix);
-  res.correlation_matrix = correlation_matrix;
-
-  // 4. Compose parameter label strings
-  std::vector<std::string> parameter_names_concatenated;
-  for (auto names : parameter_names)
-  {
-    parameter_names_concatenated.insert(parameter_names_concatenated.end(), names.begin(), names.end());
-  }
-
-  // 5. Create NamedParams for covariance and correlation results, which include labels and values for the parameters. Uses top-right triangular part of matrix.
-  Eigen::Index col_start = 0;
-  for (Eigen::Index row = 0; row < correlation_matrix.rows(); row++)
+  // 2. Extract the tangent space cov matrix
+  Eigen::MatrixXd tangent_space_cov_matrix(tangent_space_labels.size(), tangent_space_labels.size());
+  for (std::size_t r = 0; r < tangent_space_indices.size(); ++r)
   {
-    for (Eigen::Index col = col_start; col < correlation_matrix.rows(); col++)
+    for (std::size_t c = 0; c < tangent_space_indices.size(); ++c)
     {
-      NamedParam p_corr;
-      p_corr.value = correlation_matrix(row, col);
-
-      if (row == col)  // diagonal element, standard deviation
-      {
-        p_corr.names = std::make_pair(parameter_names_concatenated[static_cast<std::size_t>(row)], "");
-        res.standard_deviations.push_back(p_corr);
-        continue;
-      }
-
-      // otherwise off-diagonal element, correlation coefficient
-      p_corr.names = std::make_pair(parameter_names_concatenated[static_cast<std::size_t>(row)], parameter_names_concatenated[static_cast<std::size_t>(col)]);
-      res.correlation_coeffs.push_back(p_corr);
-
-      // for off-diagonal elements also get covariance
-      NamedParam p_cov;
-      p_cov.value = cov_matrix(row, col);
-      p_cov.names = std::make_pair(parameter_names_concatenated[static_cast<std::size_t>(row)], parameter_names_concatenated[static_cast<std::size_t>(col)]);
-      res.covariances.push_back(p_cov);
+      tangent_space_cov_matrix(r, c) = cov_matrix(tangent_space_indices[r], tangent_space_indices[c]);
     }
-    col_start++;
   }
 
-  return res;
+  // 3. Get the covariance results
+  return computeCovarianceResults(tangent_space_cov_matrix, tangent_space_labels);
 }
 }  // namespace rct_optimizations
diff --git a/rct_optimizations/src/rct_optimizations/dh_chain.cpp b/rct_optimizations/src/rct_optimizations/dh_chain.cpp
index 9ee4be80..929803c2 100644
--- a/rct_optimizations/src/rct_optimizations/dh_chain.cpp
+++ b/rct_optimizations/src/rct_optimizations/dh_chain.cpp
@@ -116,4 +116,12 @@ Eigen::Isometry3d DHChain::getBaseOffset() const
   return base_offset_;
 }
 
+Eigen::Isometry3d DHChain::getRelativeTransform(int joint_index, double value) const
+{
+  if (joint_index < 0 || joint_index >= static_cast<int>(transforms_.size()))
+    throw std::runtime_error("getRelativeTransform, Invalid joint index");
+
+  return transforms_[joint_index].createRelativeTransform(value);
+}
+
 } // namespace rct_optimizations
diff --git a/rct_optimizations/src/rct_optimizations/dh_chain_kinematic_calibration.cpp b/rct_optimizations/src/rct_optimizations/dh_chain_kinematic_calibration.cpp
index 1d5bc24e..b681040c 100644
--- a/rct_optimizations/src/rct_optimizations/dh_chain_kinematic_calibration.cpp
+++ b/rct_optimizations/src/rct_optimizations/dh_chain_kinematic_calibration.cpp
@@ -23,6 +23,54 @@ Eigen::Isometry3d createTransform(const Eigen::Vector3d& t, const Eigen::Vector3
   return result;
 }
 
+void printLabels(const std::string& block_name, const std::vector<std::string>& param_labels)
+{
+  std::cout << block_name << ":" << std::endl ;
+  if (param_labels.empty())
+  {
+    std::cout << "    - Constant" << std::endl;
+    return;
+  }
+
+  for (const auto& param_label : param_labels)
+    std::cout << "    - " << param_label << std::endl;
+
+}
+
+void printOptimizationLabels(ceres::Problem& problem,
+                             const std::map<const double*, std::string>& names,
+                             const std::map<const double*, std::vector<std::string>>& labels,
+                             const std::map<const double*, std::vector<int>>& masks)
+{
+  std::vector<double *> blocks;
+  problem.GetParameterBlocks(&blocks);
+
+  // Extract optimization labels
+  for (double* b : blocks)
+  {
+    std::vector<std::string> sub_label;
+    if (!problem.IsParameterBlockConstant(b))
+    {
+      std::vector<std::string> label = labels.at(b);
+      const std::vector<int>& mask = masks.at(b);
+      if (mask.empty())
+      {
+        sub_label = label;
+      }
+      else
+      {
+        sub_label.reserve(label.size());
+        for (std::size_t j = 0; j < label.size(); ++j)
+        {
+          if (std::find(mask.begin(), mask.end(), j) == mask.end())
+            sub_label.push_back(label.at(j));
+        }
+      }
+    }
+    printLabels(names.at(b), sub_label);
+  }
+}
+
 KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D &params)
 {
   // Initialize the optimization variables
@@ -102,8 +150,9 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D &param
                                                t_ccb_to_tcb,
                                                aa_ccb_to_tcb);
 
-  std::vector<std::vector<std::string>> param_labels
-      = DualDHChainCost2D3D::constructParameterLabels(camera_chain_param_labels,
+  std::map<const double *, std::vector<std::string>> param_labels
+      = DualDHChainCost2D3D::constructParameterLabels(parameters,
+                                                      camera_chain_param_labels,
                                                       target_chain_param_labels,
                                                       t_cm_to_c_labels,
                                                       aa_cm_to_c_labels,
@@ -112,6 +161,13 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D &param
                                                       t_ccb_to_tcb_labels,
                                                       aa_ccb_to_tcb_labels);
 
+  std::map<const double *, std::vector<int>> param_masks
+      = DualDHChainCost2D3D::constructParameterMasks(parameters, params.mask);
+
+  std::map<const double *, std::string> param_names
+      = DualDHChainCost2D3D::constructParameterNames(parameters);
+
+
   // Set up the problem
   ceres::Problem problem;
 
@@ -163,9 +219,7 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D &param
     problem.SetParameterBlockConstant(target_chain_dh_offsets.data());
 
   // Add subset parameterization to mask variables that shouldn't be optimized
-  std::array<double*, 8> tmp;
-  std::copy_n(parameters.begin(), tmp.size(), tmp.begin());
-  addSubsetParameterization(problem, params.mask, tmp);
+  addSubsetParameterization(problem, param_masks);
 
   // Add a cost to drive the camera chain DH parameters towards an expected mean
   if (params.camera_chain.dof() != 0 && !problem.IsParameterBlockConstant(parameters[0]))
@@ -203,6 +257,9 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D &param
     problem.AddResidualBlock(cost_block, nullptr, target_chain_dh_offsets.data());
   }
 
+  // Print optimization parameter labels
+  printOptimizationLabels(problem, param_names, param_labels, param_masks);
+
   // Setup the Ceres optimization parameters
   ceres::Solver::Options options;
   options.max_num_iterations = 150;
@@ -238,7 +295,8 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D &param
 
   ceres::Covariance::Options cov_options = rct_optimizations::DefaultCovarianceOptions();
   cov_options.null_space_rank = -1;  // automatically drop terms below min_reciprocal_condition_number
-  result.covariance = computeCovariance(problem, std::vector<const double*>(parameters.begin(), parameters.end()), param_labels, cov_options);
+
+  result.covariance = computeCovariance(problem, param_labels, param_masks, cov_options);
 
   return result;
 }
@@ -324,8 +382,9 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblemPose6D &par
                                                  t_ccb_to_tcb,
                                                  aa_ccb_to_tcb);
 
-  std::vector<std::vector<std::string>> param_labels
-      = DualDHChainCostPose6D::constructParameterLabels(camera_chain_param_labels,
+  std::map<const double*, std::vector<std::string>> param_labels
+      = DualDHChainCostPose6D::constructParameterLabels(parameters,
+                                                        camera_chain_param_labels,
                                                         target_chain_param_labels,
                                                         t_cm_to_c_labels,
                                                         aa_cm_to_c_labels,
@@ -334,6 +393,13 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblemPose6D &par
                                                         t_ccb_to_tcb_labels,
                                                         aa_ccb_to_tcb_labels);
 
+  std::map<const double *, std::vector<int>> param_masks
+      = DualDHChainCostPose6D::constructParameterMasks(parameters, params.mask);
+
+  std::map<const double *, std::string> param_names
+      = DualDHChainCost2D3D::constructParameterNames(parameters);
+
+
   // Set up the problem
   ceres::Problem problem;
 
@@ -376,9 +442,7 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblemPose6D &par
     problem.SetParameterBlockConstant(target_chain_dh_offsets.data());
 
   // Add subset parameterization to mask variables that shouldn't be optimized
-  std::array<double*, 8> tmp;
-  std::copy_n(parameters.begin(), tmp.size(), tmp.begin());
-  addSubsetParameterization(problem, params.mask, tmp);
+  addSubsetParameterization(problem, param_masks);
 
   // Add a cost to drive the camera chain DH parameters towards an expected mean
   if (params.camera_chain.dof() != 0 && !problem.IsParameterBlockConstant(parameters[0]))
@@ -416,6 +480,9 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblemPose6D &par
     problem.AddResidualBlock(cost_block, nullptr, target_chain_dh_offsets.data());
   }
 
+  // Print optimization parameter labels
+  printOptimizationLabels(problem, param_names, param_labels, param_masks);
+
   // Solve the optimization
   ceres::Solver::Summary summary;
   ceres::Solve(options, &problem, &summary);
@@ -444,7 +511,8 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblemPose6D &par
 
   ceres::Covariance::Options cov_options = rct_optimizations::DefaultCovarianceOptions();
   cov_options.null_space_rank = -1;  // automatically drop terms below min_reciprocal_condition_number
-  result.covariance = computeCovariance(problem, std::vector<const double*>(parameters.begin(), parameters.end()), param_labels, cov_options);
+
+  result.covariance = computeCovariance(problem, param_labels, param_masks, cov_options);
 
   return result;
 }
diff --git a/rct_optimizations/src/rct_optimizations/extrinsic_hand_eye.cpp b/rct_optimizations/src/rct_optimizations/extrinsic_hand_eye.cpp
index cc17e295..217bc889 100644
--- a/rct_optimizations/src/rct_optimizations/extrinsic_hand_eye.cpp
+++ b/rct_optimizations/src/rct_optimizations/extrinsic_hand_eye.cpp
@@ -227,9 +227,9 @@ ExtrinsicHandEyeResult optimize(const ExtrinsicHandEyeProblem2D3D& params)
     labels_target_mount_to_target.emplace_back(params.label_target_mount_to_target + "_" + label_isometry);
   }
 
-  std::vector<std::vector<std::string>> param_labels;
-  param_labels.push_back(labels_camera_mount_to_camera);
-  param_labels.push_back(labels_target_mount_to_target);
+  std::map<const double*, std::vector<std::string>> param_labels;
+  param_labels[internal_camera_to_wrist.values.data()] = labels_camera_mount_to_camera;
+  param_labels[internal_base_to_target.values.data()] = labels_target_mount_to_target;
 
   result.covariance = rct_optimizations::computeCovariance(problem, param_labels);
 
@@ -289,9 +289,9 @@ ExtrinsicHandEyeResult optimize(const ExtrinsicHandEyeProblem3D3D& params)
     labels_target_mount_to_target.emplace_back(params.label_target_mount_to_target + "_" + label_isometry);
   }
 
-  std::vector<std::vector<std::string>> param_labels;
-  param_labels.push_back(labels_camera_mount_to_camera);
-  param_labels.push_back(labels_target_mount_to_target);
+  std::map<const double*, std::vector<std::string>> param_labels;
+  param_labels[internal_camera_to_wrist.values.data()] = labels_camera_mount_to_camera;
+  param_labels[internal_base_to_target.values.data()] = labels_target_mount_to_target;
 
   result.covariance = rct_optimizations::computeCovariance(problem, param_labels);
 
diff --git a/rct_optimizations/src/rct_optimizations/extrinsic_multi_static_camera.cpp b/rct_optimizations/src/rct_optimizations/extrinsic_multi_static_camera.cpp
index 8791029d..209d1bd3 100644
--- a/rct_optimizations/src/rct_optimizations/extrinsic_multi_static_camera.cpp
+++ b/rct_optimizations/src/rct_optimizations/extrinsic_multi_static_camera.cpp
@@ -122,7 +122,7 @@ rct_optimizations::optimize(const rct_optimizations::ExtrinsicMultiStaticCameraM
   }
   param_blocks.push_back(internal_wrist_to_target.values.data());
 
-  std::vector<std::vector<std::string>> param_labels;
+  std::map<const double*, std::vector<std::string>> param_labels;
 
   // compose labels "camera_mount_to_camera_x", etc.
   for (std::size_t index_camera = 0; index_camera < internal_camera_to_base.size(); index_camera++)
@@ -132,7 +132,7 @@ rct_optimizations::optimize(const rct_optimizations::ExtrinsicMultiStaticCameraM
     {
       labels_base_to_camera.emplace_back(params.label_base_to_camera + std::to_string(index_camera) + "_" + label_isometry);
     }
-    param_labels.push_back(labels_base_to_camera);
+    param_labels[internal_camera_to_base[index_camera].values.data()] = labels_base_to_camera;
   }
 
   // compose labels "target_mount_to_target_x", etc.
@@ -141,7 +141,7 @@ rct_optimizations::optimize(const rct_optimizations::ExtrinsicMultiStaticCameraM
   {
     labels_wrist_to_target.emplace_back(params.label_wrist_to_target + "_" + label_isometry);
   }
-  param_labels.push_back(labels_wrist_to_target);
+  param_labels[internal_wrist_to_target.values.data()] = labels_wrist_to_target;
 
   result.covariance = rct_optimizations::computeCovariance(problem, param_blocks, param_labels);
 
diff --git a/rct_optimizations/src/rct_optimizations/pnp.cpp b/rct_optimizations/src/rct_optimizations/pnp.cpp
index 6f3deb86..87c72bf6 100644
--- a/rct_optimizations/src/rct_optimizations/pnp.cpp
+++ b/rct_optimizations/src/rct_optimizations/pnp.cpp
@@ -130,7 +130,9 @@ PnPResult optimize(const PnPProblem &params)
   {
     labels_camera_to_target_guess_quaternion.emplace_back(params.label_camera_to_target_guess + "_" + label_r);
   }
-  std::vector<std::vector<std::string>> param_labels = { labels_camera_to_target_guess_translation, labels_camera_to_target_guess_quaternion };
+  std::map<const double*, std::vector<std::string>> param_labels;
+  param_labels[cam_to_tgt_translation.data()] = labels_camera_to_target_guess_translation;
+  param_labels[cam_to_tgt_angle_axis.data()] = labels_camera_to_target_guess_quaternion;
 
   result.covariance = rct_optimizations::computeCovariance(problem,
                                                            std::vector<const double *>({cam_to_tgt_translation.data(), cam_to_tgt_angle_axis.data()}),
@@ -186,7 +188,9 @@ PnPResult optimize(const rct_optimizations::PnPProblem3D& params)
   {
     labels_camera_to_target_guess_quaternion.emplace_back(params.label_camera_to_target_guess + "_" + label_r);
   }
-  std::vector<std::vector<std::string>> param_labels = { labels_camera_to_target_guess_translation, labels_camera_to_target_guess_quaternion };
+  std::map<const double*, std::vector<std::string>> param_labels;
+  param_labels[cam_to_tgt_translation.data()] = labels_camera_to_target_guess_translation;
+  param_labels[cam_to_tgt_angle_axis.data()] = labels_camera_to_target_guess_quaternion;
 
   result.covariance = rct_optimizations::computeCovariance(problem,
                                                            std::vector<const double *>({cam_to_tgt_translation.data(), cam_to_tgt_angle_axis.data()}),
diff --git a/rct_optimizations/test/covariance_utest.cpp b/rct_optimizations/test/covariance_utest.cpp
index fa8225c8..b0899e0f 100644
--- a/rct_optimizations/test/covariance_utest.cpp
+++ b/rct_optimizations/test/covariance_utest.cpp
@@ -559,8 +559,14 @@ TEST(CovarianceAnalysis, CovarianceAnalysisFunctions)
   EXPECT_NEAR(0.0, result.y_center, 1e-5);
   EXPECT_NEAR(1.0, result.radius, 1e-5);
 
-  std::vector<std::vector<std::string>> labels_all({{"circle_x"}, {"circle_y"}, {"circle_r"}});
-  std::vector<std::vector<std::string>> labels_x_r({{"circle_x"}, {"circle_r"}});
+  std::map<const double*, std::vector<std::string>> labels_all;
+  labels_all[params_internal.data()] = {"circle_x"};
+  labels_all[params_internal.data()+1] = {"circle_y"};
+  labels_all[params_internal.data()+2] = {"circle_r"};
+
+  std::map<const double*, std::vector<std::string>> labels_x_r;
+  labels_x_r[params_internal.data()] = {"circle_x"};
+  labels_x_r[params_internal.data()+2] = {"circle_r"};
 
   std::vector<const double*> param_blocks_x_r(2);
   param_blocks_x_r[0] = params_internal.data();
@@ -627,14 +633,16 @@ TEST(CovarianceAnalysis, CovarianceAnalysisFunctions)
   EXPECT_THROW(rct_optimizations::computeCovariance(problem, param_blocks_x_r, labels_all), rct_optimizations::CovarianceException);
 
   // expect exception if number of labels for a parameter block is different from the number of parameters in that block in the problem
-  std::vector<std::vector<std::string>> labels_x_r_wrong_size({{"circle_x", "circle_x_extra_entry"}, {"circle_r"}});
+  std::map<const double*, std::vector<std::string>> labels_x_r_wrong_size;
+  labels_x_r_wrong_size[params_internal.data()] = {"circle_x", "circle_x_extra_entry"};
+  labels_x_r_wrong_size[params_internal.data()+2] = {"circle_r"};
   EXPECT_THROW(rct_optimizations::computeCovariance(problem, param_blocks_x_r, labels_x_r_wrong_size), rct_optimizations::CovarianceException);
 
   // expect exception with empty parameter block vector
   EXPECT_THROW(rct_optimizations::computeCovariance(problem, std::vector<const double*>(), labels_all), rct_optimizations::CovarianceException);
 
   // expect exception with empty labels vector
-  EXPECT_THROW(rct_optimizations::computeCovariance(problem, param_blocks_x_r, std::vector<std::vector<std::string>>()), rct_optimizations::CovarianceException);
+  EXPECT_THROW(rct_optimizations::computeCovariance(problem, param_blocks_x_r, std::map<const double*, std::vector<std::string>>()), rct_optimizations::CovarianceException);
 }
 
 int main(int argc, char **argv)
diff --git a/rct_optimizations/test/local_parameterization_utest.cpp b/rct_optimizations/test/local_parameterization_utest.cpp
index 49b6e6c8..908d9f65 100644
--- a/rct_optimizations/test/local_parameterization_utest.cpp
+++ b/rct_optimizations/test/local_parameterization_utest.cpp
@@ -32,19 +32,12 @@ TEST(LocalParameterizationTests, SubsetParameterization)
   cost_block->SetNumResiduals(params.size());
   problem.AddResidualBlock(cost_block, nullptr, params.data());
 
-  // Wrong number of vectors
-  {
-    std::array<std::vector<int>, 3> mask;
-    EXPECT_THROW(addSubsetParameterization(problem, mask, {{ params.data() }}), OptimizationException);
-    EXPECT_EQ(problem.GetParameterization(params.data()), nullptr);
-  }
-
   // All values
   {
-    std::array<std::vector<int>, 1> mask;
-    mask.at(0).resize(params.size());
-    std::iota(mask.at(0).begin(), mask.at(0).end(), 0);
-    EXPECT_NO_THROW(addSubsetParameterization(problem, mask, {{ params.data() }}));
+    std::map<const double*, std::vector<int>> mask;
+    mask[params.data()] = std::vector<int>(params.size());
+    std::iota(mask[params.data()].begin(), mask[params.data()].end(), 0);
+    EXPECT_NO_THROW(addSubsetParameterization(problem, mask));
 
     // Expect there to be no parameterization, but the entire block should be constant
     EXPECT_EQ(problem.GetParameterization(params.data()), nullptr);
@@ -53,17 +46,18 @@ TEST(LocalParameterizationTests, SubsetParameterization)
 
   // Index out of range
   {
-    std::array<std::vector<int>, 1> mask;
+    std::map<const double*, std::vector<int>> mask;
+    mask[params.data()] = std::vector<int>(params.size());
     int bad_idx = params.size() * 2;
-    mask.at(0).insert(mask.at(0).begin(), { bad_idx, 0, 1, 2 });
-    EXPECT_THROW(addSubsetParameterization(problem, mask, {{ params.data() }}), OptimizationException);
+    mask[params.data()].insert(mask[params.data()].begin(), { bad_idx, 0, 1, 2 });
+    EXPECT_THROW(addSubsetParameterization(problem, mask), OptimizationException);
     EXPECT_EQ(problem.GetParameterization(params.data()), nullptr);
   }
 
   // Empty mask
   {
-    std::array<std::vector<int>, 1> mask;
-    EXPECT_NO_THROW(addSubsetParameterization(problem, mask, {{ params.data() }}));
+    std::map<const double*, std::vector<int>> mask;
+    EXPECT_NO_THROW(addSubsetParameterization(problem, mask));
     // An empty mask should not have added any local parameterization
     EXPECT_EQ(problem.GetParameterization(params.data()), nullptr);
   }
@@ -71,13 +65,14 @@ TEST(LocalParameterizationTests, SubsetParameterization)
   // Hold the zero-th row constant
   Eigen::ArrayXXd original_params = params;
   {
-    std::array<std::vector<int>, 1> mask;
+    std::map<const double*, std::vector<int>> mask;
+    mask[params.data()] = std::vector<int>();
     // Remember, Eigen stores values internally in column-wise order
     for (Eigen::Index i = 0; i < params.cols(); ++i)
     {
-      mask.at(0).push_back(i * params.rows());
+      mask[params.data()].push_back(i * params.rows());
     }
-    EXPECT_NO_THROW(addSubsetParameterization(problem, mask, {{ params.data() }}));
+    EXPECT_NO_THROW(addSubsetParameterization(problem, mask));
     EXPECT_NE(problem.GetParameterization(params.data()), nullptr);
   }
 
diff --git a/rct_optimizations/test/src/dh_chain_observation_creator.cpp b/rct_optimizations/test/src/dh_chain_observation_creator.cpp
index 520d2dd0..380f7a97 100644
--- a/rct_optimizations/test/src/dh_chain_observation_creator.cpp
+++ b/rct_optimizations/test/src/dh_chain_observation_creator.cpp
@@ -114,7 +114,7 @@ KinObservation2D3D::Set createKinematicObservations(const DHChain &to_camera_cha
         continue;
       }
 
-      if (obs.correspondence_set.size() > 0)
+      if (!obs.correspondence_set.empty())
       {
         observations.push_back(obs);
         correspondences += obs.correspondence_set.size();
diff --git a/rct_ros_tools/CMakeLists.txt b/rct_ros_tools/CMakeLists.txt
index 7417a014..a7609838 100644
--- a/rct_ros_tools/CMakeLists.txt
+++ b/rct_ros_tools/CMakeLists.txt
@@ -3,6 +3,7 @@ project(rct_ros_tools)
 
 add_compile_options(-std=c++11 -Wall -Wextra)
 
+find_package(rct_common REQUIRED)
 find_package(rct_optimizations REQUIRED)
 find_package(rct_image_tools REQUIRED)
 
@@ -47,6 +48,13 @@ catkin_package(
     image_transport
     std_srvs
     pluginlib
+  DEPENDS
+    rct_common
+    rct_optimizations
+    rct_image_tools
+    OpenCV
+    Eigen3
+    yaml-cpp
 )
 
 ###########
@@ -71,6 +79,7 @@ target_link_libraries(${PROJECT_NAME}
  ${OpenCV_LIBRARIES}
  rct::rct_optimizations
  rct::rct_image_tools
+ rct::rct_common
 )
 
 add_library(${PROJECT_NAME}_target_loader_plugins
diff --git a/rct_ros_tools/include/rct_ros_tools/loader_utils.h b/rct_ros_tools/include/rct_ros_tools/loader_utils.h
index 208b82dd..a98fa65b 100644
--- a/rct_ros_tools/include/rct_ros_tools/loader_utils.h
+++ b/rct_ros_tools/include/rct_ros_tools/loader_utils.h
@@ -1,6 +1,6 @@
 #pragma once
 
-#include <yaml-cpp/node/node.h>
+#include <yaml-cpp/yaml.h>
 #include <xmlrpcpp/XmlRpcValue.h>
 
 namespace rct_ros_tools
diff --git a/rct_ros_tools/package.xml b/rct_ros_tools/package.xml
index 26a5dfaa..a5d9f04d 100644
--- a/rct_ros_tools/package.xml
+++ b/rct_ros_tools/package.xml
@@ -14,7 +14,9 @@
   <depend>cv_bridge</depend>
   <depend>image_transport</depend>
   <depend>pluginlib</depend>
+  <depend>rct_common</depend>
   <depend>rct_image_tools</depend>
+  <depend>rct_optimizations</depend>
   <depend>roscpp</depend>
   <depend>std_srvs</depend>
   <depend>tf2_ros</depend>