Egomotion.h

Go to the documentation of this file.

 
//
// 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 & AFFILIATES 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 & AFFILIATES. 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 & AFFILIATES products are not authorized for use as critical
// components in life support devices or systems without express written approval of
// NVIDIA CORPORATION & AFFILIATES.
//
// SPDX-FileCopyrightText: Copyright (c) 2015-2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
// SPDX-License-Identifier: LicenseRef-NvidiaProprietary
//
// NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
// property and proprietary rights in and to this material, related
// documentation and any modifications thereto. Any use, reproduction,
// disclosure or distribution of this material and related documentation
// without an express license agreement from NVIDIA CORPORATION or
// its affiliates is strictly prohibited.
//
 
#ifndef DW_EGOMOTION_EGOMOTION_H_
#define DW_EGOMOTION_EGOMOTION_H_
 
#include <dw/core/base/Config.h>
#include <dw/core/base/Exports.h>
#include <dw/core/context/Context.h>
#include <dw/core/base/Types.h>
 
#include <dw/control/vehicleio/VehicleIO.h>
 
#include <dw/sensors/imu/IMU.h>
 
#include <dw/rig/Rig.h>
#include <dw/rig/CoordinateSystem.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 {
    DW_EGOMOTION_MEASUREMENT_VELOCITY           = 0, 
    DW_EGOMOTION_MEASUREMENT_STEERINGANGLE      = 1, 
    DW_EGOMOTION_MEASUREMENT_STEERINGWHEELANGLE = 2  
} dwMotionModelMeasurement;
 
typedef enum dwEgomotionSpeedMeasurementType {
    DW_EGOMOTION_FRONT_SPEED = 0,
 
    DW_EGOMOTION_REAR_WHEEL_SPEED = 1,
 
    DW_EGOMOTION_REAR_SPEED = 2,
} dwEgomotionSpeedMeasurementType;
 
typedef enum dwEgomotionSteeringMeasurementType {
    DW_EGOMOTION_FRONT_STEERING       = 0, 
    DW_EGOMOTION_STEERING_WHEEL_ANGLE = 1  
} dwEgomotionSteeringMeasurementType;
 
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;
 
    float32_t odometrySamplingRateHz;
 
    dwTime_t velocityLatency;
 
    float32_t velocityFactor;
 
} dwEgomotionSensorCharacteristics;
 
typedef struct dwEgomotionParameters
{
    dwVehicle vehicle;
 
    DW_DEPRECATED("Deriving lateral slip coefficient from vehicle parameters, unless this parameter is non-zero.")
    float32_t lateralSlipCoefficient;
 
    dwTransformation3f imu2rig;
 
    dwMotionModel motionModel;
 
    bool estimateInitialOrientation;
 
    bool automaticUpdate;
 
    uint32_t historySize;
 
    float32_t gyroscopeBias[3];
 
    dwEgomotionSensorCharacteristics sensorParameters;
 
    dwEgomotionSpeedMeasurementType speedMeasurementType;
 
    dwEgomotionSteeringMeasurementType steeringMeasurementType;
 
    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, 
 
    DW_EGOMOTION_ANG_ACC_X = 1 << 10, 
    DW_EGOMOTION_ANG_ACC_Y = 1 << 11, 
    DW_EGOMOTION_ANG_ACC_Z = 1 << 12, 
 
} dwEgomotionDataField;
 
typedef struct dwEgomotionResult
{
    dwQuaternionf rotation; 
 
    dwTime_t timestamp; 
 
    float32_t linearVelocity[3]; 
 
    float32_t angularVelocity[3]; 
 
    float32_t linearAcceleration[3]; 
 
    float32_t angularAcceleration[3]; 
 
    int32_t validFlags; 
} dwEgomotionResult;
 
typedef struct
{
    dwMatrix3f rotation; 
 
    float32_t linearVelocity[3]; 
 
    float32_t angularVelocity[3]; 
 
    float32_t linearAcceleration[3]; 
 
    float32_t angularAcceleration[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* vehicleSensorName);
 
DW_API_PUBLIC
dwStatus dwEgomotion_initParamsFromRigByIndex(dwEgomotionParameters* params, dwConstRigHandle_t rigConfiguration,
                                              uint32_t imuSensorIdx, uint32_t vehicleSensorIdx);
 
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
DW_DEPRECATED("Use dwEgomotion_addVehicleState instead.")
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_addVehicleIOState(dwVehicleIOSafetyState const* safeState,
                                       dwVehicleIONonSafetyState const* nonSafeState,
                                       dwVehicleIOActuationFeedback const* actuationFeedback,
                                       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
DW_DEPRECATED("Deprecated, will be removed. Set dwEgomotionParameters.autoupdate to true instead.")
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_computeBodyTransformation(dwTransformation3f* const transformationAToB,
                                               dwEgomotionRelativeUncertainty* const uncertainty,
                                               dwTime_t const timestamp,
                                               dwCoordinateSystem const coordinateSystemA,
                                               dwCoordinateSystem const coordinateSystemB,
                                               dwEgomotionConstHandle_t const 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_getGyroscopeBias(dwVector3f* gyroBias, dwEgomotionConstHandle_t obj);
 
DW_API_PUBLIC
dwStatus dwEgomotion_getHistorySize(size_t* size, dwEgomotionConstHandle_t obj);
 
DW_API_PUBLIC
dwStatus dwEgomotion_getHistoryElement(dwEgomotionResult* pose, 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* vehicleToWorldAtB,
                                                 const dwTransformation3f* vehicleAToB,
                                                 const dwTransformation3f* vehicleToWorldAtA);
 
DW_API_PUBLIC
DW_DEPRECATED("dwEgomotion_computeSteeringAngleFromIMU() is deprecated and will be removed soon. ")
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_