Localization.h

Go to the documentation of this file.

// This code contains NVIDIA Confidential Information and is disclosed
// under the Mutual Non-Disclosure Agreement.
//
// Notice
// ALL NVIDIA DESIGN SPECIFICATIONS AND CODE ("MATERIALS") ARE PROVIDED "AS IS" NVIDIA MAKES
// NO REPRESENTATIONS, WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
// THE MATERIALS, AND EXPRESSLY DISCLAIMS ANY IMPLIED WARRANTIES OF NONINFRINGEMENT,
// MERCHANTABILITY, OR FITNESS FOR A PARTICULAR PURPOSE.
//
// NVIDIA Corporation assumes no responsibility for the consequences of use of such
// information or for any infringement of patents or other rights of third parties that may
// result from its use. No license is granted by implication or otherwise under any patent
// or patent rights of NVIDIA Corporation. No third party distribution is allowed unless
// expressly authorized by NVIDIA.  Details are subject to change without notice.
// This code supersedes and replaces all information previously supplied.
// NVIDIA Corporation products are not authorized for use as critical
// components in life support devices or systems without express written approval of
// NVIDIA Corporation.
//
// Copyright (c) 2017-2020 NVIDIA Corporation. All rights reserved.
//
// NVIDIA Corporation and its licensors retain all intellectual property and proprietary
// rights in and to this software and related documentation and any modifications thereto.
// Any use, reproduction, disclosure or distribution of this software and related
// documentation without an express license agreement from NVIDIA Corporation is
// strictly prohibited.
//

#ifndef DW_LOCALIZATION_H_
#define DW_LOCALIZATION_H_

#include <dw/core/Context.h>
#include <dw/perception/landmarks/camera/LandmarkDetector.h>
#include <dw/world/ObjectArray.h>
#include <dw/sensors/gps/GPS.h>
#include <dw/sensors/imu/IMU.h>
#include <dw/sensors/lidar/Lidar.h>
#include <dw/maps/Maps.h>
#include <dw/maps/ContentLayers.h>
#include <dw/maps/layers/ImageLayer.h>
#include <dw/rig/Rig.h>
#include <dw/egomotion/global/GlobalEgomotion.h>

