localization/docs/usecase1.md

Go to the documentation of this file.

# Copyright (c) 2019-2020 NVIDIA CORPORATION.  All rights reserved.

@page localization_usecase1 Camera Localization
@note SW Release Applicability: This tutorial is applicable to modules in **NVIDIA DRIVE Software** releases.

## Workflow
The workflow of localization is as follows:
* Initialize the @ref localization_mainsection module and its dependencies, including the @ref egomotion_mainsection module.
* While precise position is needed:
    * Update the @ref egomotion_mainsection module as raw GPS/IMU/CAN measurements are available.
    * Obtain and prepare sensor data:
        * Perform camera based detections (lane detection, pole detection, sign detection, etc.).
        * Obtain an approximate rig position and orientation in absolute WGS84 and ENU coordinates using the @ref egomotion_mainsection module.
        * Obtain a relative rig pose transformation between the previous timestamp of localization invocation to the current camera
          timestamp using the @ref egomotion_mainsection module.
    * Invoke the localization algorithm and obtain localized results.
* When done, release the acquired resources.

Below is the general structure for usage, for the full implementation refer to @ref dwx_maps_localization_sample.

## Initialization

Before the @ref localization_mainsection module can be used, all prerequisite modules need to be initialized.
Prerequisite modules include @ref maps_mainsection, @ref rig_mainsection,
 @ref landmarks_mainsection, and @ref egomotion_mainsection.

The following snippet initializes the @ref maps_mainsection.

```{.cpp}
        dwMaps_initialize(&m_map, mapFilePath, m_context);
```

The snippet below initializes the @ref rig_mainsection module.

```{.cpp}
        dwRig_initializeFromFile(&m_rigConfig, m_context, rigFile);
```

The next snippet initializes the @ref landmarks_mainsection module.
```{.cpp}
        dwMaps_initialize(&m_map, mapFilePath, m_context);
        dwMapNetParams mapNetParams{};
        dwMapNet_initDefaultParams(&mapNetParams, DW_MAPNET_TYPE_SEGMENTATION, m_context);
        dwMapNet_initialize(&m_mapNet, &mapNetParams, m_context);
        dwLandmarkDetectorParams landmarkParams;
        dwLandmarkDetector_initializeDefaultParams(&landmarkParams, m_cameraWidth, m_cameraHeight));
        dwLandmarkDetector_initializeFromMapNet(&m_landmarkDetector, m_mapNet, landmarkParams, m_cameraWidth, m_cameraHeight, m_context);
```
The @ref landmarks_mainsection module detects lane lines, road boundaries, and roadside poles. For users who wish to include road sign and traffic light detections, the
 @ref object_mainsection modules should also be initialized. This module can be initialized in a similar manner
 to the @ref landmarks_mainsection module; a detailed description is provided in @ref object_usecase1.

The snippet below initializes the @ref egomotion_mainsection module with example parameters. Both the relative and global (GNSS dependent)
variants are required.

```{.cpp}
    // relative egomotion parameters
    dwEgomotionParameters relativeEgomotionParams = {};
    dwEgomotion_initParamsFromRig(&relativeEgomotionParams, m_rigConfig, m_imuSensorName, nullptr, m_canSensorName);
    relativeEgomotionParams.motionModel = DW_EGOMOTION_IMU_ODOMETRY;
    relativeEgomotionParams.automaticUpdate = true; // update automatically
    relativeEgomotionParams.historySize = 1000;

    dwEgomotion_initialize(&m_egomotion, &relativeEgomotionParams, m_context);

    // global egomotion parameters
    dwGlobalEgomotionParameters globalEgomotionParams = {};
    dwGlobalEgomotion_initParamsFromRig(&globalEgomotionParams, m_rigConfig, m_gpsSensorName);

    dwGlobalEgomotion_initialize(&m_globalEgomotion, &globalEgomotionParams, m_context);
```

Once all dependencies are set up, the @ref localization_mainsection module can be initialized.

```{.cpp}
    dwLocalizationParameters locParams{};
    dwLocalization_initParamsFromRig(&m_params, m_rigConfig,
                                           m_numCameras, m_cameraIndices);
    dwLocalization_initialize(&m_localizer, m_map, &locParams, m_context);
```

Once the @ref localization_mainsection module is initialized, the HD map can be changed.
```{.cpp}
    dwLocalization_setMap(m_map, m_localizer);
```

## Data Reading and Preparation
Localization should be called on a per camera-frame basis. Each call requires visual features, a WGS84 position,
 an ENU orientation, and a relative transformation from the previous camera frame timestep to the current camera frame timestep.
 Regarding visual features, lane and road boundaries are required, while roadside poles, traffic signs, and traffic lights are optional inputs that improve localization accuracy.
 Position and orientation inputs need to be measured at or interpolated to the exact timestamp of the camera frame; the @ref egomotion_mainsection module
 offers this functionality.

The snippet below shows how to detect lanes, road boundaries, and poles using the @ref landmarks_mainsection module.
 Although not shown here, traffic signs and traffic lights can be detected using the @ref object_mainsection module.

