Skip to content

Commit

Permalink
Fixed build issues with CUDA 11.1
Browse files Browse the repository at this point in the history
  • Loading branch information
Alexander Poeppel committed Feb 11, 2021
1 parent a5f3791 commit 81f337f
Showing 1 changed file with 36 additions and 61 deletions.
97 changes: 36 additions & 61 deletions packages/gpu_voxels/src/examples/DistanceROSDemo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,10 +62,10 @@ using gpu_voxels::voxellist::CountingVoxelList;
shared_ptr<GpuVoxels> gvl;

Vector3ui map_dimensions(256, 256, 256);
float voxel_side_length = 0.02f; // x cm voxel size
float voxel_side_length = 0.01f; // 1 cm voxel size

bool new_data_received;
PointCloud my_point_cloud, my_point_cloud2, my_point_cloud3;
PointCloud my_point_cloud;
Matrix4f tf;

void ctrlchandler(int)
Expand Down Expand Up @@ -113,18 +113,18 @@ int main(int argc, char* argv[])

icl_core::config::GetoptParameter points_parameter("points-topic:", "t",
"Identifer of the pointcloud topic");
icl_core::config::GetoptParameter roll_parameter("roll:", "r",
"Camera roll in degrees");
icl_core::config::GetoptParameter pitch_parameter("pitch:", "p",
"Camera pitch in degrees");
icl_core::config::GetoptParameter yaw_parameter("yaw:", "y",
"Camera yaw in degrees");
icl_core::config::GetoptParameter roll_parameter ("roll:", "r",
"Camera roll in degrees");
icl_core::config::GetoptParameter pitch_parameter ("pitch:", "p",
"Camera pitch in degrees");
icl_core::config::GetoptParameter yaw_parameter ("yaw:", "y",
"Camera yaw in degrees");
icl_core::config::GetoptParameter voxel_side_length_parameter("voxel_side_length:", "s",
"Side length of a voxel, default 0.01");
icl_core::config::GetoptParameter filter_threshold_parameter("filter_threshold:", "f",
"Density filter threshold per voxel, default 1");
icl_core::config::GetoptParameter erode_threshold_parameter("erode_threshold:", "e",
"Erode voxels with fewer occupied neighbors (percentage)");
icl_core::config::GetoptParameter filter_threshold_parameter ("filter_threshold:", "f",
"Density filter threshold per voxel, default 1");
icl_core::config::GetoptParameter erode_threshold_parameter ("erode_threshold:", "e",
"Erode voxels with fewer occupied neighbors (percentage)");
icl_core::config::addParameter(points_parameter);
icl_core::config::addParameter(roll_parameter);
icl_core::config::addParameter(pitch_parameter);
Expand All @@ -139,62 +139,45 @@ int main(int argc, char* argv[])
// setup "tf" to transform from camera to world / gpu-voxels coordinates

//const Vector3f camera_offsets(2, 0, 1); // camera located at y=0, x_max/2, z_max/2
const Vector3f camera_offsets(map_dimensions.x * voxel_side_length * 0.5f, -0.2f,
map_dimensions.z * voxel_side_length *
0.5f); // camera located at y=-0.2m, x_max/2, z_max/2
const Vector3f camera_offsets(map_dimensions.x * voxel_side_length * 0.5f, -0.2f, map_dimensions.z * voxel_side_length * 0.5f); // camera located at y=-0.2m, x_max/2, z_max/2

float roll = icl_core::config::paramOptDefault<float>("roll", 0.0f) * 3.141592f / 180.0f;
float pitch = icl_core::config::paramOptDefault<float>("pitch", 0.0f) * 3.141592f / 180.0f;
float yaw = icl_core::config::paramOptDefault<float>("yaw", 0.0f) * 3.141592f / 180.0f;
tf = Matrix4f::createFromRotationAndTranslation(Matrix3f::createFromRPY(-3.14 / 2.0 + roll, 0 + pitch, 0 + yaw),
camera_offsets);
tf = Matrix4f::createFromRotationAndTranslation(Matrix3f::createFromRPY(-3.14/2.0 + roll, 0 + pitch, 0 + yaw), camera_offsets);

