Add SVG-based map annotation loader

Loads in points, poses and regions marked via SVG into the knowledgebase
Add a convenience method for creating poses with a line
Add doc commments to a few more map methods to make them visible in generated docs

include/knowledge_representation/LTMCMap.h

@@ -66,40 +66,96 @@ class LTMCMap : public LTMCInstance<LTMCImpl>
   {
     return this->map_id;
   }
+
+  /**
+   * @brief Add a new point to the map
+   */
   PointImpl addPoint(const std::string& name, double x, double y)
   {
     return this->ltmc.get().addPoint(*this, name, x, y);
   }
 
+  /**
+   * @brief Add a new pose to the map
+   *
+   * The pose is assumed to be in the map frame
+   *
+   * @param x X coordinate of the pose position
+   * @param y Y coordinate of the pose position
+   * @param theta orientation of the pose
+   */
   PoseImpl addPose(const std::string& name, double x, double y, double theta)
   {
     return this->ltmc.get().addPose(*this, name, x, y, theta);
   }
 
+  /**
+   * @brief Add a new pose using two points
+   *
+   * Coordinates are taken to be in the map frame. This convenience method is simply intended
+   * to prevent atan2 usage bugs in client code.
+   *
+   * @param x1 X coordinate of the pose position
+   * @param y1 Y coordinate of the pose position
+   * @param x2 X coordinate of a second point along the direction of the pose orientation
+   * @param y2 Y coordinate of a second point along the direction of the pose orientation
+   */
+  PoseImpl addPose(const std::string& name, double x1, double y1, double x2, double y2)
+  {
+    return addPose(name, x1, x2, atan2(y2 - y1, x2 - x1));
+  }
+
+  /**
+   * @brief Add a new region to the map
+   */
   RegionImpl addRegion(const std::string& name, const std::vector<std::pair<double, double>>& points)
   {
     return this->ltmc.get().addRegion(*this, name, points);
   }
+
+  /**
+   * @brief Retrieve an existing point by its unique name
+   */
   boost::optional<PointImpl> getPoint(const std::string& name)
   {
     return this->ltmc.get().getPoint(*this, name);
   }
+
+  /**
+ * @brief Retrieve an existing pose by its unique name
+ */
   boost::optional<PoseImpl> getPose(const std::string& name)
   {
     return this->ltmc.get().getPose(*this, name);
   }
+
+  /**
+ * @brief Retrieve an existing region by its unique name
+ */
   boost::optional<RegionImpl> getRegion(const std::string& name)
   {
     return this->ltmc.get().getRegion(*this, name);
   }
+
+  /**
+ * @brief Get all points that belong to this map
+ */
   std::vector<PointImpl> getAllPoints()
   {
     return this->ltmc.get().getAllPoints(*this);
   }
+
+  /**
+ * @brief Get all poses that belong to this map
+ */
   std::vector<PoseImpl> getAllPoses()
   {
     return this->ltmc.get().getAllPoses(*this);
   }
