Moving maplab_node to public devel

.gitmodules

@@ -4,3 +4,6 @@
 [submodule "aslam_cv2"]
 	path = aslam_cv2
 	url = https://github.com/ethz-asl/aslam_cv2.git
+[submodule "tools/evaluation_tools"]
+	path = tools/evaluation_tools
+	url = https://github.com/ethz-asl/evaluation_tools.git

README.md

@@ -1,6 +1,13 @@
-<img src="https://github.com/ethz-asl/maplab/wiki/logos/maplab.png" width="500">
+<img src="https://github.com/ethz-asl/maplab/wiki/logos/maplab_new.png" width="500">
 
-*Ubuntu 14.04+ROS indigo* and *Ubuntu 16.04+ROS kinetic*: [![Build Status](https://jenkins.asl.ethz.ch/buildStatus/icon?job=maplab_nightly)](https://jenkins.asl.ethz.ch/job/maplab_nightly)
+*Ubuntu 14.04+ROS indigo*, *Ubuntu 16.04+ROS kinetic* and *Ubuntu 18.04+ROS melodic*: [![Build Status](https://jenkins.asl.ethz.ch/buildStatus/icon?job=maplab_nightly)](https://jenkins.asl.ethz.ch/job/maplab_nightly)
+
+## News
+
+ * **May 2018:** maplab was presented at [ICRA](https://icra2018.org/) in Brisbane. ([paper](https://arxiv.org/abs/1711.10250))
+ * **March 2018:** Check out our release candidate with improved localization and lots of new features! [PR](https://github.com/ethz-asl/maplab/pull/55)
+
+## Description
 
 This repository contains **maplab**,  an  open,  research-oriented visual-inertial  mapping  framework, written  in  C++,  for  creating, processing  and  manipulating  multi-session  maps.
 On  the  one  hand, maplab  can  be  considered  as  a  ready-to-use  visual-inertial mapping  and  localization  system.
@@ -12,8 +19,7 @@ For documentation, tutorials and datasets, please visit the [wiki](https://githu
 
 Please also check out our video:
 
-<a href="https://www.youtube.com/watch?v=CrfG4v25B8k">
-<img src="https://github.com/ethz-asl/maplab/wiki/readme_images/maplab_video_thumbnail.png" alt="https://www.youtube.com/watch?v=CrfG4v25B8k" width="600">
+<a href="https://www.youtube.com/watch?v=CrfG4v25B8k"> <img src="https://github.com/ethz-asl/maplab/wiki/readme_images/maplab_video_thumbnail.png" alt="https://www.youtube.com/watch?v=CrfG4v25B8k" width="600">
 
 ## Features
 

algorithms/ceres-error-terms/CMakeLists.txt

@@ -6,10 +6,11 @@ link_directories(${CATKIN_DEVEL_PREFIX}/lib)
 find_package(catkin_simple REQUIRED)
 catkin_simple(ALL_DEPS_REQUIRED)
 
-add_definitions(-std=c++11 -Wno-enum-compare)
+add_definitions(-Wno-enum-compare)
 
 cs_add_library(${PROJECT_NAME}
   src/block-pose-prior-error-term.cc
+  src/block-pose-prior-error-term-v2.cc
   src/ceres-signal-handler.cc
   src/inertial-error-term.cc
   src/inertial-error-term-eigen.cc

algorithms/ceres-error-terms/include/ceres-error-terms/block-pose-prior-error-term-v2-inl.h

@@ -0,0 +1,58 @@
+#ifndef CERES_ERROR_TERMS_BLOCK_POSE_PRIOR_ERROR_TERM_V2_INL_H_
+#define CERES_ERROR_TERMS_BLOCK_POSE_PRIOR_ERROR_TERM_V2_INL_H_
+
+namespace ceres_error_terms {
+
+template <typename T>
+bool BlockPosePriorErrorTermV2::operator()(
+    const T* const q_G_M_jpl_p_G_M, const T* const q_B_M_jpl_p_M_B,
+    const T* const q_S_B_jpl_p_S_B, T* residuals) const {
+  // Define Quaternion and Position of type T.
+  typedef typename Eigen::Matrix<T, 3, 3> RotationMatrixT;
+  typedef aslam::PositionTemplate<T> PositionT;
+
+  const Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_G_M_map(
+      q_G_M_jpl_p_G_M + kOrientationBlockSize);
+  const Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_M_B_map(
+      q_B_M_jpl_p_M_B + kOrientationBlockSize);
+  const Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_S_B_map(
+      q_S_B_jpl_p_S_B + kOrientationBlockSize);
+
+  const Eigen::Map<const Eigen::Matrix<T, 4, 1>> q_G_M_jpl_map(q_G_M_jpl_p_G_M);
+  RotationMatrixT R_G_M_jpl;
+  common::toRotationMatrixJPL(q_G_M_jpl_map, &R_G_M_jpl);
+  const RotationMatrixT R_G_M = R_G_M_jpl;
+
+  const Eigen::Map<const Eigen::Matrix<T, 4, 1>> q_B_M_jpl_map(q_B_M_jpl_p_M_B);
+  RotationMatrixT R_B_M_jpl;
+  common::toRotationMatrixJPL(q_B_M_jpl_map, &R_B_M_jpl);
+  const RotationMatrixT R_M_B = R_B_M_jpl.transpose();
+
+  const Eigen::Map<const Eigen::Matrix<T, 4, 1>> q_S_B_jpl_map(q_S_B_jpl_p_S_B);
+  RotationMatrixT R_S_B_jpl;
+  common::toRotationMatrixJPL(q_S_B_jpl_map, &R_S_B_jpl);
+  const RotationMatrixT R_B_S = R_S_B_jpl.transpose();
+
+  const RotationMatrixT R_G_S = R_G_M * R_M_B * R_B_S;
+
+  // TODO(mfehr): DOUBLE CHECK IF THIS IS CORRECT!!
+  const Eigen::Matrix<T, 3, 1> p_B_S = -(R_B_S * p_S_B_map);
+
+  const PositionT p_G_S = R_G_M * R_M_B * p_B_S + R_G_M * p_M_B_map + p_G_M_map;
+
+  const Eigen::Quaternion<T> q_G_S_estimated(R_G_S);
+  const Eigen::Quaternion<T> q_S_G_measured(q_S_G_measured_);
+  const Eigen::Quaternion<T> q_diff = q_G_S_estimated * q_S_G_measured;
+
+  Eigen::Map<Eigen::Matrix<T, kResidualBlockSize, 1>> error(residuals);
+  error.head(3) = p_G_S - p_G_S_measured_.cast<T>();
+  error.tail(3) = Eigen::Matrix<T, 3, 1>(
+      static_cast<T>(2.0) * q_diff.x(), static_cast<T>(2.0) * q_diff.y(),
+      static_cast<T>(2.0) * q_diff.z());
+  error = error.transpose() * sqrt_information_matrix_.cast<T>();
+
+  return true;
+}
+
+}  // namespace ceres_error_terms
+#endif  // CERES_ERROR_TERMS_BLOCK_POSE_PRIOR_ERROR_TERM_V2_INL_H_

algorithms/ceres-error-terms/include/ceres-error-terms/block-pose-prior-error-term-v2.h

@@ -0,0 +1,57 @@
+#ifndef CERES_ERROR_TERMS_BLOCK_POSE_PRIOR_ERROR_TERM_V2_H_
+#define CERES_ERROR_TERMS_BLOCK_POSE_PRIOR_ERROR_TERM_V2_H_
+
+#include <memory>
+
+#include <Eigen/Core>
+#include <aslam/common/pose-types.h>
+#include <ceres-error-terms/common.h>
+#include <ceres-error-terms/parameterization/quaternion-param-jpl.h>
+#include <ceres/ceres.h>
+#include <ceres/sized_cost_function.h>
+#include <maplab-common/quaternion-math.h>
+
+namespace ceres_error_terms {
+
+// Note: this error term accepts rotations expressed as quaternions
+// in JPL convention [x, y, z, w]. This convention corresponds to the internal
+// coefficient storage of Eigen so you can directly pass pointer to your
+// Eigen quaternion data, e.g. your_eigen_quaternion.coeffs().data().
+class BlockPosePriorErrorTermV2 {
+ public:
+  BlockPosePriorErrorTermV2(
+      const aslam::Transformation& T_G_S_measured,
+      const Eigen::Matrix<double, 6, 6>& covariance);
+
+  ~BlockPosePriorErrorTermV2() {}
+
+  template <typename T>
+  bool operator()(
+      const T* const q_G_M_jpl_p_G_M, const T* const q_B_M_jpl_p_M_B,
+      const T* const q_S_B_jpl_p_S_B, T* residuals) const;
+
+  static constexpr int kResidualBlockSize = 6;
+  static constexpr int kOrientationBlockSize = 4;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ private:
+  // Don't change the ordering of the enum elements, they have to be the
+  // same as the order of the parameter blocks.
+  enum {
+    kIdxBaseframePose,
+    kIdxVertexPose,
+    kIdxSensorPose,
+  };
+
+  Eigen::Matrix<double, 6, 6> sqrt_information_matrix_;
+
+  Eigen::Quaterniond q_S_G_measured_;
+  Eigen::Vector3d p_G_S_measured_;
+};
+
+}  // namespace ceres_error_terms
+
+#include "ceres-error-terms/block-pose-prior-error-term-v2-inl.h"
+
+#endif  // CERES_ERROR_TERMS_BLOCK_POSE_PRIOR_ERROR_TERM_V2_H_

algorithms/ceres-error-terms/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <package format="2">
   <name>ceres_error_terms</name>
-  <version>0.0.0</version>
+  <version>2.2.0</version>
   <description>The ceres_error_terms package</description>
   <maintainer email="[email protected]">maplab-developers</maintainer>
   <license>Apache 2.0</license>

algorithms/ceres-error-terms/src/block-pose-prior-error-term-v2.cc

@@ -0,0 +1,29 @@
+#include <memory>
+
+#include <Eigen/Core>
+#include <aslam/common/pose-types.h>
+#include <ceres-error-terms/common.h>
+#include <ceres-error-terms/parameterization/quaternion-param-jpl.h>
+#include <ceres/ceres.h>
+#include <ceres/sized_cost_function.h>
+
+#include "ceres-error-terms/block-pose-prior-error-term-v2.h"
+
+namespace ceres_error_terms {
+
+BlockPosePriorErrorTermV2::BlockPosePriorErrorTermV2(
+    const aslam::Transformation& T_G_S_measured,
+    const Eigen::Matrix<double, 6, 6>& covariance)
+    : q_S_G_measured_(
+          T_G_S_measured.getRotation().toImplementation().inverse()),
+      p_G_S_measured_(T_G_S_measured.getPosition()) {
+  // Getting inverse square root of covariance matrix.
+  Eigen::Matrix<double, 6, 6> L = covariance.llt().matrixL();
+  sqrt_information_matrix_.setIdentity();
+  L.triangularView<Eigen::Lower>().solveInPlace(sqrt_information_matrix_);
+
+  // inverse_orientation_prior_ =
+  //    Eigen::Quaterniond(orientation_prior_).inverse().coeffs();
+}
+
+}  // namespace ceres_error_terms

algorithms/ceres-error-terms/src/ceres-signal-handler.cc

@@ -1,35 +1,45 @@
 #include "ceres-error-terms/ceres-signal-handler.h"
 
 #include <signal.h>
+#include <vector>
 
 #include <ceres/types.h>
 
 namespace ceres_error_terms {
 
-SignalHandlerCallback* signal_handler_link = nullptr;
+std::vector<SignalHandlerCallback*> signal_handler_links;
 
 SignalHandlerCallback::SignalHandlerCallback()
     : terminate_after_next_iteration_(false) {
-  CHECK(signal_handler_link == nullptr)
-      << "There is already a SignalHandlerCallback registered.";
   // Install a temporary SIGINT handler to abort the current optimization.
-  signal_handler_link = this;
+  signal_handler_links.push_back(this);
   sigaction(SIGINT, nullptr, &previous_signal_handler_);
   signal(SIGINT, static_cast<sig_t>(&signalHandler));
 }
 
 SignalHandlerCallback::~SignalHandlerCallback() {
   // Restore the original signal handler.
   signal(SIGINT, previous_signal_handler_.sa_handler);
-  signal_handler_link = nullptr;
+
+  // Remove the pointer to this signal hander from the global handler links.
+  auto it = signal_handler_links.begin();
+  while (it != signal_handler_links.end()) {
+    if (*it == this) {
+      it = signal_handler_links.erase(it);
+    } else {
+      ++it;
+    }
+  }
 }
 
 void SignalHandlerCallback::signalHandler(int signal) {
   CHECK_EQ(signal, SIGINT);
-  CHECK(signal_handler_link != nullptr)
-      << "No SignalHandlerCallback registered.";
+  CHECK(!signal_handler_links.empty())
+      << "Signal was received, but no signal handler was registered!";
 
-  signal_handler_link->terminate_after_next_iteration_ = true;
+  for (SignalHandlerCallback* callback : signal_handler_links) {
+    callback->terminate_after_next_iteration_ = true;
+  }
   LOG(WARNING)
       << "User requested optimization termination after next update...";
 }

interfaces/voxblox_interface/CMakeLists.txt → algorithms/dense-reconstruction/depth_integration/CMakeLists.txt

@@ -1,22 +1,20 @@
 cmake_minimum_required (VERSION 2.8)
-project(voxblox_interface)
+project(depth_integration)
 
 find_package(catkin_simple REQUIRED)
 catkin_simple(ALL_DEPS_REQUIRED)
 
-add_definitions(--std=c++11)
-
-cs_add_library(${PROJECT_NAME} src/integration.cc)
+cs_add_library(${PROJECT_NAME} src/depth-integration.cc)
 
 SET(PROJECT_TEST_DATA "map_resources_test_data")
 add_custom_target(${PROJECT_TEST_DATA})
 add_custom_command(TARGET ${PROJECT_TEST_DATA}
 COMMAND rm -rf "./${PROJECT_TEST_DATA}/*" && tar -xvzf ${MAPLAB_TEST_DATA_DIR}/${PROJECT_TEST_DATA}/${PROJECT_TEST_DATA}.tar.gz)
 
-catkin_add_gtest(test_voxblox_interface test/test-voxblox-interface.cc)
-target_link_libraries(test_voxblox_interface ${PROJECT_NAME})
-maplab_import_test_maps(test_voxblox_interface)
-add_dependencies(test_voxblox_interface ${PROJECT_TEST_DATA})
+catkin_add_gtest(test_voxblox_depth_integration test/test-voxblox-depth-integration.cc)
+target_link_libraries(test_voxblox_depth_integration ${PROJECT_NAME})
+maplab_import_test_maps(test_voxblox_depth_integration)
+add_dependencies(test_voxblox_depth_integration ${PROJECT_TEST_DATA})
 
 cs_install()
 cs_export()

interfaces/voxblox_interface/README.md → algorithms/dense-reconstruction/depth_integration/README.md

@@ -1,4 +1,4 @@
-voxblox_interface
+depth_integration
 =================
 
 A collection of methods to integrate pointclouds into voxblox maps.