Egomotion.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) 2015-2017 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_EGOMOTION_EGOMOTION_H__
#define DW_EGOMOTION_EGOMOTION_H__

#include <dw/core/Config.h>
#include <dw/core/Exports.h>
#include <dw/core/Context.h>
#include <dw/core/Types.h>

#include <dw/sensors/imu/IMU.h>
#include <dw/sensors/gps/GPS.h>

#ifdef __cplusplus
extern "C" {
#endif

typedef struct dwEgomotionObject *dwEgomotionHandle_t;

typedef enum dwMotionModel {
    DW_EGOMOTION_ODOMETRY         = 1 << 0,

    DW_EGOMOTION_IMU_ODOMETRY     = 1 << 1 | DW_EGOMOTION_ODOMETRY,

    DW_EGOMOTION_IMU_ODOMETRY_GPS = 1 << 2 | DW_EGOMOTION_IMU_ODOMETRY,

} dwMotionModel;

typedef enum dwMotionModelMeasurement {
    DW_EGOMOTION_MEASURMENT_VELOCITY      = 0, 
    DW_EGOMOTION_MEASURMENT_STEERINGANGLE = 1  
} dwMotionModelMeasurement;

typedef struct dwEgoMotionSensorCharacteristics
{
    float32_t gyroNoiseDensityDeg;

    float32_t imuSamplingRateHz;

}dwEgoMotionSensorCharacteristics;

typedef struct dwEgoMotionParameters {

    dwTransformation imu2rig;

    float32_t wheelBase;

    dwMotionModel motionModel;

    dwTime_t estimationPeriod;

    uint32_t historySize;

    float32_t gyroscopeBias[3];

    dwEgoMotionSensorCharacteristics sensorNoise;

} dwEgoMotionParameters;

typedef enum dwEgomotionDataField
{
    DW_EGOMOTION_ROLL    = 1 << 3,  
    DW_EGOMOTION_PITCH   = 1 << 4,  
    DW_EGOMOTION_YAW     = 1 << 5,  

    DW_EGOMOTION_VEL_X   = 1 << 6,
    DW_EGOMOTION_VEL_Y   = 1 << 7,
    DW_EGOMOTION_VEL_Z   = 1 << 8,

    DW_EGOMOTION_ACC_X   = 1 << 9,
    DW_EGOMOTION_ACC_Y   = 1 << 10,
    DW_EGOMOTION_ACC_Z   = 1 << 11,

    DW_EGOMOTION_GYR_X   = 1 << 12,
    DW_EGOMOTION_GYR_Y   = 1 << 13,
    DW_EGOMOTION_GYR_Z   = 1 << 14,

    DW_EGOMOTION_GPS_LAT = 1 << 15,
    DW_EGOMOTION_GPS_LON = 1 << 16,
    DW_EGOMOTION_GPS_ALT = 1 << 17,

}dwEgomotionDataField;

typedef struct {

    float32_t rotation[4];  
    float32_t linearVelocity[3];  
    float32_t angularVelocity[3];   

    float64_t location[3];  

    uint32_t validFlags;    
} dwEgomotionResult;

typedef struct {

    float32_t rotation[3];          
    float32_t velocity[3];          
    float32_t angularVelocity[3];   

    float32_t location[3];          

    uint32_t validFlags;
} dwEgomotionUncertainty;

DW_API_PUBLIC
dwStatus dwEgomotion_initialize(dwEgomotionHandle_t *obj, const dwEgoMotionParameters *params, dwContextHandle_t ctx);

DW_API_PUBLIC
dwStatus dwEgomotion_reset(dwEgomotionHandle_t obj);

DW_API_PUBLIC
dwStatus dwEgomotion_release(dwEgomotionHandle_t *obj);


DW_API_PUBLIC
dwStatus dwEgomotion_addOdometry(dwMotionModelMeasurement measuredType,
                                 float32_t measuredValue, dwTime_t timestamp_us,
                                 dwEgomotionHandle_t obj);

DW_API_PUBLIC
dwStatus dwEgomotion_addIMUMeasurement(const dwIMUFrame *imu, dwEgomotionHandle_t obj);


DW_API_PUBLIC
dwStatus dwEgomotion_addGPSMeasurement(const dwGPSFrame *location, dwEgomotionHandle_t obj);

DW_API_PUBLIC
dwStatus dwEgomotion_update(dwTime_t timestamp_us, dwEgomotionHandle_t obj);

DW_API_PUBLIC
dwStatus dwEgomotion_estimate(dwEgomotionResult *pose, dwEgomotionUncertainty *sigma, dwTime_t timestamp_us,
                              dwEgomotionHandle_t obj);

DW_API_PUBLIC
dwStatus dwEgomotion_computeRelativeTransformation(dwTransformation *poseAtoB,
                                                   dwTime_t timestamp_a, dwTime_t timestamp_b,
                                                   dwEgomotionHandle_t obj);

DW_API_PUBLIC
dwStatus dwEgomotion_getEstimationTimestamp(dwTime_t *timestamp_us, dwEgomotionHandle_t obj);

DW_API_PUBLIC
dwStatus dwEgomotion_getEstimation(dwEgomotionResult *result, dwEgomotionHandle_t obj);

DW_API_PUBLIC
dwStatus dwEgomotion_getUncertainty(dwEgomotionUncertainty *result, dwEgomotionHandle_t obj);

DW_API_PUBLIC
dwStatus dwEgomotion_getHistorySize(size_t *num, dwEgomotionHandle_t obj);

DW_API_PUBLIC
dwStatus dwEgomotion_getHistoryEntry(const dwEgomotionResult **pose, const dwEgomotionUncertainty **sigma, dwTime_t *timestamp_us,
                                     size_t index, dwEgomotionHandle_t obj);

DW_API_PUBLIC
dwStatus dwEgomotion_getMotionModel(dwMotionModel* model, dwEgomotionHandle_t obj);

//-----------------------------
// utility functions


DW_API_PUBLIC
dwStatus dwEgomotion_applyRelativeTransformation(dwTransformation *newVehicle2World,
                                                 const dwTransformation* poseOld2New,
                                                 const dwTransformation* oldVehicle2World);


#ifdef __cplusplus
}
#endif

#endif // DW_EGOMOTION_EGOMOTION_H__