std::string basler_pc_topic = icl_core::config::paramOptDefault<std::string>("basler-topic", "/camera/points");
std::string intel_pc_topic = icl_core::config::paramOptDefault<std::string>("intel-topic",
"/realsense/depth/color/points");
std::string velodyne_pc_topic = icl_core::config::paramOptDefault<std::string>("velodyne-topic",
"/velodyne_points");
LOGGING_INFO(Gpu_voxels, "DistanceROSDemo start. Point-cloud topics: " << basler_pc_topic << ", " << intel_pc_topic << ", " << velodyne_pc_topic << endl);
std::string point_cloud_topic = icl_core::config::paramOptDefault<std::string>("points-topic", "/camera/depth/points");
LOGGING_INFO(Gpu_voxels, "DistanceROSDemo start. Point-cloud topic: " << point_cloud_topic << endl);

// Generate a GPU-Voxels instance:
gvl = gpu_voxels::GpuVoxels::getInstance();
gvl->initialize(map_dimensions.x, map_dimensions.y, map_dimensions.z, voxel_side_length);

ros::init(argc, argv, "distance_ros_demo");
ros::NodeHandle nh;
ros::Subscriber basler_sub = nh.subscribe < pcl::PointCloud < pcl::PointXYZ > > (basler_pc_topic, 1, baslerCallback); // Basler
ros::Subscriber intel_sub = nh.subscribe < pcl::PointCloud < pcl::PointXYZ > > (intel_pc_topic, 1, intelCallback); // Intel
ros::Subscriber velodyne_sub = nh.subscribe < pcl::PointCloud < pcl::PointXYZ > > (velodyne_pc_topic, 1, velodyneCallback); // Velodyne
ros::Subscriber sub = nh.subscribe<pcl::PointCloud<pcl::PointXYZ> >(point_cloud_topic, 1, callback);

//Vis Helper
gvl->addPrimitives(primitive_array::ePRIM_SPHERE, "measurementPoints");

//PBA
gvl->addMap(MT_DISTANCE_VOXELMAP, "pbaDistanceVoxmap");
shared_ptr<DistanceVoxelMap> pbaDistanceVoxmap = dynamic_pointer_cast<DistanceVoxelMap>(
gvl->getMap("pbaDistanceVoxmap"));
shared_ptr<DistanceVoxelMap> pbaDistanceVoxmap = dynamic_pointer_cast<DistanceVoxelMap>(gvl->getMap("pbaDistanceVoxmap"));

gvl->addMap(MT_PROBAB_VOXELMAP, "erodeTempVoxmap1");
shared_ptr<ProbVoxelMap> erodeTempVoxmap1 = dynamic_pointer_cast<ProbVoxelMap>(gvl->getMap("erodeTempVoxmap1"));
gvl->addMap(MT_PROBAB_VOXELMAP, "erodeTempVoxmap2");
shared_ptr<ProbVoxelMap> erodeTempVoxmap2 = dynamic_pointer_cast<ProbVoxelMap>(gvl->getMap("erodeTempVoxmap2"));

gvl->addMap(MT_COUNTING_VOXELLIST, "countingVoxelList");
shared_ptr<CountingVoxelList> countingVoxelList = dynamic_pointer_cast<CountingVoxelList>(
gvl->getMap("countingVoxelList"));

/*gvl->addMap(MT_MULTIMODAL_VOXELLIST, "multiModalVoxelList");
shared_ptr<MultiModalVoxelList> mmVoxelList = dynamic_pointer_cast<MultiModalVoxelList>(
gvl->getMap("multiModalVoxelList"));*/
shared_ptr<CountingVoxelList> countingVoxelList = dynamic_pointer_cast<CountingVoxelList>(gvl->getMap("countingVoxelList"));

gvl->addMap(MT_COUNTING_VOXELLIST, "countingVoxelListFiltered");
shared_ptr<CountingVoxelList> countingVoxelListFiltered = dynamic_pointer_cast<CountingVoxelList>(
gvl->getMap("countingVoxelListFiltered"));
shared_ptr<CountingVoxelList> countingVoxelListFiltered = dynamic_pointer_cast<CountingVoxelList>(gvl->getMap("countingVoxelListFiltered"));

//PBA map clone for visualization without artifacts
gvl->addMap(MT_DISTANCE_VOXELMAP, "pbaDistanceVoxmapVisual");
shared_ptr<DistanceVoxelMap> pbaDistanceVoxmapVisual = dynamic_pointer_cast<DistanceVoxelMap>(
gvl->getMap("pbaDistanceVoxmapVisual"));
shared_ptr<DistanceVoxelMap> pbaDistanceVoxmapVisual = dynamic_pointer_cast<DistanceVoxelMap>(gvl->getMap("pbaDistanceVoxmapVisual"));
pbaDistanceVoxmapVisual->clearMap();

// Define two measurement points:
Expand All @@ -213,16 +196,16 @@ int main(int argc, char* argv[])
size_t iteration = 0;
new_data_received = true; // call visualize on the first iteration

