void obstaclePoseCallback(const geometry_msgs::Pose::ConstPtr& msg)
{
std::cout << "Got PoseMessage" << std::endl;
gvl->clearMap("myObjectVoxelmap");
object_position.x = msg->position.x;
object_position.y = msg->position.y;
object_position.z = msg->position.z;
gvl->insertPointcloudFromFile("myObjectVoxelmap", "hals_vereinfacht.binvox", true,
eBVM_OCCUPIED, false, object_position, 0.3);
}
void jointStateCallback(const sensor_msgs::JointState::ConstPtr& msg)
{
//std::cout << "Got JointStateMessage" << std::endl;
gvl->clearMap("myHandVoxellist");
for(size_t i = 0; i < msg->name.size(); i++)
{
myRobotJointValues[msg->name[i]] = msg->position[i];
}
// update the robot joints:
gvl->setRobotConfiguration("myUrdfRobot", myRobotJointValues);
// insert the robot into the map:
gvl->insertRobotIntoMap("myUrdfRobot", "myHandVoxellist", eBVM_OCCUPIED);
}
int main(int argc, char* argv[])
{
signal(SIGINT, ctrlchandler);
signal(SIGTERM, killhandler);
icl_core::logging::initialize(argc, argv);
gvl = GpuVoxels::getInstance();
Vector3ui dim(89, 123, 74);
float side_length = 1.f; // voxel side length
gvl->initialize(dim.x, dim.y, dim.z, side_length);
gvl->addMap(MT_PROBAB_VOXELMAP, "myProbVoxelMap1");
gvl->addMap(MT_PROBAB_VOXELMAP, "myProbVoxelMap2");
boost::shared_ptr<ProbVoxelMap> map_1(gvl->getMap("myProbVoxelMap1")->as<ProbVoxelMap>());
boost::shared_ptr<ProbVoxelMap> map_2(gvl->getMap("myProbVoxelMap2")->as<ProbVoxelMap>());
std::vector<Vector3f> this_testpoints1;
std::vector<Vector3f> this_testpoints2;
this_testpoints1 = createBoxOfPoints( Vector3f(2.1, 2.1, 2.1), Vector3f(4.1, 4.1, 4.1), 0.5);
this_testpoints2 = createBoxOfPoints( Vector3f(3.1, 3.1, 3.1), Vector3f(5.1, 5.1, 5.1), 0.5);
map_1->insertPointCloud(this_testpoints1, eBVM_OCCUPIED);
map_2->insertPointCloud(this_testpoints2, eBVM_OCCUPIED);
std::cout << "Collisions w offset: " << map_1->collideWith(map_2.get(), 0.1, Vector3i(-1,-0,-1)) << std::endl;
std::cout << "Collisions w/o offset: " << map_1->collideWith(map_2.get(), 0.1) << std::endl;
while(true)
{
gvl->visualizeMap("myProbVoxelMap1");
gvl->visualizeMap("myProbVoxelMap2");
sleep(1);
}
return 0;
}
int main(int argc, char* argv[])
{
signal(SIGINT, ctrlchandler);
signal(SIGTERM, killhandler);
ros::init(argc, argv, "gpu_voxels");
icl_core::logging::initialize(argc, argv);
/*
* First, we generate an API class, which defines the
* volume of our space and the resolution.
* Be careful here! The size is limited by the memory
* of your GPU. Even if an empty Octree is small, a
* Voxelmap will always require the full memory.
*/
gvl = GpuVoxels::getInstance();
gvl->initialize(200, 200, 200, 0.001); // ==> 200 Voxels, each one is 1 mm in size so the map represents 20x20x20 centimeter
// Add a map:
gvl->addMap(MT_PROBAB_VOXELMAP, "myObjectVoxelmap");
gvl->addMap(MT_BITVECTOR_VOXELLIST, "myHandVoxellist");
// And a robot, generated from a ROS URDF file:
gvl->addRobot("myUrdfRobot", "svh-standalone.urdf", true);
ros::NodeHandle n;
ros::Subscriber sub1 = n.subscribe("joint_states", 1, jointStateCallback);
ros::Subscriber sub2 = n.subscribe("obstacle_pose", 1, obstaclePoseCallback);
// A an obstacle that can be moved with the ROS callback
gvl->insertPointcloudFromFile("myObjectVoxelmap", "hals_vereinfacht.binvox", true,
eBVM_OCCUPIED, false, object_position, 0.3);
// update the robot joints:
gvl->setRobotConfiguration("myUrdfRobot", myRobotJointValues);
// insert the robot into the map:
gvl->insertRobotIntoMap("myUrdfRobot", "myHandVoxellist", eBVM_OCCUPIED);
/*
* Now we start the main loop, that will read ROS messages and update the Robot.
*/
BitVectorVoxel bits_in_collision;
size_t num_colls;
while(ros::ok())
{
ros::spinOnce();
num_colls = gvl->getMap("myHandVoxellist")->as<voxellist::BitVectorVoxelList>()->collideWithTypes(gvl->getMap("myObjectVoxelmap")->as<voxelmap::ProbVoxelMap>(), bits_in_collision);
std::cout << "Detected " << num_colls << " collisiosn " << std::endl;
//std::cout << "with bits \n" << bits_in_collision << std::endl;
// tell the visualier that the map has changed:
gvl->visualizeMap("myHandVoxellist");
gvl->visualizeMap("myObjectVoxelmap");
usleep(30000);
}
gvl.reset();
exit(EXIT_SUCCESS);
}
int main(int argc, char* argv[])
{
signal(SIGINT, ctrlchandler);
signal(SIGTERM, killhandler);
icl_core::config::GetoptParameter ident_parameter("device-identifier:", "id",
"Identifer of the kinect device");
icl_core::config::addParameter(ident_parameter);
icl_core::logging::initialize(argc, argv);
std::string identifier = icl_core::config::Getopt::instance().paramOpt("device-identifier");
/*
* First, we generate an API class, which defines the
* volume of our space and the resolution.
* Be careful here! The size is limited by the memory
* of your GPU. Even if an empty Octree is small, a
* Voxelmap will always require the full memory.
*/
gvl = GpuVoxels::getInstance();
gvl->initialize(200, 200, 100, 0.02);
/*
* Now we add a map, that will represent the robot.
* The robot is inserted with deterministic poses,
* so a deterministic map is sufficient here.
*/
gvl->addMap(MT_BITVECTOR_VOXELMAP, "myRobotMap");
/*
* A second map will represent the environment.
* As it is captured by a sensor, this map is probabilistic.
*/
gvl->addMap(MT_BITVECTOR_OCTREE, "myEnvironmentMap");
/*
* Lets create a kinect driver and an according pointcloud.
* To allow easy transformation of the Kinect pose,
* we declare it as a robot and model a pan-tilt-unit.
*/
Kinect* kinect = new Kinect(identifier);
kinect->run();
std::vector<std::string> kinect_link_names(6);
kinect_link_names[0] = "z_translation";
kinect_link_names[1] = "y_translation";
kinect_link_names[2] = "x_translation";
kinect_link_names[3] = "pan";
kinect_link_names[4] = "tilt";
kinect_link_names[5] = "kinect";
std::vector<robot::DHParameters> kinect_dh_params(6);
kinect_dh_params[0] = robot::DHParameters(0.0, 0.0, 0.0, -1.5708, 0.0, robot::PRISMATIC); // Params for Y translation
kinect_dh_params[1] = robot::DHParameters(0.0, -1.5708, 0.0, -1.5708, 0.0, robot::PRISMATIC); // Params for X translation
kinect_dh_params[2] = robot::DHParameters(0.0, 1.5708, 0.0, 1.5708, 0.0, robot::PRISMATIC); // Params for Pan axis
kinect_dh_params[3] = robot::DHParameters(0.0, 1.5708, 0.0, 1.5708, 0.0, robot::REVOLUTE); // Params for Tilt axis
kinect_dh_params[4] = robot::DHParameters(0.0, 0.0, 0.0, -3.1415, 0.0, robot::REVOLUTE); // Params for Kinect
kinect_dh_params[5] = robot::DHParameters(0.0, 0.0, 0.0, 0.0, 0.0, robot::REVOLUTE); // Pseudo Param
robot::JointValueMap kinect_joints;
kinect_joints["z_translation"] = 0.6; // moves along the Z axis
kinect_joints["y_translation"] = 1.0; // moves along the Y Axis
kinect_joints["x_translation"] = 1.0; // moves along the X Axis
kinect_joints["pan"] = -0.7;
kinect_joints["tilt"] = 0.5;
std::vector<Vector3f> kinect_pc(640*480);
MetaPointCloud myKinectCloud;
myKinectCloud.addCloud(kinect_pc, true, kinect_link_names[5]);
gvl->addRobot("kinectData", kinect_link_names, kinect_dh_params, myKinectCloud);
/*
* Of course, we need a robot. At this point, you can choose between
* describing your robot via ROS URDF or via conventional DH parameter.
* In this example, we simply hardcode a DH robot:
*/
// First, we load the robot geometry which contains 9 links with 7 geometries:
// Geometries are required to have the same names as links, if they should get transformed.
std::vector<std::string> linknames(10);
std::vector<std::string> paths_to_pointclouds(7);
linknames[0] = "z_translation";
linknames[1] = "y_translation";
linknames[2] = "x_translation";
linknames[3] = paths_to_pointclouds[0] = "hollie/arm_0_link.xyz";
linknames[4] = paths_to_pointclouds[1] = "hollie/arm_1_link.xyz";
linknames[5] = paths_to_pointclouds[2] = "hollie/arm_2_link.xyz";
linknames[6] = paths_to_pointclouds[3] = "hollie/arm_3_link.xyz";
linknames[7] = paths_to_pointclouds[4] = "hollie/arm_4_link.xyz";
linknames[8] = paths_to_pointclouds[5] = "hollie/arm_5_link.xyz";
linknames[9] = paths_to_pointclouds[6] = "hollie/arm_6_link.xyz";
std::vector<robot::DHParameters> dh_params(10);
// _d, _theta, _a, _alpha, _value, _type
dh_params[0] = robot::DHParameters(0.0, 0.0, 0.0, -1.5708, 0.0, robot::PRISMATIC); // Params for Y translation
dh_params[1] = robot::DHParameters(0.0, -1.5708, 0.0, -1.5708, 0.0, robot::PRISMATIC); // Params for X translation
dh_params[2] = robot::DHParameters(0.0, 1.5708, 0.0, 1.5708, 0.0, robot::PRISMATIC); // Params for first Robot axis (visualized by 0_link)
dh_params[3] = robot::DHParameters(0.0, 1.5708, 0.0, 1.5708, 0.0, robot::REVOLUTE); // Params for second Robot axis (visualized by 1_link)
dh_params[4] = robot::DHParameters(0.0, 0.0, 0.35, -3.1415, 0.0, robot::REVOLUTE); //
dh_params[5] = robot::DHParameters(0.0, 0.0, 0.0, 1.5708, 0.0, robot::REVOLUTE); //
dh_params[6] = robot::DHParameters(0.0, 0.0, 0.365, -1.5708, 0.0, robot::REVOLUTE); //
dh_params[7] = robot::DHParameters(0.0, 0.0, 0.0, 1.5708, 0.0, robot::REVOLUTE); //
dh_params[8] = robot::DHParameters(0.0, 0.0, 0.0, 0.0, 0.0, robot::REVOLUTE); // Params for last Robot axis (visualized by 6_link)
dh_params[9] = robot::DHParameters(0.0, 0.0, 0.0, 0.0, 0.0, robot::REVOLUTE); // Params for the not viusalized tool
gvl->addRobot("myRobot", linknames, dh_params, paths_to_pointclouds, true);
// initialize the joint interpolation
std::size_t counter = 0;
const float ratio_delta = 0.02;
robot::JointValueMap min_joint_values;
min_joint_values["z_translation"] = 0.0; // moves along the Z axis
min_joint_values["y_translation"] = 0.5; // moves along the Y Axis
min_joint_values["x_translation"] = 0.5; // moves along the X Axis
min_joint_values["hollie/arm_0_link.xyz"] = 1.0;
min_joint_values["hollie/arm_1_link.xyz"] = 1.0;
min_joint_values["hollie/arm_2_link.xyz"] = 1.0;
min_joint_values["hollie/arm_3_link.xyz"] = 1.0;
min_joint_values["hollie/arm_4_link.xyz"] = 1.0;
min_joint_values["hollie/arm_5_link.xyz"] = 1.0;
min_joint_values["hollie/arm_6_link.xyz"] = 1.0;
robot::JointValueMap max_joint_values;
max_joint_values["z_translation"] = 0.0; // moves along the Z axis
max_joint_values["y_translation"] = 2.5; // moves along the Y axis
max_joint_values["x_translation"] = 2.5; // moves along the X Axis
max_joint_values["hollie/arm_0_link.xyz"] = 1.5;
max_joint_values["hollie/arm_1_link.xyz"] = 1.5;
max_joint_values["hollie/arm_2_link.xyz"] = 1.5;
max_joint_values["hollie/arm_3_link.xyz"] = 1.5;
max_joint_values["hollie/arm_4_link.xyz"] = 1.5;
max_joint_values["hollie/arm_5_link.xyz"] = 1.5;
max_joint_values["hollie/arm_6_link.xyz"] = 1.5;
const int num_swept_volumes = 50;// < BIT_VECTOR_LENGTH;
/*
* SWEPT VOLUME:
* The robot moves and changes it's pose, so we "voxelize"
* the links in every step and insert it into the robot map.
* As the map is not cleared, this will generate a sweep.
* The ID within the sweep is incremented with the single poses
* so we can later identify, which pose created a collision.
*/
LOGGING_INFO(Gpu_voxels, "Generating Swept Volume..." << endl);
robot::JointValueMap myRobotJointValues;
for (int i = 0; i < num_swept_volumes; ++i)
{
myRobotJointValues = gpu_voxels::interpolateLinear(min_joint_values, max_joint_values,
ratio_delta * counter++);
gvl->setRobotConfiguration("myRobot", myRobotJointValues);
BitVoxelMeaning v = BitVoxelMeaning(eBVM_SWEPT_VOLUME_START + 1 + i);
gvl->insertRobotIntoMap("myRobot", "myRobotMap", v);
}
/*
* MAIN LOOP:
* In this loop we update the Kinect Pointcloud
* and collide it with the Swept-Volume of the robot.
*/
LOGGING_INFO(Gpu_voxels, "Starting collision detection..." << endl);
while (true)
{
// Insert Kinect data (in cam-coordinate system)
gvl->updateRobotPart("kinectData", "kinect", kinect->getDataPtr());
// Call setRobotConfiguration to trigger transformation of Kinect data:
gvl->setRobotConfiguration("kinectData", kinect_joints);
// Insert the Kinect data (now in world coordinates) into the map
gvl->insertRobotIntoMap("kinectData", "myEnvironmentMap", eBVM_OCCUPIED);
size_t num_cols = 0;
BitVectorVoxel collision_types;
num_cols = gvl->getMap("myEnvironmentMap")->as<NTree::GvlNTreeDet>()->collideWithTypes(gvl->getMap("myRobotMap")->as<voxelmap::BitVectorVoxelMap>(), collision_types, 1.0f);
LOGGING_INFO(Gpu_voxels, "Collsions: " << num_cols << endl);
printf("Voxel types in collision:\n");
DrawTypes draw_types;
for(size_t i = 0; i < BIT_VECTOR_LENGTH; ++i)
{
if(collision_types.bitVector().getBit(i))
{
draw_types.draw_types[i] = 1;
printf("%lu; ", i);
}
}
printf("\n");
// this informs the visualizer which Sub-Volumes should be rendered
gvl->getVisualization("myRobotMap")->setDrawTypes(draw_types);
// tell the visualizer that the data has changed.
gvl->visualizeMap("myRobotMap");
gvl->visualizeMap("myEnvironmentMap");
usleep(10000);
// We only clear the environment to update it with new Kinect data.
// The robot maps stays static to not loose the Sweeps.
gvl->clearMap("myEnvironmentMap");
}
}
void killhandler(int)
{
gvl.reset();
exit(EXIT_SUCCESS);
}
void ctrlchandler(int)
{
gvl.reset();
exit(EXIT_SUCCESS);
}
int main(int argc, char* argv[])
{
signal(SIGINT, ctrlchandler);
signal(SIGTERM, killhandler);
icl_core::logging::initialize(argc, argv);
gvl = GpuVoxels::getInstance();
Vector3ui dim(136, 136, 136);
float side_length = 1.0; // voxel side length
gvl->initialize(dim.x, dim.y, dim.z, side_length);
gvl->addMap(MT_PROBAB_VOXELMAP, "myProbVoxelMap");
boost::shared_ptr<ProbVoxelMap> prob_map(gvl->getMap("myProbVoxelMap")->as<ProbVoxelMap>());
std::vector<Vector3f> boxpoints;
boxpoints = createBoxOfPoints( Vector3f(10, 10, 30), Vector3f(30, 30, 50), 0.9); // choose large delta, so that only 1 point falls into each voxel (mostly)
PointCloud box(boxpoints);
Matrix4f shift_diag_up(Matrix4f::createFromRotationAndTranslation(Matrix3f::createIdentity(), Vector3f(0, 15, 15)));
Matrix4f shift_down(Matrix4f::createFromRotationAndTranslation(Matrix3f::createIdentity(), Vector3f(0, 15, -15)));
Matrix4f shift_diag_down(Matrix4f::createFromRotationAndTranslation(Matrix3f::createIdentity(), Vector3f(0, -15, -15)));
// insert cube into map
prob_map->insertPointCloud(box, eBVM_MAX_OCC_PROB); // = 254
box.transformSelf(&shift_diag_up);
prob_map->insertPointCloud(box, eBVM_MAX_OCC_PROB); // = 254
box.transformSelf(&shift_diag_up);
prob_map->insertPointCloud(box, BitVoxelMeaning(229));
box.transformSelf(&shift_diag_up);
prob_map->insertPointCloud(box, BitVoxelMeaning(204));
box.transformSelf(&shift_diag_up);
prob_map->insertPointCloud(box, BitVoxelMeaning(179));
box.transformSelf(&shift_diag_up);
prob_map->insertPointCloud(box, BitVoxelMeaning(154));
box.transformSelf(&shift_down);
// eBVM_UNCERTAIN_OCC_PROB); // = 129
prob_map->insertPointCloud(box, BitVoxelMeaning(104));
box.transformSelf(&shift_diag_down);
prob_map->insertPointCloud(box, BitVoxelMeaning(79));
box.transformSelf(&shift_diag_down);
prob_map->insertPointCloud(box, BitVoxelMeaning(54));
box.transformSelf(&shift_diag_down);
prob_map->insertPointCloud(box, BitVoxelMeaning(29));
box.transformSelf(&shift_diag_down);
prob_map->insertPointCloud(box, eBVM_MAX_FREE_PROB); // = 4
box.transformSelf(&shift_diag_down);
prob_map->insertPointCloud(box, eBVM_MAX_FREE_PROB); // = 4
boxpoints = createBoxOfPoints( Vector3f(10, 5, 5), Vector3f(12, 130, 130), 0.9); // choose large delta, so that only 1 point falls into each voxel (mostly)
box = PointCloud(boxpoints);
prob_map->insertPointCloud(box, eBVM_UNCERTAIN_OCC_PROB); // = 129
// this will not influence voxels which were also set to other probabilities, as it converts to adding 0 to their values.
while(true)
{
gvl->visualizeMap("myProbVoxelMap");
// this will show partly overlapping cubes, whose occupancy values will get summed up.
// all "unknown" voxels will not get drawn, but the uncertain ones will (unkown = initialization value, uncertain = 0.5 probability)
sleep(1);
}
return 0;
}
int main(int argc, char* argv[])
{
signal(SIGINT, ctrlchandler);
signal(SIGTERM, killhandler);
icl_core::logging::initialize(argc, argv);
/*
* First, we generate an API class, which defines the
* volume of our space and the resolution.
* Be careful here! The size is limited by the memory
* of your GPU. Even if an empty Octree is small, a
* Voxelmap will always require the full memory.
*/
gvl = GpuVoxels::getInstance();
gvl->initialize(200, 200, 200, 0.01);
// Now we add some maps
gvl->addMap(MT_PROBAB_VOXELMAP, "myProbabVoxmap");
gvl->addMap(MT_BITVECTOR_VOXELMAP, "myBitmapVoxmap");
gvl->addMap(MT_BITVECTOR_OCTREE, "myOctree");
gvl->addMap(MT_PROBAB_VOXELMAP, "myCoordinateSystemMap");
// And two different primitive types
gvl->addPrimitives(primitive_array::ePRIM_SPHERE, "myPrims");
gvl->addPrimitives(primitive_array::ePRIM_CUBOID, "mySecondPrims");
std::vector<Vector4f> prim_positions(1000);
std::vector<Vector4i> prim_positions2(1000);
// These coordinates are used for three boxes that are inserted into the maps
Vector3f center1_min(0.5,0.5,0.5);
Vector3f center1_max(0.6,0.6,0.6);
Vector3f center2_min(0.5,0.5,0.5);
Vector3f center2_max(0.6,0.6,0.6);
Vector3f center3_min(0.5,0.5,0.5);
Vector3f center3_max(0.6,0.6,0.6);
Vector3f corner1_min;
Vector3f corner2_min;
Vector3f corner3_min;
Vector3f corner1_max;
Vector3f corner2_max;
Vector3f corner3_max;
// We load the model of a coordinate system.
if (!gvl->insertPointCloudFromFile("myCoordinateSystemMap", "coordinate_system_100.binvox", true,
eBVM_OCCUPIED, true, Vector3f(0, 0, 0),0.5))
{
LOGGING_WARNING(Gpu_voxels, "Could not insert the PCD file..." << endl);
}
/*
* Now we start the main loop, that will animate the scene.
*/
float t = 0.0;
int j = 0;
while(true)
{
// Calculate new positions for the boxes
float x = sin(t);
float y = cos(t);
t += 0.03;
corner1_min = center1_min + Vector3f(0.2 * x, 0.2 * y, 0);
corner1_max = center1_max + Vector3f(0.2 * x, 0.2 * y, 0);
gvl->insertBoxIntoMap(corner1_min, corner1_max, "myProbabVoxmap", eBVM_OCCUPIED, 2);
corner2_min = center2_min + Vector3f(0.0, 0.2 * x, 0.2 * y);
corner2_max = center2_max + Vector3f(0.0, 0.2 * x, 0.2 * y);
gvl->insertBoxIntoMap(corner3_min, corner3_max, "myBitmapVoxmap", eBVM_OCCUPIED, 2);
corner3_min = center3_min + Vector3f(0.2 * x, 0.0, 0.2 * y);
corner3_max = center3_max + Vector3f(0.2 * x, 0.0, 0.2 * y);
gvl->insertBoxIntoMap(corner2_min, corner2_max, "myOctree", eBVM_OCCUPIED, 2);
// generate info on the occuring collisions:
LOGGING_INFO(
Gpu_voxels, "Collsions myProbabVoxmap + myBitmapVoxmap: " << gvl->getMap("myProbabVoxmap")->as<voxelmap::ProbVoxelMap>()->collideWith(gvl->getMap("myBitmapVoxmap")->as<voxelmap::BitVectorVoxelMap>()) << endl <<
"Collsions myOctree + myBitmapVoxmap: " << gvl->getMap("myOctree")->as<NTree::GvlNTreeDet>()->collideWith(gvl->getMap("myBitmapVoxmap")->as<voxelmap::BitVectorVoxelMap>()) << endl <<
"Collsions myOctree + myProbabVoxmap: " << gvl->getMap("myOctree")->as<NTree::GvlNTreeDet>()->collideWith(gvl->getMap("myProbabVoxmap")->as<voxelmap::ProbVoxelMap>()) << endl);
// tell the visualier that the maps have changed
gvl->visualizeMap("myProbabVoxmap");
gvl->visualizeMap("myBitmapVoxmap");
gvl->visualizeMap("myOctree");
gvl->visualizeMap("myCoordinateSystemMap");
// update the primitves:
for(size_t i = 0; i < prim_positions.size(); i++)
{
// x, y, z, size
prim_positions[i] = Vector4f(0.2 + (i / 250.0), 0.2 + (sin(i/5.0)/50.0), (sin(j/5.0) / 50.0), 0.01);
prim_positions2[i] = Vector4i(20 + (sin(i/5.0)/0.5), 20 + (sin(j/5.0) / 0.5), i / 2.5, 1);
j++;
}
gvl->modifyPrimitives("myPrims", prim_positions);
gvl->modifyPrimitives("mySecondPrims", prim_positions2);
// tell the visualizier that the data has changed:
gvl->visualizePrimitivesArray("myPrims");
gvl->visualizePrimitivesArray("mySecondPrims");
usleep(30000);
// Reset the maps:
gvl->clearMap("myProbabVoxmap");
gvl->clearMap("myBitmapVoxmap");
gvl->clearMap("myOctree");
}
}
int main(int argc, char* argv[])
{
signal(SIGINT, ctrlchandler);
signal(SIGTERM, killhandler);
icl_core::logging::initialize(argc, argv);
gvl = GpuVoxels::getInstance();
gvl->initialize(200, 200, 100, 3.0);
gvl->addMap(MT_BITVECTOR_VOXELLIST, "voxellist");
// This should result in two lines of points with equal spacing inbetween the points of each line.
// The Primitives hover a bit above the pointcloud points.
std::vector<Vector3f> listPoints;
listPoints.push_back(Vector3f(30.0f, 9.0f, 12.0f));
listPoints.push_back(Vector3f(30.0f,15.0f, 12.0f));
listPoints.push_back(Vector3f(30.0f,21.0f, 12.0f));
listPoints.push_back(Vector3f(30.0f,27.0f, 12.0f));
std::vector<Vector3f> metric3Point;
metric3Point.push_back(Vector3f(30.0f,9.0f,9.0f));
std::vector<Vector4f> metric4Point;
metric4Point.push_back(Vector4f(30.0f,15.0f,9.0f,3.0f));
std::vector<Vector3i> voxel3Point;
voxel3Point.push_back(Vector3i(10,7,3));
std::vector<Vector4i> voxel4Point;
voxel4Point.push_back(Vector4i(10,9,3,1));
gvl->insertPointCloudIntoMap(listPoints, "voxellist", BitVoxelMeaning(1));
gvl->addPrimitives(primitive_array::ePRIM_SPHERE, "metric3Sphere");
bool prim = gvl->modifyPrimitives("metric3Sphere", metric3Point, 3.0f);
gvl->addPrimitives(primitive_array::ePRIM_SPHERE, "metric4Sphere");
prim = gvl->modifyPrimitives("metric4Sphere", metric4Point);
gvl->addPrimitives(primitive_array::ePRIM_SPHERE, "voxel3Sphere");
prim = gvl->modifyPrimitives("voxel3Sphere", voxel3Point, 1);
gvl->addPrimitives(primitive_array::ePRIM_SPHERE, "voxel4Sphere");
prim = gvl->modifyPrimitives("voxel4Sphere", voxel4Point);
std::cout << "Entering Draw Loop : " << prim << std::endl;
while(true)
{
gvl->visualizeMap("voxellist");
gvl->visualizePrimitivesArray("metric3Sphere");
gvl->visualizePrimitivesArray("metric4Sphere");
gvl->visualizePrimitivesArray("voxel3Sphere");
gvl->visualizePrimitivesArray("voxel4Sphere");
}
}
void killhandler(int)
{
//delete gvl;
gvl.reset();
exit(EXIT_SUCCESS);
}
