C++ GpuVoxelsSharedPtr::getMap方法代码示例

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

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

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

}

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

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