C++ Kinect::run方法代码示例

int main( int argc, char* argv[] )
{
    try{
        Kinect kinect;
        kinect.run();
    } catch( std::exception& ex ){
        std::cout << ex.what() << std::endl;
    }

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