C++ GpuVoxelsSharedPtr类代码示例

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);

    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);  //
//.........这里部分代码省略.........

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);

//.........这里部分代码省略.........

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);
}