#ifdef __cplusplus
extern "C" {
#endif

typedef struct dwLocalizationObject* dwLocalizationHandle_t;

typedef enum dwLocalizationResultIndicator {
    DW_LOCALIZATION_RESULT_INDICATOR_NORMAL                  = 0,
    DW_LOCALIZATION_RESULT_INDICATOR_BAD_SENSOR_DATA0        = (1u << 0),
    DW_LOCALIZATION_RESULT_INDICATOR_BAD_SENSOR_DATA1        = (1u << 1),
    DW_LOCALIZATION_RESULT_INDICATOR_BAD_SENSOR_DATA2        = (1u << 2),
    DW_LOCALIZATION_RESULT_INDICATOR_BAD_SENSOR_DATA3        = (1u << 3),
    DW_LOCALIZATION_RESULT_INDICATOR_BAD_SENSOR_DATA4        = (1u << 4),
    DW_LOCALIZATION_RESULT_INDICATOR_BAD_SENSOR_DATA5        = (1u << 5),
    DW_LOCALIZATION_RESULT_INDICATOR_BAD_SENSOR_DATA6        = (1u << 6),
    DW_LOCALIZATION_RESULT_INDICATOR_BAD_SENSOR_DATA7        = (1u << 7),
    DW_LOCALIZATION_RESULT_INDICATOR_BAD_SENSOR_CALIBRATION0 = (1u << 8),
    DW_LOCALIZATION_RESULT_INDICATOR_BAD_SENSOR_CALIBRATION1 = (1u << 9),
    DW_LOCALIZATION_RESULT_INDICATOR_BAD_SENSOR_CALIBRATION2 = (1u << 10),
    DW_LOCALIZATION_RESULT_INDICATOR_BAD_SENSOR_CALIBRATION3 = (1u << 11),
    DW_LOCALIZATION_RESULT_INDICATOR_BAD_SENSOR_CALIBRATION4 = (1u << 12),
    DW_LOCALIZATION_RESULT_INDICATOR_BAD_SENSOR_CALIBRATION5 = (1u << 13),
    DW_LOCALIZATION_RESULT_INDICATOR_BAD_SENSOR_CALIBRATION6 = (1u << 14),
    DW_LOCALIZATION_RESULT_INDICATOR_BAD_SENSOR_CALIBRATION7 = (1u << 15),
    DW_LOCALIZATION_RESULT_INDICATOR_BAD_GLOBAL_EGOMOTION    = (1u << 16),
    DW_LOCALIZATION_RESULT_INDICATOR_BAD_RELATIVE_EGOMOTION  = (1u << 17),
    DW_LOCALIZATION_RESULT_INDICATOR_BAD_MAP_CORE            = (1u << 18),
    DW_LOCALIZATION_RESULT_INDICATOR_BAD_MAP_SENSOR_LAYER    = (1u << 19)
} dwLocalizationResultIndicator;

typedef struct
{
    dwTime_t timestamp;

    dwLocalizationResultIndicator indicator;

    float32_t confidence;

    bool isMapAvailable;

    dwMapsRoadSegmentId roadSegmentId;

    dwVector3f relativePosition;

    dwQuaterniond relativeOrientation;

    dwMapsGeoPoint globalPosition;

    dwQuaterniond globalOrientation;

    dwMatrix3d positionUncertainty;

    dwMatrix3d orientationUncertainty;
} dwLocalizationResult;

// The maximum number of cameras that the localization module can use.
#define DW_LOCALIZATION_MAX_CAMERA_COUNT 8

typedef struct
{
    uint32_t numCameras;

    dwFThetaCameraConfig cameraIntrinsics[DW_LOCALIZATION_MAX_CAMERA_COUNT];

    dwTransformation3f camerasToRig[DW_LOCALIZATION_MAX_CAMERA_COUNT];

} dwLocalizationCameraParameters;

typedef struct
{
    uint32_t pointSkip;

    bool asyncLoading;
} dwLocalizationRadarParameters;

typedef dwLocalizationRadarParameters dwLocalizationLidarParameters;

//
// Camera based localization.
//

DW_API_PUBLIC
dwStatus dwLocalization_cameraInitParamsFromRig(dwLocalizationCameraParameters* params,
                                                dwConstRigHandle_t rig,
                                                uint32_t numCameras,
                                                const uint32_t cameraIndices[]);

DW_API_PUBLIC
dwStatus dwLocalization_cameraInitialize(dwLocalizationHandle_t* localization,
                                         dwConstMapHandle_t map,
                                         const dwLocalizationCameraParameters* params,
                                         dwContextHandle_t context);

DW_API_PUBLIC
dwStatus dwLocalization_cameraLocalize(dwLocalizationResult* localizationResult,
                                       const dwLaneDetection laneDetectionsPerCamera[],
                                       const dwLandmarkDetection poleDetectionsPerCamera[],
                                       const dwObjectArray signAndLightDetectionsPerCamera[],
                                       const dwGlobalEgomotionResult* globalEgoResult,
                                       const dwGlobalEgomotionUncertainty* globalEgoUncertainty,
                                       const dwTransformation3f* currToPrev,
                                       const dwEgomotionRelativeUncertainty* currToPrevUncertainty,
                                       dwTime_t timestamp,
                                       dwLocalizationHandle_t localization);

DW_API_PUBLIC
dwStatus dwLocalization_cameraSetExtrinsics(uint32_t cameraIndex, const dwTransformation3f* cameraToRig, dwLocalizationHandle_t localization);

//
// Radar based localization.
//

DW_API_PUBLIC
dwStatus dwLocalization_radarInitialize(dwLocalizationHandle_t* localization,
                                        dwConstMapHandle_t map,
                                        dwMapProviderHandle_t mapProvider,
                                        const dwMapsContentLayerBuffer* contentLayers,
                                        const dwLocalizationRadarParameters* params,
                                        dwContextHandle_t context);

DW_API_PUBLIC
dwStatus dwLocalization_radarLocalize(dwLocalizationResult* localizationResult,
                                      const dwVector4f* radarPoints,
                                      size_t numPoints,
                                      const dwGlobalEgomotionResult* globalEgoResult,
                                      const dwGlobalEgomotionUncertainty* globalEgoUncertainty,
                                      const dwTransformation3f* currToPrev,
                                      const dwEgomotionRelativeUncertainty* currToPrevUncertainty,
                                      dwTime_t timestamp,
                                      dwLocalizationHandle_t localization);

//
// Lidar based localization.
//

DW_API_PUBLIC
dwStatus dwLocalization_lidarInitialize(dwLocalizationHandle_t* localization,
                                        dwConstMapHandle_t map,
                                        dwMapProviderHandle_t mapProvider,
                                        const dwMapsContentLayerBuffer* contentLayers,
                                        const dwLocalizationLidarParameters* params,
                                        dwContextHandle_t context);

DW_API_PUBLIC
dwStatus dwLocalization_lidarLocalize(dwLocalizationResult* localizationResult,
                                      const dwLidarPointXYZI* lidarPoints,
                                      size_t numPoints,
                                      const dwGlobalEgomotionResult* globalEgoResult,
                                      const dwGlobalEgomotionUncertainty* globalEgoUncertainty,
                                      const dwTransformation3f* currToPrev,
                                      const dwEgomotionRelativeUncertainty* currToPrevUncertainty,
                                      dwTime_t timestamp,
                                      dwLocalizationHandle_t localization);

//
// Common functions across camera, radar and lidar localization.
//

DW_API_PUBLIC
dwStatus dwLocalization_setMap(dwConstMapHandle_t map,
                               dwLocalizationHandle_t localization);

DW_API_PUBLIC
dwStatus dwLocalization_reset(dwLocalizationHandle_t localization);

DW_API_PUBLIC
dwStatus dwLocalization_release(dwLocalizationHandle_t localization);

DW_API_PUBLIC
dwStatus dwLocalization_setCUDAStream(cudaStream_t stream, dwLocalizationHandle_t localization);

DW_API_PUBLIC
dwStatus dwLocalization_getCUDAStream(cudaStream_t* stream, dwLocalizationHandle_t localization);

#ifdef __cplusplus
}
#endif

#endif // DW_LOCALIZATION_H_