```{.cpp}
dwLaneDetection lanes{};
dwLandmarkDetection poles{};
dwRoadmarkDetection roadmarks{};
dwLandmarkDetector_detectLandmarks(&lanes, &poles, &roadmarks, cameraFrame, m_landmarkDetector)
```

The @ref egomotion_mainsection modules have the ability to track global and relative position
 and orientation. Every time new GPS, IMU or CAN data is available, the modules should be updated.
The snippet below illustrates the steps needed to update the @ref egomotion_mainsection objects with new measurements.
 Refer to @ref egomotion_usecase1 and @ref egomotion_usecase2 for more details.

```{.cpp}
    // CAN and IMU data required for relative egomotion

    if (hasCANMessage())
    {
        ... // Parse the CAN message and get vehicle state

        dwEgomotion_addVehicleState(&currVehicleState, m_egomotion)
    }

    if (hasIMUMeasurement())
    {
        dwEgomotion_addIMUMeasurement(&imuFrame, m_egomotion);
    }

    // GPS data and relative egomotion required for global egomotion

    if (hasGPSMeasurement())
    {
        dwGlobalEgomotion_addGPSMeasurement(&gpsFrame, m_globalEgomotion);
    }

    {
        // Update global egomotion module with latest relative egomotion state
        dwEgomotionResult state = {};
        dwEgomotionUncertainty uncertainty = {};

        if (dwEgomotion_getEstimation(&state, m_egomotion) == DW_SUCCESS &&
            dwEgomotion_getUncertainty(&uncertainty, m_egomotion) == DW_SUCCESS)
        {
            dwGlobalEgomotion_addRelativeMotion(&state, &uncertainty, m_globalEgomotion);
        }
    }

```

When a new camera frame is captured, the user queries the global @ref egomotion_mainsection object for the vehicle's
WGS-84 position and ENU orientation. Additionally, the user queries the relative egomotion object for the relative motion.
When querying any of the @ref egomotion_mainsection modules, it is important to ensure that the reported estimates are valid. Depending on the internal
state of an egomotion object, estimates may not be available. The @ref localization_mainsection
module assumes all inputs are valid, and therefore, invalid measurements should not be passed in.

The snippet below highlights the steps necessary for obtaining correct measurements from the @ref egomotion_mainsection modules:

```{.cpp}
    // Obtain relative transform from egomotion
    dwTransformation3f currToPrev = {};
    dwStatus relativeEgoStatus = dwEgomotion_computeRelativeTransformation(&currToPrev, nullptr,
                                                                           cameraTimestamp, m_prevCameraTimestamp,
                                                                           m_egomotion);

    // Obtain global orientation and position from egomotion
    dwGlobalEgomotionResult globalEgoResult           = {};
    dwGlobalEgomotionUncertainty globalEgoUncertainty = {};
    dwStatus globalEgoStatus = dwGlobalEgomotion_computeEstimate(&globalEgoResult, &globalEgoUncertainty,
                                                                 cameraTimestamp, m_globalEgomotion);
    bool relativeEgomotionValid = (relativeEgoStatus == DW_SUCCESS);
    bool globalEgomotionValid = (globalEgoStatus == DW_SUCCESS) && globalEgoResult.validPosition && globalEgoResult.validOrientation;
```

@note In cases where the estimates from @ref egomotion_mainsection are invalid or unavailable, it is best to clean the internal state
of the @ref localization_mainsection module by calling dwLocalization_reset() and resume localization only when valid global and relative
estimates are available.

## Localization
Once the input data is ready, the localization algorithm can be invoked. As a localization module instance maintains
an internal state of previous positions and detections, the module should be repeatedly invoked over a sequence of data frames.

```{.cpp}
dwLocalizationResult locResult{};
dwLocalization_localize(&locResult,
                        laneDetectionsPerCamera,
                        poleDetectionsPerCamera,
                        signAndLightDetectionsPerCamera,
                        &globalEgoResult.position, &globalEgoResult.orientation,
                        &globalEgoUncertainty.position.covariance, &globalEgoUncertainty.orientation.covariance,
                        &currToPrev,
                        cameraTimestamp,
                        m_localizer);
```

If a new map needs to be set, the setMap function can be invoked. This function assumes all road segment ID's of the new map match those of the old map.
```{.cpp}
    dwLocalization_setMap(newMap, m_localizer);
```
@note The setMap function does not delete the old map. To avoid memory leaks, users should delete the old map if they do not plan to use it again.


## Termination

Upon termination, all allocated resources must be freed:

```
    dwRig_release(&m_rigConfig);
    dwMaps_release(&m_map);
    dwLocalization_release(&m_localizer);
    dwLandmarkDetector_release(&m_landmarkDetector);
    dwEgomotion_release(&m_egomotion)
    dwGlobalEgomotion_release(&m_globalEgomotion)
```


This workflow is demonstrated in the following sample: @ref dwx_maps_localization_sample