+
+  /**
+ * @brief Get all regions that belong to this map
+ */
   std::vector<RegionImpl> getAllRegions()
   {
     return this->ltmc.get().getAllRegions(*this);

scripts/populate_with_map

@@ -1,94 +1,16 @@
 #!/usr/bin/env python
 import knowledge_representation
-from knowledge_representation import LongTermMemoryConduit
+from knowledge_representation.map_loader import load_map_from_yaml, populate_with_map_annotations
 import argparse
-import sys
-import os
-import yaml
-
-
-def populate(ltmc, files_path):
-    map_name = os.path.basename(files_path)
-    connectivity_file_path = os.path.join(files_path, "connectivity.yaml")
-    if not os.path.isfile(connectivity_file_path):
-        print(
-            "Connectivity file not found at " + connectivity_file_path + ".")
-        exit(1)
-    doors_file_path = os.path.join(files_path, "doors.yaml")
-    if not os.path.isfile(doors_file_path):
-        print(
-            "Doors file not found at " + doors_file_path + ".")
-        exit(1)
-    # FIXME: This file's name is outdated. It should eventually be changed to locations.yaml when the
-    # annotation tool is updated
-    locations_file_path = os.path.join(files_path, "objects.yaml")
-    if not os.path.isfile(locations_file_path):
-        print(
-            "Locations file not found at " + locations_file_path + ".")
-        exit(1)
-    connectivity_data = read_yaml_from_file(connectivity_file_path)
-    doors_data = read_yaml_from_file(doors_file_path)
-    locations_data = read_yaml_from_file(locations_file_path)
-    door_con = ltmc.get_concept("door")
-    location_con = ltmc.get_concept("location")
-    door_con.add_attribute_entity("is_a", location_con)
-    room_con = ltmc.get_concept("room")
-
-    for location in locations_data:
-        location_entity = location_con.create_instance(location["name"])
-
-    room_name_to_entity = {}
-    for room, _ in connectivity_data.items():
-        room_entity = room_con.create_instance(room)
-        room_name_to_entity[room] = room_entity
-
-    # Rooms connected by doors are not directly connected. Remove
-    # them from the connectivity map
-    for door in doors_data:
-        room_one, room_two = door["approach"][0]["from"], door["approach"][1]["from"]
-        if room_two in connectivity_data[room_one]:
-            connectivity_data[room_one].remove(room_two)
-        else:
-            print(
-                "WARNING: {} is not connected to {}, "
-                "even though there is a door between them. This is an error in region annotation".format(
-                    room_one, room_two))
-        if room_one in connectivity_data[room_two]:
-            connectivity_data[room_two].remove(room_one)
-        else:
-            print(
-                "WARNING: {} is not connected to {}, "
-                "even though there is a door between them. This is an error in region annotation".format(
-                    room_two, room_one))
-        door_entity = door_con.create_instance(door["name"])
-
-        room_name_to_entity[room_one].add_attribute_entity("has", door_entity)
-        room_name_to_entity[room_two].add_attribute_entity("has", door_entity)
-
-    for room, neighbors in connectivity_data.items():
-        room_entity = room_name_to_entity[room]
-        for neighbor in neighbors:
-            neighbor_entity = room_name_to_entity[neighbor]
-            room_entity.add_attribute_entity("is_connected", neighbor_entity)
-
-
-def read_yaml_from_file(file_path):
-    with open(file_path, 'r') as stream:
-        try:
-            contents = yaml.load(stream)
-            return contents
-        except yaml.YAMLError:
-            print("File found at " + file_path + ", but cannot be parsed by YAML parser.")
-            exit(1)
 
 
 def main():
     parser = argparse.ArgumentParser()
-    parser.add_argument("files_path", type=str)
+    parser.add_argument("map_yaml_path", type=str)
     args = parser.parse_args()
     ltmc = knowledge_representation.get_default_ltmc()
-    dir(args)
-    populate(ltmc, args.files_path)
+    map_name, annotations = load_map_from_yaml(args.map_yaml_path)
+    populate_with_map_annotations(ltmc, map_name, *annotations)
 
 
 if __name__ == "__main__":

src/knowledge_representation/map_loader.py

@@ -0,0 +1,123 @@
+import os
+from xml.etree import ElementTree as ElTree
+import yaml
+from PIL import Image
+from math import atan2
+
+text_el = "{http://www.w3.org/2000/svg}text"
+circle_el = "{http://www.w3.org/2000/svg}circle"
+line_el = "{http://www.w3.org/2000/svg}line"
+poly_el = "{http://www.w3.org/2000/svg}polygon"
+
+
+def populate_with_map_annotations(ltmc, map_name, points, poses, regions):
+    # Wipe any existing map by this name
+    map = ltmc.get_map(map_name)
+    map.delete()
+    map = ltmc.get_map(map_name)
+
+    for name, point in points:
+        point = map.add_point(name, *point)
+        assert point.is_valid()
+
+    for name, (p1_x, p1_y), (p2_x, p2_y) in poses:
+        pose = map.add_pose(name, p1_x, p1_y, p2_x, p2_y)
+        assert pose
+
+    for name, points in regions:
+        region = map.add_region(name, points)
+        assert region
+
+
+def load_map_from_yaml(path_to_yaml):
+    parent_dir = os.path.dirname(path_to_yaml)
+    yaml_name = os.path.basename(path_to_yaml).split(".")[0]
+    with open(path_to_yaml) as map_yaml:
+        map_metadata = yaml.load(map_yaml, Loader=yaml.FullLoader)
+
+    image_path = os.path.join(parent_dir, map_metadata["image"])
+    map_image = Image.open(image_path)
+    map_metadata["width"] = map_image.size[0]
+    map_metadata["height"] = map_image.size[1]
+
+    if "annotations" not in map_metadata:
+        # Fallback when no annotations key is given: look for an svg
+        # file that has the same name as the yaml file
+        annotation_path = os.path.join(parent_dir, yaml_name + ".svg")
+    else:
+        annotation_path = os.path.join(parent_dir, map_metadata["annotations"])
+
+    if not os.path.isfile(annotation_path):
+        return None
+
+    with open(annotation_path) as test_svg:
+        svg_data = test_svg.readlines()
+        svg_data = " ".join(svg_data)
+
+    annotations = load_svg(svg_data)
+    annotations = transform_to_map_coords(map_metadata, *annotations)
+    return yaml_name, annotations
+
+
+def load_svg(svg_data):
+    tree = ElTree.fromstring(svg_data)
+    point_annotations = tree.findall(".//{}[@class='circle_annotation']".format(circle_el))
+    point_names = tree.findall(".//{}[@class='circle_annotation']/../{}".format(circle_el, text_el))
+    pose_annotations = tree.findall(".//{}[@class='pose_line_annotation']".format(line_el))
+    pose_names = tree.findall(".//{}[@class='pose_line_annotation']/../{}".format(line_el, text_el))
+    region_annotations = tree.findall(".//{}[@class='region_annotation']".format(poly_el))
+    region_names = tree.findall(".//{}[@class='region_annotation']/../{}".format(poly_el, text_el))
+
+    points = []
+    poses = []
+    regions = []
+    for point, text in zip(point_annotations, point_names):
+        name = text.text
+        pixel_coord = float(point.attrib["cx"]), float(point.attrib["cy"])
+        points.append((name, pixel_coord))
+
+    for pose, text in zip(pose_annotations, pose_names):
+        name = text.text
+        start_cord = float(pose.attrib["x1"]), float(pose.attrib["y1"])
+        stop_cord = float(pose.attrib["x2"]), float(pose.attrib["y2"])
+        poses.append((name, start_cord, stop_cord))
+
+    for region, text in zip(region_annotations, region_names):
+        name = text.text
+        points_strs = region.attrib["points"].split()
+        poly_points = [(float(x_str), float(y_str)) for x_str, y_str in map(lambda x: x.split(","), points_strs)]
+        regions.append((name, poly_points))
+
+    return points, poses, regions
+
+
+def transform_to_map_coords(map_info, points, poses, regions):
+    for i, point in enumerate(points):
+        name, point = point
+        point = point_to_map_coords(map_info, point)
+        points[i] = (name, point)
+
+    for i, pose in enumerate(poses):
+        name, p1, p2 = pose
+        p1 = point_to_map_coords(map_info, p1)
+        p2 = point_to_map_coords(map_info, p2)
+        poses[i] = (name, p1, p2)
+
+    for i, region in enumerate(regions):
+        name, poly_points = region
+        poly_points = list(map(lambda p: point_to_map_coords(map_info, p), poly_points))
+        regions[i] = (name, poly_points)
+
+    return points, poses, regions
+
+
+def point_to_map_coords(map_info, point):
+    map_origin, resolution, width, height = map_info["origin"][0:2], map_info["resolution"], map_info["width"], \
+                                            map_info["height"]
+    x, y = point
+    # the map coordinate corresponding to the bottom left pixel
+    origin_x, origin_y = map_origin
+    vertically_flipped_point = x, height - y - 1
+    map_x, map_y = vertically_flipped_point
+    point = origin_x + map_x * resolution, origin_y + map_y * resolution
+    return point

src/libknowledge_rep/python_wrapper.cpp

@@ -295,7 +295,8 @@ BOOST_PYTHON_MODULE(_libknowledge_rep_wrapper_cpp)
   class_<Map, bases<Instance>>("Map", init<uint, uint, string, LTMC&>())
       .def("add_point", &Map::addPoint)
       .def("add_region", &Map::addRegion)
-      .def("add_pose", &Map::addPose)
+      .def("add_pose", static_cast<Pose (Map::*)(const string&, double, double, double)>(&Map::addPose))
+      .def("add_pose", static_cast<Pose (Map::*)(const string&, double, double, double, double)>(&Map::addPose))
       .def("get_point", &Map::getPoint, python::return_value_policy<ReturnOptional>())
       .def("get_pose", &Map::getPose, python::return_value_policy<ReturnOptional>())
       .def("get_region", &Map::getRegion, python::return_value_policy<ReturnOptional>())