I copy the code from “samples/src/icp” to “samples/src/sensors/lidar/lidar_replay” ,to get the complete point cloud in one frame.
the code in “samples/src/sensors/lidar/lidar_replay”,I changed like this :
…
// declaration
dwPointCloudAccumulatorHandle_t accumulator = DW_NULL_HANDLE;
dwVector2ui dmSize = {0, 0}; // size of depthmap if we are using it.
dwPointCloud pointCloud;
…
…
// init accumulator and dwPointCloud
{
dwPointCloudAccumulatorParams params{};
CHECK_DW_ERROR(dwPointCloudAccumulator_getDefaultParams(¶ms));
CHECK_DW_ERROR_MSG(dwPointCloudAccumulator_initialize(&accumulator, ¶ms, &m_lidarProperties, m_context), “Point Cloud Accumulator Init failed”);
CHECK_DW_ERROR_MSG(dwPointCloudAccumulator_getSweepSize(&dmSize, accumulator), “Cannot retrieve sweep size from point cloud accumulator”);
pointCloud.type = DW_MEMORY_TYPE_CPU;
pointCloud.format = DW_POINTCLOUD_FORMAT_XYZI;
pointCloud.capacity = m_lidarProperties.pointsPerSpin;
CHECK_DW_ERROR_MSG(dwPointCloud_createBuffer(&pointCloud), “Failed to create buffer for full spin point cloud”);
}
…
…
// in void computeSpin() I changed code like this
while (1)
{
status = dwSensorLidar_readPacket(&nextPacket, 100000, m_lidarSensor);
if (status == DW_SUCCESS)
{
CHECK_DW_ERROR(dwPointCloudAccumulator_addLidarPacket(nextPacket, accumulator));
packetCount++;
hostTimestamp = nextPacket->hostTimestamp;
sensorTimestamp = nextPacket->sensorTimestamp;
// Append the packet to the buffer
float32_t* map = &m_pointCloud[accumulatedPoints * m_lidarProperties.pointStride];
memcpy(map, nextPacket->pointsXYZI, nextPacket->nPoints * sizeof(dwLidarPointXYZI));
accumulatedPoints += nextPacket->nPoints;
// If we go beyond a full spin, update the render data then return
//CHECK_DW_ERROR(dwPointCloudAccumulator_addLidarPacket(nextPacket, accumulator));
if (nextPacket->scanComplete)
{
// Should be ready
bool isReady = false;
CHECK_DW_ERROR(dwPointCloudAccumulator_isReady(&isReady, accumulator));
if(!isReady)
{
throw std::runtime_error("Accumulator collected all spin packets but still reports it is not ready");
}
CHECK_DW_ERROR_MSG(dwPointCloudAccumulator_bindOutput(&pointCloud, accumulator), "Cannot bind output buffer to Point Cloud Accumulator");
CHECK_DW_ERROR_MSG(dwPointCloudAccumulator_process(accumulator), "Cannot process accumulated data");
updateFrame(accumulatedPoints,
packetCount,
hostTimestamp,
sensorTimestamp);
accumulatedPoints = 0;
packetCount = 0;
endOfSpin = true;
dwSensorLidar_returnPacket(nextPacket, m_lidarSensor);
return;
}
dwSensorLidar_returnPacket(nextPacket, m_lidarSensor);
}
…
…
Compile is OK ,but when I run the code the got the error in
CHECK_DW_ERROR(dwPointCloudAccumulator_isReady(&isReady, accumulator));
the return isReady is flase.
I don’t konw why .
Could you refer to /usr/local/driveworks/samples/src/icp to figure out the problem?
Is it because accumulated packets not exactly the same as packetsPerSpin?
Thanks!
(below code snippet from /usr/local/driveworks/samples/src/icp/main.cpp)
//------------------------------------------------------------------------------
// Fetch a spin from lidar.
bool getSpin(dwPointCloud& pointCloud)
{
for (uint32_t i = 0; i < lidarProperties.<b>packetsPerSpin</b>
; ++i)
{
const dwLidarDecodedPacket* pckt = nullptr;
if (dwSensorLidar_readPacket(&pckt, 50000, lidarSensor) != DW_SUCCESS)
{
return false;
}
CHECK_DW_ERROR(dwPointCloudAccumulator_addLidarPacket(pckt, accumulator));
dwSensorLidar_returnPacket(pckt, lidarSensor);
}
// Should be ready
bool isReady = false;
CHECK_DW_ERROR(dwPointCloudAccumulator_isReady(&isReady, accumulator));
...
}
I still can not find the cause in /usr/local/driveworks/samples/src/icp. I just want to get the every point belong to the one frame with an efficient way,like using Accumulator . Not decoding the point one by one from the packet
dwPointCloudAccumulator_addLidarPacket() call times should be the same as packetsPerSpin and then it will get isReady as true from dwPointCloudAccumulator_isReady() call.
still error. I used variable calltimes to record the call times of dwPointCloudAccumulator_addLidarPacket(nextPacket, accumulator).
and if the calltimes== packetsPerSpin then I call the function of dwPointCloudAccumulator_isReady().still get the false return.
int calltimes=0;
while (1)
{
status = dwSensorLidar_readPacket(&nextPacket, 100000, m_lidarSensor);
if (status == DW_SUCCESS)
{
CHECK_DW_ERROR(dwPointCloudAccumulator_addLidarPacket(nextPacket, accumulator));
calltimes+=1;
packetCount++;
hostTimestamp = nextPacket->hostTimestamp;
sensorTimestamp = nextPacket->sensorTimestamp;
// Append the packet to the buffer
float32_t* map = &m_pointCloud[accumulatedPoints * m_lidarProperties.pointStride];
memcpy(map, nextPacket->pointsXYZI, nextPacket->nPoints * sizeof(dwLidarPointXYZI));
memmove(total, nextPacket->pointsXYZI, nextPacket->nPoints * sizeof(dwLidarPointXYZI));
accumulatedPoints += nextPacket->nPoints;
if (nextPacket->scanComplete)
{
std::cout<<"call times="<<calltimes<<std::endl;
std::cout<<"m_lidarProperties.perscan="<<m_lidarProperties.packetsPerSpin<<std::endl;
if (calltimes==(int)m_lidarProperties.packetsPerSpin)
{
bool isReady = false;
CHECK_DW_ERROR(dwPointCloudAccumulator_isReady(&isReady, accumulator));
if(!isReady) {
throw std::runtime_error("Accumulator collected all spin packets but still reports it is not ready");
}
if(isReady){
CHECK_DW_ERROR_MSG(dwPointCloudAccumulator_bindOutput(&pointCloud, accumulator), "Cannot bind output buffer to Point Cloud Accumulator");
CHECK_DW_ERROR_MSG(dwPointCloudAccumulator_process(accumulator), "Cannot process accumulated data");
std::cout<<"pointCloud.size()="<<pointCloud.size<<std::endl;
}
}
updateFrame(accumulatedPoints,
packetCount,
hostTimestamp,
sensorTimestamp);
accumulatedPoints = 0;
packetCount = 0;
endOfSpin = true;
dwSensorLidar_returnPacket(nextPacket, m_lidarSensor);
return;
}
dwSensorLidar_returnPacket(nextPacket, m_lidarSensor);
}
Could you try if calling dwPointCloudAccumulator_isReady() everytime will get a ready state sometime? Does dwPointCloudAccumulator_isReady work well in the ICP sample? If yes, you can refer to it to see what is different in your implementation.