ros::Publisher pub_gpu_multi = nh.advertise<sensor_msgs::PointCloud2> ("/gpu_voxels_multi_pointcloud", 1);

LOGGING_INFO(Gpu_voxels, "start visualizing maps" << endl);
while (ros::ok()) {
while (ros::ok())
{
ros::spinOnce();

LOGGING_DEBUG(Gpu_voxels, "START iteration" << endl);

// visualize new pointcloud if there is new data
if (new_data_received) {
if (new_data_received)
{
new_data_received = false;
iteration++;

Expand All @@ -233,31 +216,26 @@ int main(int argc, char* argv[])
erodeTempVoxmap2->clearMap();

// Insert the CAMERA data (now in world coordinates) into the list
// TODO PointCloud is inserted into map here --> Change type of PointCloud!
countingVoxelList->insertPointCloud(my_point_cloud, eBVM_OCCUPIED);
countingVoxelList->insertPointCloud(my_point_cloud2, eBVM_OCCUPIED);
gvl->visualizeMap("countingVoxelList");

countingVoxelListFiltered->merge(countingVoxelList);
countingVoxelListFiltered->remove_underpopulated(filter_threshold);
gvl->visualizeMap("countingVoxelListFiltered");

//gvl->visualizeMap("multiModalVoxelList");

LOGGING_INFO(Gpu_voxels, "erode voxels into pbaDistanceVoxmap" << endl);
erodeTempVoxmap1->merge(countingVoxelListFiltered);
if (erode_threshold > 0) {
if (erode_threshold > 0)
{
erodeTempVoxmap1->erodeInto(*erodeTempVoxmap2, erode_threshold);
} else {
erodeTempVoxmap1->erodeLonelyInto(
*erodeTempVoxmap2); //erode only "lonely voxels" without occupied neighbors
} else
{
erodeTempVoxmap1->erodeLonelyInto(*erodeTempVoxmap2); //erode only "lonely voxels" without occupied neighbors
}
pbaDistanceVoxmap->mergeOccupied(erodeTempVoxmap2);

// Calculate the distance map:
LOGGING_INFO(Gpu_voxels,
"calculate distance map for " << countingVoxelList->getDimensions().x << " occupied voxels"
<< endl);
LOGGING_INFO(Gpu_voxels, "calculate distance map for " << countingVoxelList->getDimensions().x << " occupied voxels" << endl);
pbaDistanceVoxmap->parallelBanding3D();

LOGGING_INFO(Gpu_voxels, "start cloning pbaDistanceVoxmap" << endl);
Expand All @@ -268,22 +246,19 @@ int main(int argc, char* argv[])
gvl->visualizePrimitivesArray("measurementPoints");

// For the measurement points we query the clearance to the closest obstacle:
thrust::device_ptr <DistanceVoxel> dvm_thrust_ptr(pbaDistanceVoxmap->getDeviceDataPtr());
for (size_t i = 0; i < measurement_points.size(); i++) {
thrust::device_ptr<DistanceVoxel> dvm_thrust_ptr(pbaDistanceVoxmap->getDeviceDataPtr());
for(size_t i = 0; i < measurement_points.size(); i++)
{
int id = voxelmap::getVoxelIndexSigned(map_dimensions, measurement_points[i]);

//get DistanceVoxel with closest obstacle information
// DistanceVoxel dv = dvm_thrust_ptr[id]; // worked before Cuda9
DistanceVoxel dv; //get DistanceVoxel with closest obstacle information
cudaMemcpy(&dv, (dvm_thrust_ptr + id).get(), sizeof(DistanceVoxel), cudaMemcpyDeviceToHost);
cudaMemcpy(&dv, (dvm_thrust_ptr+id).get(), sizeof(DistanceVoxel), cudaMemcpyDeviceToHost);

float metric_free_space = sqrtf(dv.squaredObstacleDistance(measurement_points[i])) * voxel_side_length;
LOGGING_INFO(Gpu_voxels, "Obstacle @ " << dv.getObstacle() << " Voxel @ " << measurement_points[i]
<< " has a clearance of " << metric_free_space << "m." << endl);
LOGGING_INFO(Gpu_voxels, "Obstacle @ " << dv.getObstacle() << " Voxel @ " << measurement_points[i] << " has a clearance of " << metric_free_space << "m." << endl);
}

publish_pointcloud(countingVoxelListFiltered, pub_gpu_multi);

}

r.sleep();
Expand Down

0 comments on commit 81f337f

Please sign in to comment.