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

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

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

}

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

}