Merge pull request MIT-SPARK#18 from SPARK/feature/squashed_history

Feature/squashed history

.gitignore

@@ -0,0 +1,7 @@
+*.swp
+*.idea
+**/mesh_results/*
+**/tsdf_results/*
+*.ccls*
+*.vscode/
+*.ipynb_checkpoints/

README.md

@@ -1,7 +1,7 @@
 # Kimera-Semantics
 
 <div align="center">
-    <img src="kimera/docs/media/kimera_semantics.gif">
+    <img src="docs/media/kimera_semantics.gif">
 </div>
 
 ## Release News
@@ -12,15 +12,15 @@
   you can play with both methods by changing the parameter `semantic_tsdf_integrator_type` in the [launch file](./kimera_semantics_ros/launch/kimera_semantics.launch).
   [High-res video here.](https://www.youtube.com/watch?v=ex1oMByJtyQ&feature=share&fbclid=IwAR33TB2t2SEbGTAfUbCO8pKFmJTsTjBCtWf-TAluY93BlzfSUEQbbN3GITQ)
 <div align="center">
-    <img src="kimera/docs/media/fast_vs_merged_kimera_semantics.gif">
+    <img src="docs/media/fast_vs_merged_kimera_semantics.gif">
 </div>
 
 ## Publications
 
 We kindly ask to cite our paper if you find this library useful:
 
 - A. Rosinol, M. Abate, Y. Chang, L. Carlone, [**Kimera: an Open-Source Library for Real-Time Metric-Semantic Localization and Mapping**](https://arxiv.org/abs/1910.02490). IEEE Intl. Conf. on Robotics and Automation (ICRA), 2020. [arXiv:1910.02490](https://arxiv.org/abs/1910.02490).
- 
+
  ```bibtex
  @InProceedings{Rosinol20icra-Kimera,
    title = {Kimera: an Open-Source Library for Real-Time Metric-Semantic Localization and Mapping},
@@ -50,7 +50,7 @@ A related work to ours is [Voxblox++](https://github.com/ethz-asl/voxblox-pluspl
 
 ## A. Prerequisities
 
-- Install ROS by following [our reference](./kimera/docs/ros_installation.md), or the official [ROS website](https://www.ros.org/install/).
+- Install ROS by following [our reference](./docs/ros_installation.md), or the official [ROS website](https://www.ros.org/install/).
 
 - Install system dependencies:
 ```bash
@@ -82,10 +82,13 @@ git clone git@github.com:MIT-SPARK/Kimera-Semantics.git
 # Install dependencies from rosinstall file using wstool
 wstool init # Use unless wstool is already initialized
 
+# Optionally add Kimera-Semantics to the rosinstall file
+# wstool scrape
+
 # For ssh:
-wstool merge Kimera-Semantics/kimera/install/kimera_semantics_ssh.rosinstall
+wstool merge Kimera-Semantics/install/kimera_semantics_ssh.rosinstall
 # For https:
-#wstool merge Kimera-Semantics/kimera/install/kimera_semantics_https.rosinstall
+#wstool merge Kimera-Semantics/install/kimera_semantics_https.rosinstall
 
 # Download and update all dependencies
 wstool update
@@ -140,10 +143,10 @@ First, install Kimera-Semantics, see [instructions above](https://github.com/MIT
   roslaunch kimera_vio_ros kimera_vio_ros_euroc.launch run_stereo_dense:=true
   ```
    >  The flag `run_stereo_dense:=true` will do stereo dense reconstruction (using OpenCV's StereoBM algorithm).
-  
+
   4. In another terminal, launch Kimera-Semantics:
   ```bash
-  roslaunch kimera_semantics_ros kimera_semantics_euroc.launch 
+  roslaunch kimera_semantics_ros kimera_semantics_euroc.launch
   ```
   5. In yet another terminal, run the Euroc rosbag downloaded in step 0:
   ```bash
@@ -152,21 +155,21 @@ First, install Kimera-Semantics, see [instructions above](https://github.com/MIT
    > Note 1: Don't forget the `--clock` flag!
    >
    > Note 2: Kimera is so fast that you could also increase the rosbag rate by 3 `--rate 3` and still see a good performance (results depend on available compute power).
-   
+
   6. Finally, in another terminal, run Rviz for visualization:
   ```bash
-  rviz -d $(rospack find kimera_semantics_ros)/rviz/kimera_semantics_euroc.rviz 
+  rviz -d $(rospack find kimera_semantics_ros)/rviz/kimera_semantics_euroc.rviz
   ```
 
   # 3. FAQ
 
   - Minkindr doesn't compile:
-  
+
     Catkin ignore the `minkindr_python` catkin package:
     `touch ~/catkin_ws/src/minkindr/minkindr_python/CATKIN_IGNORE`
 
   - How to run Kimera-Semantics without Semantics?
-  
+
     We are using Voxblox as our 3D reconstruction library, therefore, to run without semantics, simply do:
     ```bash
     roslaunch kimera_semantics_ros kimera_semantics.launch play_bag:=true metric_semantic_reconstruction:=false
@@ -183,4 +186,3 @@ roslaunch kimera_semantics_ros kimera_semantics.launch run_stereo_dense:=1
 This will publish a `/points2` topic, which you can visualize in Rviz as a 3D pointcloud.
 Alternatively, if you want to visualize the depth image, since Rviz does not provide a plugin to
 visualize a [disparity image](http://docs.ros.org/api/stereo_msgs/html/msg/DisparityImage.html), we also run a [disparity_image_proc](https://github.com/ToniRV/disparity_image_proc) nodelet that will publish the depth image to `/depth_image`.
-

kimera/install/kimera_semantics_https.rosinstall → install/kimera_semantics_https.rosinstall

@@ -1,6 +1,9 @@
 - git:
     local-name: catkin_simple
     uri: https://github.com/catkin/catkin_simple.git
+- git:
+    local-name: disparity_image_proc
+    uri: https://github.com/ToniRV/disparity_image_proc.git
 - git:
     local-name: eigen_catkin
     uri: https://github.com/ethz-asl/eigen_catkin.git
@@ -19,21 +22,15 @@
 - git:
     local-name: minkindr_ros
     uri: https://github.com/ethz-asl/minkindr_ros.git
+- git:
+    local-name: numpy_eigen
+    uri: https://github.com/ethz-asl/numpy_eigen.git
 - git:
     local-name: protobuf_catkin
     uri: https://github.com/ethz-asl/protobuf_catkin.git
 - git:
     local-name: catkin_boost_python_buildtool
     uri: https://github.com/ethz-asl/catkin_boost_python_buildtool.git
-- git:
-    local-name: schweizer_messer
-    uri: https://github.com/ethz-asl/schweizer_messer.git
-- git:
-    local-name: pointcloud_recolor
-    uri: https://github.com/ToniRV/ROS_pointcloud_recolor.git
 - git:
     local-name: voxblox
     uri: https://github.com/ethz-asl/voxblox.git
-- git:
-    local-name: Kimera-Semantics
-    uri: https://github.com/MIT-SPARK/Kimera-Semantics.git

kimera/install/kimera_semantics_ssh.rosinstall → install/kimera_semantics_ssh.rosinstall

@@ -1,6 +1,9 @@
 - git:
     local-name: catkin_simple
     uri: git@github.com:catkin/catkin_simple.git
+- git:
+    local-name: disparity_image_proc
+    uri: git@github.com:ToniRV/disparity_image_proc.git
 - git:
     local-name: eigen_catkin
     uri: git@github.com:ethz-asl/eigen_catkin.git
@@ -19,21 +22,15 @@
 - git:
     local-name: minkindr_ros
     uri: git@github.com:ethz-asl/minkindr_ros.git
+- git:
+    local-name: numpy_eigen
+    uri: git@github.com:ethz-asl/numpy_eigen.git
 - git:
     local-name: protobuf_catkin
     uri: git@github.com:ethz-asl/protobuf_catkin.git
 - git:
     local-name: catkin_boost_python_buildtool
     uri: git@github.com:ethz-asl/catkin_boost_python_buildtool.git
-- git:
-    local-name: schweizer_messer
-    uri: git@github.com:ethz-asl/schweizer_messer.git
-- git:
-    local-name: pointcloud_recolor
-    uri: git@github.com:ToniRV/ROS_pointcloud_recolor.git
 - git:
     local-name: voxblox
     uri: git@github.com:ethz-asl/voxblox.git
-- git:
-    local-name: Kimera-Semantics
-    uri: git@github.com:MIT-SPARK/Kimera-Semantics.git

kimera_semantics/include/kimera_semantics/color.h

@@ -65,7 +65,7 @@ inline SemanticLabelToColorMap getRandomSemanticLabelToColorMap() {
     semantic_label_color_map_[i] = vxb::randomColor();
   }
   // Make first colours easily distinguishable.
-  CHECK_GE(semantic_label_color_map_.size(), 8);
+  CHECK_GE(semantic_label_color_map_.size(), 8u);
   CHECK_GE(semantic_label_color_map_.size(), kTotalNumberOfLabels);
   // TODO(Toni): Check it Matches with default value for SemanticVoxel!
   semantic_label_color_map_.at(0) = HashableColor::Gray();    // Label unknown

kimera_semantics/include/kimera_semantics/common.h

@@ -16,6 +16,9 @@ namespace vxb = voxblox;
 
 typedef uint8_t SemanticLabel;
 typedef vxb::AlignedVector<SemanticLabel> SemanticLabels;
+// Consider id 0 to be the `unknown' label, for which we don't update the
+// log-likelihood for that measurement.
+static constexpr uint8_t kUnknownSemanticLabelId = 0u;
 // The size of this array determines how many semantic labels SemanticVoxblox
 // supports.
 // TODO(Toni): parametrize this, although that means it becomes unknown at
@@ -40,4 +43,5 @@ std::unique_ptr<T> make_unique(Args&&... args) {
   return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
 }
 
+
 }  // namespace kimera

kimera_semantics/include/kimera_semantics/macros.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include <memory>
+
+// These macros were inspired mainly on Maplab's macros
+// https://github.com/ethz-asl/maplab
+
+#define KIMERA_POINTER_TYPEDEFS(TypeName)                 \
+  typedef std::shared_ptr<TypeName> Ptr;                  \
+  typedef std::shared_ptr<const TypeName> ConstPtr;       \
+  typedef std::unique_ptr<TypeName> UniquePtr;            \
+  typedef std::unique_ptr<const TypeName> ConstUniquePtr; \
+  typedef std::weak_ptr<TypeName> WeakPtr;                \
+  typedef std::weak_ptr<const TypeName> WeakConstPtr;     \
+  void definePointerTypedefs##__FILE__##__LINE__(void)
+
+#define KIMERA_DELETE_COPY_CONSTRUCTORS(TypeName) \
+  TypeName(const TypeName&) = delete;             \
+  void operator=(const TypeName&) = delete

kimera_semantics/src/color.cpp

@@ -61,7 +61,9 @@ SemanticLabel2Color::SemanticLabel2Color(const std::string& filename)
   }
   // TODO(Toni): remove
   // Assign color 255,255,255 to unknown object 0u
-  color_to_semantic_label_[HashableColor::White()] = 0u;
+  semantic_label_to_color_map_[kUnknownSemanticLabelId] =
+      HashableColor::White();
+  color_to_semantic_label_[HashableColor::White()] = kUnknownSemanticLabelId;
 }
 
 SemanticLabel SemanticLabel2Color::getSemanticLabelFromColor(
@@ -71,11 +73,11 @@ SemanticLabel SemanticLabel2Color::getSemanticLabelFromColor(
     return it->second;
   } else {
     LOG(ERROR) << "Caught an unknown color: \n"
-               << "RGB: " << std::to_string(color.r) << ' '
+               << "RGBA: " << std::to_string(color.r) << ' '
                <<  std::to_string(color.g) << ' '
                <<  std::to_string(color.b) << ' '
                <<  std::to_string(color.a);
-    return 0u; // Assign unknown label for now...
+    return kUnknownSemanticLabelId; // Assign unknown label for now...
   }
 }
 

kimera_semantics/src/semantic_integrator_base.cpp

@@ -119,6 +119,12 @@ void SemanticIntegratorBase::setSemanticProbabilities() {
           std::pow(kTotalNumberOfLabels, 2) * log_non_match_probability_ -
           kTotalNumberOfLabels * log_non_match_probability_,
       1000 * vxb::kFloatEpsilon);
+  // Set the likelihood row for unknown label to 0, so to avoid integrating
+  // our knowledge that the voxel is unknown.
+  // Log(0) is -inf.
+  // Well, now, rather, we give uniform weight, saying that we don't know what
+  // is what.
+  semantic_log_likelihood_.col(kUnknownSemanticLabelId).setZero();
 }
 
 // TODO(Toni): Complete this function!!
@@ -354,6 +360,9 @@ void SemanticIntegratorBase::calculateMaximumLikelihoodLabel(
   CHECK(!semantic_posterior.hasNaN())
       << "Eigen's maxCoeff has undefined behaviour with NaNs, fix your "
          "posteriors.";
+  // TODO(Toni): if the semantic_label is still unknown, then, if the
+  // kUnknownSemanticLabel is set to something != 0, the 0 class will be
+  // unfairly shown as the class for this voxel, while in reality we don't know.
   semantic_posterior.maxCoeff(semantic_label);
 }
 

kimera_semantics/src/semantic_tsdf_integrator_merged.cpp

@@ -325,7 +325,6 @@ void MergedSemanticTsdfIntegrator::integrateVoxel(
                         &mutexes_,
                         voxel,
                         semantic_voxel);
-
   }
 }
 

kimera_semantics_ros/CMakeLists.txt

@@ -12,6 +12,8 @@ cs_add_library(${PROJECT_NAME}
   include/kimera_semantics_ros/semantic_simulation_server.h
   include/kimera_semantics_ros/semantic_tsdf_server.h
   src/ros_params.cpp
+  src/semantic_simulation_server.cpp
+  src/semantic_tsdf_server.cpp
   src/rosbag_data_provider.cpp
   src/semantic_simulation_server.cpp
   src/semantic_tsdf_server.cpp
@@ -30,7 +32,7 @@ target_link_libraries(kimera_semantics_rosbag ${PROJECT_NAME})
 cs_add_executable(semantic_simulator_eval
   src/semantic_simulation_eval.cpp
 )
-target_link_libraries(semantic_simulator_eval ${PROJECT_NAME})
+target_link_libraries(semantic_simulator_eval ${PROJECT_NAME} ${PCL_LIBRARIES})
 
 cs_install()