# Towards Open-Set NLP-Based Multi-Level Planning for Robotic Tasks
## 4 Implementation
### Perception
#### 4.3.2 Environment Modeling  
To populate the object descriptions in the plans produced by the HLP, an environment modeling system capable of extracting discrete object information from regions of semantically annotated space is required. Two such environment representations were developed--an interim 2.5D solution consisting of a simple depth map, sufficient for modeling small static scenes from a fixed vantage point, as well as a fully 3-dimensional occupancy map capable of integrating many depth images or laser scans taken from multiple vantage points, and semantically annotating the resulting points with image segmentations.  
**The 2.5D semantic depthmap** is obtained by associating a depth value with each pixel in the segmented image. When using most depth cameras, this may involve the additional step of registering the depth image in the RGB image reference frame. However, in cases where an already-aligned depth channel is available, this is essentially \"free\". For this demonstrator, a fixed _Zivid One_+ depth camera was used, which provides a registered depth image. To turn this into a map of the robot's environment, an extrinsic calibration to the workspace coordinate frame and the camera's intrinsic parameters are required (see Section 5.1), enabling conversion between the depth values and point coordinates in \(\mathbb{R}^{3}\).  
**The fully 3-dimensional semantic map** is based on the the **vectree**--an octree occupancy grid map with vector-valued nodes, shown in Figure 4 (II). As the reconstruction of high-fidelity visual images is not a priority, this data structure was selected due to the simplicity of its associated search, retrieval and integration operations. We use the octree implementation in _Octanap_[26], extended with our _VectorOcTreeNode_ and _VectorOcTree_ data structures templated by node vector dimension.  
To avoid unnecessary memory allocation and access overhead, as well as to decouple the arithmetic type of the vector from its storage properties, we also provide an abstracted _VectorStore_ data structure, which holds a _unique_ptr_ to a vector. After initial experiments revealed that a full precision floating point number does not have to be stored for acceptable recall, versions storing the embeddings as fixed point 8-bit integers and ternary values packed 5 _trits_ to a byte were implemented. Currently, to maintain compatibility with past and future experiments, tree storage, vector integration, and dot product comparison operations are implemented in 64-bit floating point precision, meaning that the quantized representations need to be expanded for arithmetic or filesystem 1/O operations.  
The vectree is designed to accept point clouds pre-aligned with the map reference frame by an external pose estimation system (approximated here by mounting the sensor on the robot and using a known calibration), alongside segmented images with a known transformation matrix to the scan point of origin. The raycast-based update method is inherited directly from _Octanap_, with the only notable addition being the fact that, during the update step, updated points which fall inside the segmentation field of view also have their vector values updated. If a point is previously unseen, its _VectorStore_ allocates a new vector with the observed value; otherwise, a weighted averaging betweenthe new and old values is performed. For this demonstrator the following averaging scheme, inherited directly from _ColorOcTree_, was found to be adequate:  
\[\mathbf{v}_{t+1}(\mathbf{x})=o_{t}(\mathbf{x})\mathbf{v}_{t}(\mathbf{x})+(1-o_{ t}(\mathbf{x}))\mathbf{h}(\mathbf{x}) \tag{3}\]  
where at time step \(t\) and spatial coordinate \(\mathbf{x}\), \(\mathbf{v}_{t}(\mathbf{x}),o_{t}(\mathbf{x})\) are the embedding vector and occupancy value, while \(\mathbf{h}(\mathbf{x})\) is the novel observation to be integrated.