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-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_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/control/vehicleio/VehicleIO.h>

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

#include <dw/rig/Rig.h>

#ifdef __cplusplus
extern "C" {
#endif

typedef struct dwEgomotionObject* dwEgomotionHandle_t;
typedef struct dwEgomotionObject const* dwEgomotionConstHandle_t;

typedef enum dwMotionModel {
    DW_EGOMOTION_ODOMETRY = 1 << 0,

    DW_EGOMOTION_IMU_ODOMETRY = 1 << 1 | DW_EGOMOTION_ODOMETRY

} dwMotionModel;

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

typedef enum dwEgomotionSpeedMeasurementType {
    DW_EGOMOTION_FRONT_SPEED = 0,

    DW_EGOMOTION_REAR_WHEEL_SPEED = 1
} dwEgomotionSpeedMeasurementType;

typedef enum dwEgomotionLinearAccelerationFilterMode {
    DW_EGOMOTION_ACC_FILTER_NO_FILTERING, 
    DW_EGOMOTION_ACC_FILTER_SIMPLE        
} dwEgomotionLinearAccelerationFilterMode;

typedef enum dwEgomotionSuspensionModel {
    DW_EGOMOTION_SUSPENSION_RIGID_MODEL            = 0, 
    DW_EGOMOTION_SUSPENSION_TORSIONAL_SPRING_MODEL = 1  
} dwEgomotionSuspensionModel;

typedef struct dwEgomotionLinearAccelFilterParams
{
    dwEgomotionLinearAccelerationFilterMode mode;

    // Simple filter parameters (ignored for other modes)
    float32_t accelerationFilterTimeConst;       
    float32_t processNoiseStdevSpeed;            
    float32_t processNoiseStdevAcceleration;     
    float32_t measurementNoiseStdevSpeed;        
    float32_t measurementNoiseStdevAcceleration; 
} dwEgomotionLinearAccelerationFilterParams;

typedef struct
{
    dwEgomotionSuspensionModel model;

    float32_t torsionalSpringPitchNaturalFrequency;

    float32_t torsionalSpringPitchDampingRatio;

} dwEgomotionSuspensionParameters;

typedef struct dwEgomotionSensorCharacteristics
{
    float32_t gyroNoiseDensityDeg;

    float32_t gyroDriftRate;

    float32_t gyroBiasRange;

    float32_t accNoiseDensityMicroG;

    float32_t imuSamplingRateHz;

    dwTime_t velocityLatency;

    float32_t velocityFactor;

} dwEgomotionSensorCharacteristics;

typedef struct dwEgomotionParameters
{
    dwVehicle vehicle;

    float32_t lateralSlipCoefficient;

    dwTransformation3f imu2rig;

    dwMotionModel motionModel;

    bool estimateInitialOrientation;

    bool automaticUpdate;

    uint32_t historySize;

    float32_t gyroscopeBias[3];

    dwEgomotionSensorCharacteristics sensorParameters;
    dwEgomotionSpeedMeasurementType speedMeasurementType;

    dwEgomotionLinearAccelerationFilterParams linearAccelerationFilterParameters;

    dwEgomotionSuspensionParameters suspension;

} dwEgomotionParameters;

typedef enum dwEgomotionDataField {
    DW_EGOMOTION_ROTATION = 1 << 0, 

    DW_EGOMOTION_LIN_VEL_X = 1 << 1, 
    DW_EGOMOTION_LIN_VEL_Y = 1 << 2, 
    DW_EGOMOTION_LIN_VEL_Z = 1 << 3, 

    DW_EGOMOTION_ANG_VEL_X = 1 << 4, 
    DW_EGOMOTION_ANG_VEL_Y = 1 << 5, 
    DW_EGOMOTION_ANG_VEL_Z = 1 << 6, 

    DW_EGOMOTION_LIN_ACC_X = 1 << 7, 
    DW_EGOMOTION_LIN_ACC_Y = 1 << 8, 
    DW_EGOMOTION_LIN_ACC_Z = 1 << 9, 

} dwEgomotionDataField;

typedef struct
{
    dwQuaternionf rotation; 

    dwTime_t timestamp; 

    float32_t linearVelocity[3]; 

    float32_t angularVelocity[3]; 

    float32_t linearAcceleration[3]; 

    int32_t validFlags; 
} dwEgomotionResult;

typedef struct
{
    dwMatrix3f rotation;

    float32_t linearVelocity[3];

    float32_t angularVelocity[3];

    float32_t linearAcceleration[3];

    dwTime_t timestamp; 

    int64_t validFlags;
} dwEgomotionUncertainty;

typedef struct
{
    dwMatrix3f rotation;    
    dwMatrix3f translation; 
    dwTime_t timeInterval;  
    bool valid;             
} dwEgomotionRelativeUncertainty;

DW_API_PUBLIC
dwStatus dwEgomotion_initParamsFromRig(dwEgomotionParameters* params, dwConstRigHandle_t rigConfiguration,
                                       const char* imuSensorName, const char* canSensorName);

DW_API_PUBLIC
dwStatus dwEgomotion_initParamsFromRigByIndex(dwEgomotionParameters* params, dwConstRigHandle_t rigConfiguration,
                                              uint32_t imuSensorIdx, uint32_t canSensorIdx);

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,
                                 dwEgomotionHandle_t obj);

DW_API_PUBLIC
dwStatus dwEgomotion_addVehicleState(const dwVehicleIOState* state, dwEgomotionHandle_t obj);

DW_API_PUBLIC
dwStatus dwEgomotion_updateVehicle(const dwVehicle* vehicle, dwEgomotionHandle_t obj);

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

DW_API_PUBLIC
dwStatus dwEgomotion_updateIMUExtrinsics(const dwTransformation3f* imuToRig, 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* uncertainty,
                              dwTime_t timestamp_us, dwEgomotionConstHandle_t obj);

DW_API_PUBLIC
dwStatus dwEgomotion_computeRelativeTransformation(dwTransformation3f* poseAtoB,
                                                   dwEgomotionRelativeUncertainty* uncertainty,
                                                   dwTime_t timestamp_a, dwTime_t timestamp_b,
                                                   dwEgomotionConstHandle_t obj);

DW_API_PUBLIC
dwStatus dwEgomotion_getEstimationTimestamp(dwTime_t* timestamp, dwEgomotionConstHandle_t obj);

DW_API_PUBLIC
dwStatus dwEgomotion_hasEstimation(bool* result, dwEgomotionConstHandle_t obj);

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

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

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

DW_API_PUBLIC
dwStatus dwEgomotion_getHistoryElement(const dwEgomotionResult** pose, const dwEgomotionUncertainty** uncertainty,
                                       size_t index, dwEgomotionConstHandle_t obj);

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

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

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

DW_API_PUBLIC
dwStatus dwEgomotion_computeSteeringAngleFromIMU(float32_t* steeringAngle,
                                                 float32_t* inverseSteeringR,
                                                 const dwIMUFrame* imuMeasurement,
                                                 dwEgomotionConstHandle_t obj);

DW_API_PUBLIC
dwStatus dwEgomotion_steeringWheelAngleToSteeringAngle(float32_t* steeringAngle, float32_t steeringWheelAngle,
                                                       dwEgomotionHandle_t obj);

DW_API_PUBLIC
dwStatus dwEgomotion_steeringAngleToSteeringWheelAngle(float32_t* steeringWheelAngle, float32_t steeringAngle,
                                                       dwEgomotionHandle_t obj);

#ifdef __cplusplus
}
#endif

#endif // DW_EGOMOTION_EGOMOTION_H_