Hi Allen,
in the following sample I show you how to transform each pixel from image space to world space without using lanegraph at all.
// Draw lanes
for (uint32_t laneIdx = 0U; laneIdx < m_laneDetectionResult.numLaneMarkings; ++laneIdx) {
const dwLaneMarking& laneMarking = m_laneDetectionResult.laneMarkings[laneIdx];
/******************************************************************************
NVIDIA, Fabian Weise, fweise@nvidia.com 12/10/2018
1. Get the points from our lane markings
2. For each point we need to transform it into world space
******************************************************************************/
for (uint32_t pointIdx = 0U; pointIdx < laneMarking.numPoints; ++pointIdx) {
dwVector2f point = laneMarking.imagePoints[pointIdx];
/******************************************************************************
NVIDIA, Fabian Weise, fweise@nvidia.com 12/10/2018
3. Print out the point in image space
******************************************************************************/
printf("[Image Point] x: %f, y: %f\n", point.x, point.y);
/******************************************************************************
NVIDIA, Fabian Weise, fweise@nvidia.com 12/04/2018
4. u and v are our coordinates in image space
5. To transform them, we need some 3D world space coordinates, let them be
the following x, y and z
******************************************************************************/
float32_t u = point.x;
float32_t v = point.y;
// Transform bottom center of the camera into 3D world space
float32_t x, y, z;
/******************************************************************************
NVIDIA, Fabian Weise, fweise@nvidia.com 12/10/2018
6. This is the camera which acquired the original image
7. Print its values in ray space
******************************************************************************/
CHECK_DW_ERROR(dwCalibratedCamera_pixel2Ray(&x, &y, &z, u, v, m_calibratedCamera));
printf("[Ray] x: %f, y: %f, z: %f\n", x, y, z);
/******************************************************************************
NVIDIA, Fabian Weise, fweise@nvidia.com 12/10/2018
8. Now we are moving from camera coordinate system to world coordinate
system, hence we need some empty variables first
9. Transform using our previous calculated Sensor --> Rig matrix into our
rayInRig array
10. Print its values in world space
******************************************************************************/
float32_t pointH[4] = {x, y, z};
float32_t rayInRig[3];
Mat4_Rxp(rayInRig, m_transformationSensor2Rig.array, pointH);
printf("[rayInRig] x: %f, y: %f, z: %f\n", rayInRig[0], rayInRig[1], rayInRig[2]);
/******************************************************************************
NVIDIA, Fabian Weise, fweise@nvidia.com 12/10/2018
11. Calculate the intersection to get the 2D positions (image space again)in
order to e.g. render them in another view (needs to be done on your
side)
12. This equals a simple plane model with some set camera height
13. Imagine the world view where the camera it positioned at the bottom,
now let us print our the values according to the lane marking points
******************************************************************************/
float32_t t = m_transformationSensor2Rig.array[14] / rayInRig[2];
float32_t xGround = t*rayInRig[0] + m_transformationSensor2Rig.array[12];
float32_t yGround = t*rayInRig[1] + m_transformationSensor2Rig.array[13];
std::cout << "Ground X:" << xGround << " Y:" << yGround << std::endl;
}
dwLanePositionType category = laneMarking.positionType;
CHECK_DW_ERROR(dwRenderEngine_setColor(getLaneMarkingColor(category), m_renderEngine));
CHECK_DW_ERROR(dwRenderEngine_setLineWidth(6.0f, m_renderEngine));
CHECK_DW_ERROR(dwRenderEngine_render(DW_RENDER_ENGINE_PRIMITIVE_TYPE_LINESTRIP_2D,
laneMarking.imagePoints,
sizeof(dwVector2f), 0, laneMarking.numPoints,
m_renderEngine));
// Render lane type at the far end of the lane
char laneType[2] = "";
switch (laneMarking.lineType) {
case DW_LANEMARK_TYPE_SOLID:
laneType[0] = 'S';
break;
case DW_LANEMARK_TYPE_DASHED:
laneType[0] = 'D';
break;
case DW_LANEMARK_TYPE_ROAD_BOUNDARY:
laneType[0] = 'B';
break;
case DW_LANEMARK_TYPE_UNDEFINED:
default:
break;
}
CHECK_DW_ERROR(dwRenderEngine_renderText2D(laneType,
laneMarking.imagePoints[laneMarking.numPoints-1],
m_renderEngine));
}
Hope that helps as all the comments are pretty self explaining.