Restore stuff from origin/develop that was blown away during rebase

pano/README.md

@@ -148,4 +148,44 @@ Note that some of the individual steps within each panorama stitch (e.g., `enble
 
 ### View the tour
 
-In the host OS, point a web browser at `http://127.0.0.1:8080/`. The `run.sh` script forwards port 8080 of the host to port 80 of the container, where an Apache server is running to provide a preview of the tour.
+In the host OS, point a web browser at `http://127.0.0.1:8080/`. The `run.sh` script forwards port 8080 of the host to port 80 of the container, where an Apache server is running to provide a preview of the tour.
+
+### Install the tour
+
+Recursively copy the `$ISAAC_PANO_OUTPUT/html` folder wherever you want on your web server. Make sure permissions are set to enable your users to access files in the folder. No special configuration should be needed as long as the server is configured to serve static files with the appropriate MIME types. None of the application logic runs on the web server; the viewing interfaces run completely in the browser. When users make annotations, they will be stored locally in the user's web browser, not on the web server.
+
+### Generate target pose from image
+
+Once you have the json file exported you can generate the target pose where the attitude of the target is extracted based on the camera point of view at the time of the picture:
+(1) use the bagfile where that panorama was recorded to extract the point cloud and estimate target pose
+(2) use the mesh generated by the geometry mapper (optional)
+or both and compare results. To use the tool, you will first have to define `ASTROBEE_CONFIG_DIR`, `ASTROBEE_RESOURCE_DIR`, `ASTROBEE_ROBOT`, `ASTROBEE_WORLD`, and then run:
+
+    rosrun pano_view find_point_coordinate [OPTIONS]
+
+General parameters
+| Parameter            | Default value         | Description                                                                |
+| -------------------- | --------------------- | --------------------------------------------------------                   |
+| camera               | "sci_cam"             | Camera to use                                                              |
+| depth                | "haz"                 | Depth camera name                                                          |
+| json_config          | ""                    | json file with configure data                                              |
+| bag_name             | ""                    | Bagname where image can be found. Make sure it has haz cam and ground truth|
+| mesh_name            | ""                    | Meshfile path                                                              |
+| depth_cam_topic      | "/hw/depth_haz/points"| Point Cloud topic name                                                     |
+| ground_truth_topic   | "/gnc/ekf"            | Robot pose topic name                                                      |
+
+The script will print in the screen the target pose estimates.
+An example of this script would be:
+
+    rosrun pano_view find_point_coordinate -bag_name bag_name.bag -mesh_name mesh_name.obj -json_config test/viewpoint.json
+
+An example output would be:
+
+    Closest timestamp depth: 0.050772; Closest timestamp pose: 0.000275373
+    Point Cloud Point to vector distance: 0.00341976
+    Intersection point pcl: (0.997071, 0.238026, -0.571279)(0,4.71044,-14.8078)
+    Intersection point mesh: (0.993323, 0.237302, -0.5721)(0,4.71044,-14.8078)
+
+Things to look for are that the timestamps are not too large, this would mean that results are unreliable.
+Pay attention to the point cloud point to vector distance to make sure that the point is not too far away from the target vector, meaning that the point cloud does not include the target which is possible due to different field of views between the cameras.
+Lastly the mesh and pcl should not disagree, if they do, some manual analysis is needed. Be aware that the attitude provided is defined by the direction pointing at the target assuming roll zero.

pano/docker/pano.Dockerfile

@@ -13,17 +13,27 @@ RUN apt-get update \
   && apt-get install -y --no-install-recommends \
     apache2 \
     default-jre \
+    gfortran \
     hugin \
+    libfftw3-dev \
+    libopenblas-dev \
     libvips-tools \
     python3-pip \
   && rm -rf /var/lib/apt/lists/*
 
-# pandas: pulled in as pyshtools dependency but install breaks if not mentioned explicitly (?)
-# pyshtools: used during Pannellum multires generation
-# snakemake: modern build system based on Python, manages stitching workflows
+# Install Jupyter explicitly first
+RUN pip3 install --no-cache-dir --upgrade pip && \
+    pip3 install --no-cache-dir jupyter
+
+# Install other Python packages: jupyter package needs to be installed before attempting to build pyshtools
+# - pandas: pulled in as pyshtools dependency but install breaks if not mentioned explicitly (?)
+# - pyshtools: used during Pannellum multires generation
+# - snakemake: modern build system based on Python, manages stitching workflows
 RUN pip3 install --no-cache-dir --upgrade pip \
   && pip3 install --no-cache-dir \
+    --ignore-installed PyYAML \
     pandas \
+    pulp==2.7 \
     pyshtools \
     snakemake
 

pano/pano_stitch/scripts/config_panos.py

@@ -34,9 +34,10 @@
 import sys
 
 import numpy as np
-import pano_image_meta
 import yaml
 
+import pano_image_meta
+
 SCI_CAM_IMG_REGEX = re.compile(r"\d{10}\.\d{3}\.jpg$")
 
 ROBOT_REGEX = re.compile(r"(\b|_)(?P<robot>honey|bumble|queen)(\b|_)", re.IGNORECASE)

pano/pano_stitch/scripts/pano_image_meta.py

@@ -26,7 +26,7 @@
 import rosbag
 from tf.transformations import euler_from_quaternion
 
-IMAGE_TOPIC = ["/hw/cam_sci/compressed", "/hw/cam_sci_info"]
+IMAGE_TOPICS = ["/hw/cam_sci/compressed", "/hw/cam_sci_info"]
 POSE_TOPIC = "/loc/pose"
 FIELD_NAMES = (
     "timestamp",
@@ -50,8 +50,8 @@ def get_image_meta(inbag_path, num_images=None):
     images = []
     with rosbag.Bag(inbag_path) as bag:
         img_meta = None
-        for topic, msg, t in bag.read_messages([IMAGE_TOPIC, POSE_TOPIC]):
-            if topic == IMAGE_TOPIC:
+        for topic, msg, t in bag.read_messages([*IMAGE_TOPICS, POSE_TOPIC]):
+            if topic in IMAGE_TOPICS:
                 if num_images is not None and len(images) == num_images:
                     break
 

pano/pano_stitch/scripts/stitch_panorama.py

@@ -745,7 +745,9 @@ def main():
             "Preserving final metadata %s alongside output pano image"
             % os.path.basename(pto_final)
         )
-        shutil.copyfile(pto_final, output_dir)
+        shutil.copyfile(
+            pto_final, os.path.join(output_dir, os.path.basename(pto_final))
+        )
 
         print("\n=== Final stitched pano in %s ===\n" % final_png_path)
 

pano/pano_view/CMakeLists.txt

@@ -23,6 +23,68 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(pano_view)
 
-find_package(catkin REQUIRED COMPONENTS)
+## Compile as C++14, supported in ROS Kinetic and newer
+add_compile_options(-std=c++14)
+
+## Find catkin macros and libraries
+SET(catkin2_DIR "${CMAKE_SOURCE_DIR}/../../../cmake")
+find_package(catkin2 REQUIRED COMPONENTS
+    roscpp
+    rosbag
+    ff_msgs
+	camera
+	inspection
+	)
+
+
+find_package(PCL REQUIRED COMPONENTS common)
+find_package(pcl_conversions REQUIRED)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  CATKIN_DEPENDS
+    roscpp
+    rosbag
+    ff_msgs
+	camera
+	inspection
+)
+
+
+###########
+## Build ##
+###########
+
+# Specify additional locations of header files
+# Your package locations should be listed before other locations
+include_directories(
+	include
+  ${catkin_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+  ${pcl_conversions_INCLUDE_DIRS}
+)
+
+
+# Declare C++ libraries
+add_library(mesh_intersect
+  src/mesh_intersect.cc
+)
+add_dependencies(mesh_intersect ${catkin_EXPORTED_TARGETS})
+target_link_libraries(mesh_intersect ${catkin_LIBRARIES})
+
+# Declare C++ libraries
+add_library(point_cloud_intersect
+  src/point_cloud_intersect.cc
+)
+add_dependencies(point_cloud_intersect ${catkin_EXPORTED_TARGETS})
+target_link_libraries(point_cloud_intersect ${catkin_LIBRARIES})
+
+
+## Declare a C++ executable: find_point_coordinate
+add_executable(find_point_coordinate tools/find_point_coordinate.cc)
+add_dependencies(find_point_coordinate ${catkin_EXPORTED_TARGETS})
+target_link_libraries(find_point_coordinate mesh_intersect point_cloud_intersect
+gflags glog jsoncpp ${catkin_LIBRARIES} ${PCL_LIBRARIES})
 
-catkin_package()

pano/pano_view/package.xml

@@ -14,4 +14,19 @@
   </maintainer>
 
   <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rosbag</build_depend>
+  <build_depend>ff_msgs</build_depend>
+  <build_depend>camera</build_depend>
+  <build_depend>inspection</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rosbag</build_export_depend>
+  <build_export_depend>ff_msgs</build_export_depend>
+  <build_export_depend>camera</build_export_depend>
+  <build_export_depend>inspection</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rosbag</exec_depend>
+  <exec_depend>ff_msgs</exec_depend>
+  <exec_depend>camera</exec_depend>
+  <exec_depend>inspection</exec_depend>
 </package>

pano/pano_view/scripts/generate_tour.py

@@ -337,7 +337,6 @@ def link_source_images(config, tour_scenes, out_folder):
         with open(src_images_meta_path, "r") as src_images_meta_stream:
             src_images_meta = json.load(src_images_meta_stream)
 
-        # XYZ
         scene_meta = get_display_scene_meta(scene_id, config_scene_meta)
 
         img_ids = sorted(src_images_meta.keys())

pano/pano_view/static/js/isaac_pano.js

@@ -395,4 +395,3 @@ function isaacPanoInit(event) {
 }
 
 document.addEventListener('pannellumloaded', isaacPanoInit, false);
-

pano/pano_view/static/js/isaac_source_image.js

@@ -243,7 +243,7 @@ function isaacRenderState(history, state, imageStoragePath, anno) {
 
 /* Perform an undo operation. This updates the current annotation state, adjusts the history undo/redo stacks,
  * saves the annotation state to the storage object, and updates the UI.
- * 
+ *
  * @param history A history object with fields 'current', 'undoStack', 'redoStack'.
  * @param imageStoragePath A path within the storage object referencing the annotation state for the current image.
  * @param anno A reference to the live instance of the Annotorious viewer.

pano/pano_view/templates/help.html

@@ -26,7 +26,7 @@ <h2><a name="pano">Panoramic Tour Viewer</a></h2>
     <ul>
       <li>Click/tap and drag to pan your view and look in different directions.</li>
       <li>Zoom in and out with a pinch gesture on a multitouch display, or mouse wheel scroll if you have a mouse.</li>
-    </ul>        
+    </ul>
   </li>
   <li>The panorama controls at the upper left provide:
     <ul>
@@ -74,7 +74,7 @@ <h2><a name="image">Image Viewer</a></h2>
       <li>Click/tap and drag to pan your view and look at different parts of the image.</li>
       <li>Zoom in and out with a pinch gesture on a multitouch display, or mouse wheel scroll if you have a mouse.</li>
       <li>Shift-click and drag to add an annotation if you have a keyboard and mouse.</li>
-    </ul>        
+    </ul>
   </li>
   <li>The view controls at the upper left provide:
     <ul>