Hi everyone,
I’m trying to run/modify the DriveWorks lidar calibration sample (calibration_hesai / lidar_calibration_sample style workflow) on my own dataset (Hesai AT128 via CUSTOM_EX plugin + CAN replay). ICP runs fine, but egomotion always returns an identity transform, so the calibration never progresses (everything stays zero / identity). I use only CAN (DW_EGOMOTION_ODOMETRY).
What I see
With m_verbose = 1 I get output like:
-
ICP pose looks plausible (small rotation + translation):
- “Adding current ICP pose to calibration engine: … (non-identity matrix)”
-
Egomotion pose is always identity:
- “Adding Ego-Motion pose to calibration engine: … (identity 4x4)”
So: ICP works, egomotion is always identity.
Timing looks fine
- LiDAR timestamps are non-zero, monotonic.
- Delta between consecutive sweeps is about 100000 µs (~10 Hz), which matches the LiDAR.
- I logged both timestamps passed into egomotion, they are different and in the correct order.
CAN / VehicleIO inputs
I “rebuilt” the VehicleIO state update logic (by-passing):
-
I use persistent member structs:
dwVehicleIOSafetyState m_vioSafetydwVehicleIONonSafetyState m_vioNonSafetydwVehicleIOActuationFeedback m_vioAct
-
For each CAN event I update only the signals needed for egomotion:
wheelSpeed[i],speedESCandfrontSteeringAngle -
I set validity/timestamps accordingly. All my validity/encoding calls return DW_SUCCESS.
-
I call:
dwEgomotion_addVehicleIOState(&m_vioSafety, &m_vioNonSafety, &m_vioAct, m_egomotion)- This returns DW_SUCCESS.
-
Logged values look plausible:
speedESCoften ~ 1.75 m/sfrontSteeringAnglesmall around 0wheelSpeed[]often ~ 5 rad/s
Egomotion compute
My getEgomotionTransform calls dwEgomotion_computeRelativeTransformation(...) and returns true (DW_SUCCESS). If it fails, I skip the sweep. But it succeeds and still outputs identity every time.
Calibration engine
dwCalibrationEngine_addLidarPose(&icpPose, &egoPose, ...)succeeds.icpPoseis non-identity,egoPoseis always identity.- Calibration status / updated sensor-to-rig transform stays unchanged (zeros / identity).
Question
What are the typical reasons for CAN-only egomotion returning an identity relative transform even though:
- timestamps are correct,
dwEgomotion_addVehicleIOStatereturns success and inputs look non-zero,dwEgomotion_computeRelativeTransformationreturns success?
Any hints on what to verify next (or minimal required signals/vehicle params for CAN-only egomotion) would be super helpful.
Thanks!
Jis
Here is some of the code:
Some Codeparts
Initialize of Ego Motion (DW_EGOMOTION_ODOMETRY)
// -----------------------------
// Initialize Ego Motion
// -----------------------------
{
dwEgomotionParameters egomotionParameters{};
egomotionParameters.vehicle = m_vehicle;
egomotionParameters.motionModel = DW_EGOMOTION_ODOMETRY ; // nur CAN
egomotionParameters.automaticUpdate = true;
CHECK_DW_ERROR_MSG(dwEgomotion_initialize(&m_egomotion, &egomotionParameters, m_context),
"Error: Initialization of egomotion module failed.");
}
Initialize Lidar Self-Calibration with spoofing (overwriting the device string so the dw function accepts the lidar data)
// -----------------------------------------
// Initialize Lidar Self-Calibration
// -----------------------------------------
{
CHECK_DW_ERROR_MSG(dwCalibrationEngine_initialize(&m_calibEngine, m_rigConfig, m_context),
"Error: Initialize calibration engine failed.");
// Spoof: Currently, the calibration engine does not support the Hesai lidar type (Spoof: overwrite device string )
m_lidarPropertiesCalib = m_lidarProperties; //copy
std::strncpy(reinterpret_cast<char*>(m_lidarPropertiesCalib.deviceString),
"VELO_HDL64E",
sizeof(m_lidarPropertiesCalib.deviceString) - 1);
m_lidarPropertiesCalib.deviceString[sizeof(m_lidarPropertiesCalib.deviceString) - 1] = '\0';
dwCalibrationLidarParams lidarCalibrationParams{};
lidarCalibrationParams.lidarProperties = &m_lidarPropertiesCalib;
CHECK_DW_ERROR_MSG(dwCalibrationEngine_initializeLidar(&m_calibRoutine,
m_lidarInfo.sensorId,
m_canIndex,
&lidarCalibrationParams,
cudaStreamDefault,
m_calibEngine),
"Error: Initializing lidar calibration failed.");
CHECK_DW_ERROR_MSG(dwCalibrationEngine_startCalibration(m_calibRoutine, m_calibEngine),
"Error: Starting lidar calibration failed.");
}
Function where egomotion tranform is computet and should be filled with non-itentity:
bool getEgomotionTransform(dwTime_t A, dwTime_t B, dwTransformation3f& poseAtoB)
{
dwStatus status = dwEgomotion_computeRelativeTransformation(
&poseAtoB, nullptr, A + m_offsetTime, B + m_offsetTime, m_egomotion);
if (status == DW_SUCCESS) return true;
if (status == DW_NOT_READY || status == DW_NOT_AVAILABLE) return false;
CHECK_DW_ERROR_MSG(status, "Error: getting egomotion transform failed.");
return false;
}
dwTransformation3f ego{};
bool egoOK = getEgomotionTransform(m_currentGPU.timestamp, m_previousGPU.timestamp, ego);
...
m_currentToPreviousRigEgo = ego;
==> this function returns true, but poseAtoB stays always Itentity!
By-Pass Part / Here m_egomotion gets fed with CAN Messages:
for (uint32_t i = 0; i < num; ++i)
{
dwStatus status_name = dwCANInterpreter_getSignalName(&name, i, m_aCanInterpreter);
if (status_name== DW_SUCCESS && status_num==DW_SUCCESS && num>0)
{
dwStatus status = dwCANInterpreter_getf32(&value, ×tamp, i, m_aCanInterpreter);
...
if (status == DW_SUCCESS && timestamp > 0)
{...
if (strcmp(name, "ESP_21.ESP_v_Signal") == 0)
{
m_vioNonSafety.speedESC = value * (1000.0f/3600.0f);
m_vioNonSafety.speedESCTimestamp = timestamp;
setSignalValid(&m_vioNonSafety.validityInfo.speedESC);
setSignalValid(&m_vioNonSafety.validityInfo.speedESCTimestamp);
...
}
}
}
}
if (m_lastEgoMotionAddedTime < msg.timestamp_us)
{
dwStatus status = dwEgomotion_addVehicleIOState(&m_vioSafety, &m_vioNonSafety, &m_vioAct, m_egomotion);
m_lastEgoMotionAddedTime = sensorEvent->timestamp_us;
CHECK_DW_ERROR(dwCalibrationEngine_addVehicleIONonSafetyState(&m_vioNonSafety, m_canIndex, m_calibEngine));
CHECK_DW_ERROR(dwCalibrationEngine_addVehicleIOActuationFeedback(&m_vioAct, m_canIndex, m_calibEngine));
}