<commit_before>/*
 * Vulkan Samples
 *
 * Copyright (C) 2016 Valve Corporation
 * Copyright (C) 2016 LunarG, Inc.
 * Copyright (C) 2016 Google, Inc.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included
 * in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 */

/*
VULKAN_SAMPLE_SHORT_DESCRIPTION
Create and use a pipeline cache accross runs.
*/

#include <util_init.hpp>
#include <assert.h>
#include <string.h>
#include <cstdlib>
#include "cube_data.h"

// This sample tries to save and reuse pipeline cache data between runs
// On first run, no cache will be found, it will be created and saved
// to disk. On later runs, the cache should be found, loaded, and used.
// Hopefully a speedup will observed.  In the future, the pipeline could
// be complicated a bit, to show a greater cache benefit.  Also, two
// pipelines could be created and merged.

const char *vertShaderText =
    "#version 400\n"
    "#extension GL_ARB_separate_shader_objects : enable\n"
    "#extension GL_ARB_shading_language_420pack : enable\n"
    "layout (std140, binding = 0) uniform buf {\n"
    "        mat4 mvp;\n"
    "} ubuf;\n"
    "layout (location = 0) in vec4 pos;\n"
    "layout (location = 1) in vec2 inTexCoords;\n"
    "layout (location = 0) out vec2 texcoord;\n"
    "out gl_PerVertex { \n"
    "    vec4 gl_Position;\n"
    "};\n"
    "void main() {\n"
    "   texcoord = inTexCoords;\n"
    "   gl_Position = ubuf.mvp * pos;\n"
    "\n"
    "   // GL->VK conventions\n"
    "   gl_Position.y = -gl_Position.y;\n"
    "   gl_Position.z = (gl_Position.z + gl_Position.w) / 2.0;\n"
    "}\n";

const char *fragShaderText =
    "#version 400\n"
    "#extension GL_ARB_separate_shader_objects : enable\n"
    "#extension GL_ARB_shading_language_420pack : enable\n"
    "layout (binding = 1) uniform sampler2D tex;\n"
    "layout (location = 0) in vec2 texcoord;\n"
    "layout (location = 0) out vec4 outColor;\n"
    "void main() {\n"
    "   outColor = textureLod(tex, texcoord, 0.0);\n"
    "}\n";

int sample_main() {
    VkResult U_ASSERT_ONLY res;
    struct sample_info info = {};
    char sample_title[] = "Pipeline Cache";
    const bool depthPresent = true;

    init_global_layer_properties(info);
    init_instance_extension_names(info);
    init_device_extension_names(info);
    init_instance(info, sample_title);
    init_enumerate_device(info);
    init_window_size(info, 500, 500);
    init_connection(info);
    init_window(info);
    init_swapchain_extension(info);
    init_device(info);
    init_command_pool(info);
    init_command_buffer(info);
    execute_begin_command_buffer(info);
    init_device_queue(info);
    init_swap_chain(info);
    init_depth_buffer(info);
    init_texture(info, "blue.ppm");
    init_uniform_buffer(info);
    init_descriptor_and_pipeline_layouts(info, true);
    init_renderpass(info, depthPresent);
    init_shaders(info, vertShaderText, fragShaderText);
    init_framebuffers(info, depthPresent);
    init_vertex_buffer(info, g_vb_texture_Data, sizeof(g_vb_texture_Data),
                       sizeof(g_vb_texture_Data[0]), true);
    init_descriptor_pool(info, true);
    init_descriptor_set(info, true);

    /* VULKAN_KEY_START */

    // Check disk for existing cache data
    size_t startCacheSize = 0;
    void *startCacheData = nullptr;

    const char *readFileName = "pipeline_cache_data.bin";
    FILE *pReadFile = fopen(readFileName, "rb");

    if (pReadFile) {

        // Determine cache size
        fseek(pReadFile, 0, SEEK_END);
        startCacheSize = ftell(pReadFile);
        rewind(pReadFile);

        // Allocate memory to hold the initial cache data
        startCacheData = (char *)malloc(sizeof(char) * startCacheSize);
        if (startCacheData == nullptr) {
            fputs("Memory error", stderr);
            exit(EXIT_FAILURE);
        }

        // Read the data into our buffer
        size_t result = fread(startCacheData, 1, startCacheSize, pReadFile);
        if (result != startCacheSize) {
            fputs("Reading error", stderr);
            free(startCacheData);
            exit(EXIT_FAILURE);
        }

        // Clean up and print results
        fclose(pReadFile);
        printf("  Pipeline cache HIT!\n");
        printf("  cacheData loaded from %s\n", readFileName);

    } else {
        // No cache found on disk
        printf("  Pipeline cache miss!\n");
    }

    if (startCacheData != nullptr) {
        // clang-format off
        //
        // Check for cache validity
        //
        // TODO: Update this as the spec evolves. The fields are not defined by the header.
        //
        // The code below supports SDK 0.10 Vulkan spec, which contains the following table:
        //
        // Offset	 Size            Meaning
        // ------    ------------    ------------------------------------------------------------------
        //      0               4    a device ID equal to VkPhysicalDeviceProperties::DeviceId written
        //                           as a stream of bytes, with the least significant byte first
        //
        //      4    VK_UUID_SIZE    a pipeline cache ID equal to VkPhysicalDeviceProperties::pipelineCacheUUID
        //
        //
        // The code must be updated for latest Vulkan spec, which contains the following table:
        //
        // Offset	 Size            Meaning
        // ------    ------------    ------------------------------------------------------------------
        //      0               4    length in bytes of the entire pipeline cache header written as a
        //                           stream of bytes, with the least significant byte first
        //      4               4    a VkPipelineCacheHeaderVersion value written as a stream of bytes,
        //                           with the least significant byte first
        //      8               4    a vendor ID equal to VkPhysicalDeviceProperties::vendorID written
        //                           as a stream of bytes, with the least significant byte first
        //     12               4    a device ID equal to VkPhysicalDeviceProperties::deviceID written
        //                           as a stream of bytes, with the least significant byte first
        //     16    VK_UUID_SIZE    a pipeline cache ID equal to VkPhysicalDeviceProperties::pipelineCacheUUID
        //
        // clang-format on
        uint32_t headerLength = 0;
        uint32_t cacheHeaderVersion = 0;
        uint32_t vendorID = 0;
        uint32_t deviceID = 0;
        uint8_t pipelineCacheUUID[VK_UUID_SIZE] = {};

        memcpy(&headerLength, (uint8_t *)startCacheData + 0, 4);
        memcpy(&cacheHeaderVersion, (uint8_t *)startCacheData + 4, 4);
        memcpy(&vendorID, (uint8_t *)startCacheData + 8, 4);
        memcpy(&deviceID, (uint8_t *)startCacheData + 12, 4);
        memcpy(pipelineCacheUUID, (uint8_t *)startCacheData + 16, VK_UUID_SIZE);

        // Check each field and report bad values before freeing existing cache
        bool badCache = false;

        if (headerLength <= 0) {
            badCache = true;
            printf("  Bad header length in %s.\n", readFileName);
            printf("    Cache contains: 0x%.8x\n", headerLength);
        }

        if (cacheHeaderVersion != VK_PIPELINE_CACHE_HEADER_VERSION_ONE) {
            badCache = true;
            printf("  Unsupported cache header version in %s.\n", readFileName);
            printf("    Cache contains: 0x%.8x\n", cacheHeaderVersion);
        }

        if (vendorID != info.gpu_props.vendorID) {
            badCache = true;
            printf("  Vendor ID mismatch in %s.\n", readFileName);
            printf("    Cache contains: 0x%.8x\n", vendorID);
            printf("    Driver expects: 0x%.8x\n", info.gpu_props.vendorID);
        }

        if (deviceID != info.gpu_props.deviceID) {
            badCache = true;
            printf("  Device ID mismatch in %s.\n", readFileName);
            printf("    Cache contains: 0x%.8x\n", deviceID);
            printf("    Driver expects: 0x%.8x\n", info.gpu_props.deviceID);
        }

        if (memcmp(pipelineCacheUUID, info.gpu_props.pipelineCacheUUID,
                   sizeof(pipelineCacheUUID)) != 0) {
            badCache = true;
            printf("  UUID mismatch in %s.\n", readFileName);
            printf("    Cache contains: ");
            print_UUID(pipelineCacheUUID);
            printf("\n");
            printf("    Driver expects: ");
            print_UUID(info.gpu_props.pipelineCacheUUID);
            printf("\n");
        }

        if (badCache) {
            // Don't submit initial cache data if any version info is incorrect
            free(startCacheData);
            startCacheSize = 0;
            startCacheData = nullptr;

            // And clear out the old cache file for use in next run
            printf("  Deleting cache entry %s to repopulate.\n", readFileName);
            if (remove(readFileName) != 0) {
                fputs("Reading error", stderr);
                exit(EXIT_FAILURE);
            }
        }
    }

    // Feed the initial cache data into pipeline creation
    VkPipelineCacheCreateInfo pipelineCache;
    pipelineCache.sType = VK_STRUCTURE_TYPE_PIPELINE_CACHE_CREATE_INFO;
    pipelineCache.pNext = NULL;
    pipelineCache.initialDataSize = startCacheSize;
    pipelineCache.pInitialData = startCacheData;
    pipelineCache.flags = 0;
    res = vkCreatePipelineCache(info.device, &pipelineCache, nullptr,
                                &info.pipelineCache);
    assert(res == VK_SUCCESS);

    // Free our initialData now that pipeline has been created
    free(startCacheData);

    // Time (roughly) taken to create the graphics pipeline
    timestamp_t start = get_milliseconds();
    init_pipeline(info, depthPresent);
    timestamp_t elapsed = get_milliseconds() - start;
    printf("  vkCreateGraphicsPipeline time: %0.f ms\n", (double)elapsed);

    // Begin standard draw stuff

    init_presentable_image(info);
    VkClearValue clear_values[2];
    init_clear_color_and_depth(info, clear_values);
    VkRenderPassBeginInfo rp_begin;
    init_render_pass_begin_info(info, rp_begin);
    rp_begin.clearValueCount = 2;
    rp_begin.pClearValues = clear_values;
    vkCmdBeginRenderPass(info.cmd, &rp_begin, VK_SUBPASS_CONTENTS_INLINE);
    vkCmdBindPipeline(info.cmd, VK_PIPELINE_BIND_POINT_GRAPHICS, info.pipeline);
    vkCmdBindDescriptorSets(info.cmd, VK_PIPELINE_BIND_POINT_GRAPHICS,
                            info.pipeline_layout, 0, NUM_DESCRIPTOR_SETS,
                            info.desc_set.data(), 0, NULL);
    const VkDeviceSize offsets[1] = {0};
    vkCmdBindVertexBuffers(info.cmd, 0, 1, &info.vertex_buffer.buf, offsets);
    init_viewports(info);
    init_scissors(info);
    vkCmdDraw(info.cmd, 12 * 3, 1, 0, 0);
    vkCmdEndRenderPass(info.cmd);
    execute_pre_present_barrier(info);
    res = vkEndCommandBuffer(info.cmd);
    assert(res == VK_SUCCESS);
    VkFence drawFence = {};
    init_fence(info, drawFence);
    VkPipelineStageFlags pipe_stage_flags =
        VK_PIPELINE_STAGE_BOTTOM_OF_PIPE_BIT;
    VkSubmitInfo submit_info = {};
    init_submit_info(info, submit_info, pipe_stage_flags);
    /* Queue the command buffer for execution */
    res = vkQueueSubmit(info.queue, 1, &submit_info, drawFence);
    assert(res == VK_SUCCESS);
    /* Now present the image in the window */
    VkPresentInfoKHR present = {};
    init_present_info(info, present);
    /* Make sure command buffer is finished before presenting */
    do {
        res =
            vkWaitForFences(info.device, 1, &drawFence, VK_TRUE, FENCE_TIMEOUT);
    } while (res == VK_TIMEOUT);
    assert(res == VK_SUCCESS);
    res = vkQueuePresentKHR(info.queue, &present);
    assert(res == VK_SUCCESS);
    wait_seconds(1);

    // End standard draw stuff

    if (startCacheData) {
        // TODO: Create another pipeline, preferably different from the first
        // one and merge it here.  Then store the merged one.
    }

    // Store away the cache that we've populated.  This could conceivably happen
    // earlier, depends on when the pipeline cache stops being populated
    // internally.
    size_t endCacheSize = 0;
    void *endCacheData = nullptr;

    // Call with nullptr to get cache size
    vkGetPipelineCacheData(info.device, info.pipelineCache, &endCacheSize,
                           nullptr);

    // Allocate memory to hold the populated cache data
    endCacheData = (char *)malloc(sizeof(char) * endCacheSize);
    if (!endCacheData) {
        fputs("Memory error", stderr);
        exit(EXIT_FAILURE);
    }

    // Call again with pointer to buffer
    vkGetPipelineCacheData(info.device, info.pipelineCache, &endCacheSize,
                           endCacheData);

    // Write the file to disk, overwriting whatever was there
    FILE *pWriteFile;
    const char *writeFileName = "pipeline_cache_data.bin";
    pWriteFile = fopen(writeFileName, "wb");
    if (pWriteFile) {
        fwrite(endCacheData, sizeof(char), endCacheSize, pWriteFile);
        fclose(pWriteFile);
        printf("  cacheData written to %s\n", writeFileName);
    } else {
        // Something bad happened
        printf("  Unable to write cache data to disk!\n");
    }

    /* VULKAN_KEY_END */

    vkDestroyFence(info.device, drawFence, NULL);
    vkDestroySemaphore(info.device, info.presentCompleteSemaphore, NULL);
    destroy_pipeline(info);
    destroy_pipeline_cache(info);
    destroy_textures(info);
    destroy_descriptor_pool(info);
    destroy_vertex_buffer(info);
    destroy_framebuffers(info);
    destroy_shaders(info);
    destroy_renderpass(info);
    destroy_descriptor_and_pipeline_layouts(info);
    destroy_uniform_buffer(info);
    destroy_depth_buffer(info);
    destroy_swap_chain(info);
    destroy_command_buffer(info);
    destroy_command_pool(info);
    destroy_device(info);
    destroy_window(info);
    destroy_instance(info);
    return 0;
}
<commit_msg>Check return value from vkGetPipelineCacheData in pipeline_cache<commit_after>/*
 * Vulkan Samples
 *
 * Copyright (C) 2016 Valve Corporation
 * Copyright (C) 2016 LunarG, Inc.
 * Copyright (C) 2016 Google, Inc.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included
 * in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 */

/*
VULKAN_SAMPLE_SHORT_DESCRIPTION
Create and use a pipeline cache accross runs.
*/

#include <util_init.hpp>
#include <assert.h>
#include <string.h>
#include <cstdlib>
#include "cube_data.h"

// This sample tries to save and reuse pipeline cache data between runs
// On first run, no cache will be found, it will be created and saved
// to disk. On later runs, the cache should be found, loaded, and used.
// Hopefully a speedup will observed.  In the future, the pipeline could
// be complicated a bit, to show a greater cache benefit.  Also, two
// pipelines could be created and merged.

const char *vertShaderText =
    "#version 400\n"
    "#extension GL_ARB_separate_shader_objects : enable\n"
    "#extension GL_ARB_shading_language_420pack : enable\n"
    "layout (std140, binding = 0) uniform buf {\n"
    "        mat4 mvp;\n"
    "} ubuf;\n"
    "layout (location = 0) in vec4 pos;\n"
    "layout (location = 1) in vec2 inTexCoords;\n"
    "layout (location = 0) out vec2 texcoord;\n"
    "out gl_PerVertex { \n"
    "    vec4 gl_Position;\n"
    "};\n"
    "void main() {\n"
    "   texcoord = inTexCoords;\n"
    "   gl_Position = ubuf.mvp * pos;\n"
    "\n"
    "   // GL->VK conventions\n"
    "   gl_Position.y = -gl_Position.y;\n"
    "   gl_Position.z = (gl_Position.z + gl_Position.w) / 2.0;\n"
    "}\n";

const char *fragShaderText =
    "#version 400\n"
    "#extension GL_ARB_separate_shader_objects : enable\n"
    "#extension GL_ARB_shading_language_420pack : enable\n"
    "layout (binding = 1) uniform sampler2D tex;\n"
    "layout (location = 0) in vec2 texcoord;\n"
    "layout (location = 0) out vec4 outColor;\n"
    "void main() {\n"
    "   outColor = textureLod(tex, texcoord, 0.0);\n"
    "}\n";

int sample_main() {
    VkResult U_ASSERT_ONLY res;
    struct sample_info info = {};
    char sample_title[] = "Pipeline Cache";
    const bool depthPresent = true;

    init_global_layer_properties(info);
    init_instance_extension_names(info);
    init_device_extension_names(info);
    init_instance(info, sample_title);
    init_enumerate_device(info);
    init_window_size(info, 500, 500);
    init_connection(info);
    init_window(info);
    init_swapchain_extension(info);
    init_device(info);
    init_command_pool(info);
    init_command_buffer(info);
    execute_begin_command_buffer(info);
    init_device_queue(info);
    init_swap_chain(info);
    init_depth_buffer(info);
    init_texture(info, "blue.ppm");
    init_uniform_buffer(info);
    init_descriptor_and_pipeline_layouts(info, true);
    init_renderpass(info, depthPresent);
    init_shaders(info, vertShaderText, fragShaderText);
    init_framebuffers(info, depthPresent);
    init_vertex_buffer(info, g_vb_texture_Data, sizeof(g_vb_texture_Data),
                       sizeof(g_vb_texture_Data[0]), true);
    init_descriptor_pool(info, true);
    init_descriptor_set(info, true);

    /* VULKAN_KEY_START */

    // Check disk for existing cache data
    size_t startCacheSize = 0;
    void *startCacheData = nullptr;

    const char *readFileName = "pipeline_cache_data.bin";
    FILE *pReadFile = fopen(readFileName, "rb");

    if (pReadFile) {

        // Determine cache size
        fseek(pReadFile, 0, SEEK_END);
        startCacheSize = ftell(pReadFile);
        rewind(pReadFile);

        // Allocate memory to hold the initial cache data
        startCacheData = (char *)malloc(sizeof(char) * startCacheSize);
        if (startCacheData == nullptr) {
            fputs("Memory error", stderr);
            exit(EXIT_FAILURE);
        }

        // Read the data into our buffer
        size_t result = fread(startCacheData, 1, startCacheSize, pReadFile);
        if (result != startCacheSize) {
            fputs("Reading error", stderr);
            free(startCacheData);
            exit(EXIT_FAILURE);
        }

        // Clean up and print results
        fclose(pReadFile);
        printf("  Pipeline cache HIT!\n");
        printf("  cacheData loaded from %s\n", readFileName);

    } else {
        // No cache found on disk
        printf("  Pipeline cache miss!\n");
    }

    if (startCacheData != nullptr) {
        // clang-format off
        //
        // Check for cache validity
        //
        // TODO: Update this as the spec evolves. The fields are not defined by the header.
        //
        // The code below supports SDK 0.10 Vulkan spec, which contains the following table:
        //
        // Offset	 Size            Meaning
        // ------    ------------    ------------------------------------------------------------------
        //      0               4    a device ID equal to VkPhysicalDeviceProperties::DeviceId written
        //                           as a stream of bytes, with the least significant byte first
        //
        //      4    VK_UUID_SIZE    a pipeline cache ID equal to VkPhysicalDeviceProperties::pipelineCacheUUID
        //
        //
        // The code must be updated for latest Vulkan spec, which contains the following table:
        //
        // Offset	 Size            Meaning
        // ------    ------------    ------------------------------------------------------------------
        //      0               4    length in bytes of the entire pipeline cache header written as a
        //                           stream of bytes, with the least significant byte first
        //      4               4    a VkPipelineCacheHeaderVersion value written as a stream of bytes,
        //                           with the least significant byte first
        //      8               4    a vendor ID equal to VkPhysicalDeviceProperties::vendorID written
        //                           as a stream of bytes, with the least significant byte first
        //     12               4    a device ID equal to VkPhysicalDeviceProperties::deviceID written
        //                           as a stream of bytes, with the least significant byte first
        //     16    VK_UUID_SIZE    a pipeline cache ID equal to VkPhysicalDeviceProperties::pipelineCacheUUID
        //
        // clang-format on
        uint32_t headerLength = 0;
        uint32_t cacheHeaderVersion = 0;
        uint32_t vendorID = 0;
        uint32_t deviceID = 0;
        uint8_t pipelineCacheUUID[VK_UUID_SIZE] = {};

        memcpy(&headerLength, (uint8_t *)startCacheData + 0, 4);
        memcpy(&cacheHeaderVersion, (uint8_t *)startCacheData + 4, 4);
        memcpy(&vendorID, (uint8_t *)startCacheData + 8, 4);
        memcpy(&deviceID, (uint8_t *)startCacheData + 12, 4);
        memcpy(pipelineCacheUUID, (uint8_t *)startCacheData + 16, VK_UUID_SIZE);

        // Check each field and report bad values before freeing existing cache
        bool badCache = false;

        if (headerLength <= 0) {
            badCache = true;
            printf("  Bad header length in %s.\n", readFileName);
            printf("    Cache contains: 0x%.8x\n", headerLength);
        }

        if (cacheHeaderVersion != VK_PIPELINE_CACHE_HEADER_VERSION_ONE) {
            badCache = true;
            printf("  Unsupported cache header version in %s.\n", readFileName);
            printf("    Cache contains: 0x%.8x\n", cacheHeaderVersion);
        }

        if (vendorID != info.gpu_props.vendorID) {
            badCache = true;
            printf("  Vendor ID mismatch in %s.\n", readFileName);
            printf("    Cache contains: 0x%.8x\n", vendorID);
            printf("    Driver expects: 0x%.8x\n", info.gpu_props.vendorID);
        }

        if (deviceID != info.gpu_props.deviceID) {
            badCache = true;
            printf("  Device ID mismatch in %s.\n", readFileName);
            printf("    Cache contains: 0x%.8x\n", deviceID);
            printf("    Driver expects: 0x%.8x\n", info.gpu_props.deviceID);
        }

        if (memcmp(pipelineCacheUUID, info.gpu_props.pipelineCacheUUID,
                   sizeof(pipelineCacheUUID)) != 0) {
            badCache = true;
            printf("  UUID mismatch in %s.\n", readFileName);
            printf("    Cache contains: ");
            print_UUID(pipelineCacheUUID);
            printf("\n");
            printf("    Driver expects: ");
            print_UUID(info.gpu_props.pipelineCacheUUID);
            printf("\n");
        }

        if (badCache) {
            // Don't submit initial cache data if any version info is incorrect
            free(startCacheData);
            startCacheSize = 0;
            startCacheData = nullptr;

            // And clear out the old cache file for use in next run
            printf("  Deleting cache entry %s to repopulate.\n", readFileName);
            if (remove(readFileName) != 0) {
                fputs("Reading error", stderr);
                exit(EXIT_FAILURE);
            }
        }
    }

    // Feed the initial cache data into pipeline creation
    VkPipelineCacheCreateInfo pipelineCache;
    pipelineCache.sType = VK_STRUCTURE_TYPE_PIPELINE_CACHE_CREATE_INFO;
    pipelineCache.pNext = NULL;
    pipelineCache.initialDataSize = startCacheSize;
    pipelineCache.pInitialData = startCacheData;
    pipelineCache.flags = 0;
    res = vkCreatePipelineCache(info.device, &pipelineCache, nullptr,
                                &info.pipelineCache);
    assert(res == VK_SUCCESS);

    // Free our initialData now that pipeline has been created
    free(startCacheData);

    // Time (roughly) taken to create the graphics pipeline
    timestamp_t start = get_milliseconds();
    init_pipeline(info, depthPresent);
    timestamp_t elapsed = get_milliseconds() - start;
    printf("  vkCreateGraphicsPipeline time: %0.f ms\n", (double)elapsed);

    // Begin standard draw stuff

    init_presentable_image(info);
    VkClearValue clear_values[2];
    init_clear_color_and_depth(info, clear_values);
    VkRenderPassBeginInfo rp_begin;
    init_render_pass_begin_info(info, rp_begin);
    rp_begin.clearValueCount = 2;
    rp_begin.pClearValues = clear_values;
    vkCmdBeginRenderPass(info.cmd, &rp_begin, VK_SUBPASS_CONTENTS_INLINE);
    vkCmdBindPipeline(info.cmd, VK_PIPELINE_BIND_POINT_GRAPHICS, info.pipeline);
    vkCmdBindDescriptorSets(info.cmd, VK_PIPELINE_BIND_POINT_GRAPHICS,
                            info.pipeline_layout, 0, NUM_DESCRIPTOR_SETS,
                            info.desc_set.data(), 0, NULL);
    const VkDeviceSize offsets[1] = {0};
    vkCmdBindVertexBuffers(info.cmd, 0, 1, &info.vertex_buffer.buf, offsets);
    init_viewports(info);
    init_scissors(info);
    vkCmdDraw(info.cmd, 12 * 3, 1, 0, 0);
    vkCmdEndRenderPass(info.cmd);
    execute_pre_present_barrier(info);
    res = vkEndCommandBuffer(info.cmd);
    assert(res == VK_SUCCESS);
    VkFence drawFence = {};
    init_fence(info, drawFence);
    VkPipelineStageFlags pipe_stage_flags =
        VK_PIPELINE_STAGE_BOTTOM_OF_PIPE_BIT;
    VkSubmitInfo submit_info = {};
    init_submit_info(info, submit_info, pipe_stage_flags);
    /* Queue the command buffer for execution */
    res = vkQueueSubmit(info.queue, 1, &submit_info, drawFence);
    assert(res == VK_SUCCESS);
    /* Now present the image in the window */
    VkPresentInfoKHR present = {};
    init_present_info(info, present);
    /* Make sure command buffer is finished before presenting */
    do {
        res =
            vkWaitForFences(info.device, 1, &drawFence, VK_TRUE, FENCE_TIMEOUT);
    } while (res == VK_TIMEOUT);
    assert(res == VK_SUCCESS);
    res = vkQueuePresentKHR(info.queue, &present);
    assert(res == VK_SUCCESS);
    wait_seconds(1);

    // End standard draw stuff

    if (startCacheData) {
        // TODO: Create another pipeline, preferably different from the first
        // one and merge it here.  Then store the merged one.
    }

    // Store away the cache that we've populated.  This could conceivably happen
    // earlier, depends on when the pipeline cache stops being populated
    // internally.
    size_t endCacheSize = 0;
    void *endCacheData = nullptr;

    // Call with nullptr to get cache size
    res = vkGetPipelineCacheData(info.device, info.pipelineCache, &endCacheSize,
                           nullptr);
    assert(res == VK_SUCCESS);

    // Allocate memory to hold the populated cache data
    endCacheData = (char *)malloc(sizeof(char) * endCacheSize);
    if (!endCacheData) {
        fputs("Memory error", stderr);
        exit(EXIT_FAILURE);
    }

    // Call again with pointer to buffer
    res = vkGetPipelineCacheData(info.device, info.pipelineCache, &endCacheSize,
                           endCacheData);
    assert(res == VK_SUCCESS);

    // Write the file to disk, overwriting whatever was there
    FILE *pWriteFile;
    const char *writeFileName = "pipeline_cache_data.bin";
    pWriteFile = fopen(writeFileName, "wb");
    if (pWriteFile) {
        fwrite(endCacheData, sizeof(char), endCacheSize, pWriteFile);
        fclose(pWriteFile);
        printf("  cacheData written to %s\n", writeFileName);
    } else {
        // Something bad happened
        printf("  Unable to write cache data to disk!\n");
    }

    /* VULKAN_KEY_END */

    vkDestroyFence(info.device, drawFence, NULL);
    vkDestroySemaphore(info.device, info.presentCompleteSemaphore, NULL);
    destroy_pipeline(info);
    destroy_pipeline_cache(info);
    destroy_textures(info);
    destroy_descriptor_pool(info);
    destroy_vertex_buffer(info);
    destroy_framebuffers(info);
    destroy_shaders(info);
    destroy_renderpass(info);
    destroy_descriptor_and_pipeline_layouts(info);
    destroy_uniform_buffer(info);
    destroy_depth_buffer(info);
    destroy_swap_chain(info);
    destroy_command_buffer(info);
    destroy_command_pool(info);
    destroy_device(info);
    destroy_window(info);
    destroy_instance(info);
    return 0;
}
<|endoftext|>


<commit_before>// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is licensed under the Apache License, Version 2.0 (the
//  "License"); you may not use this file except in compliance with the
//  License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

/// \file rpc_gen.cc
///
/// This program will generate an RPC model from a DG model.
/// It will work by creating a 3D grid in the provided lon-lat-height box,
/// at each of those points finding the corresponding pixel value, and
/// then finding the best-fitting RPC transform. For increased
/// accuracy, both lon-lat-height and pixel values are normalized.
///
/// Reference: The Cubic Rational Polynomial Camera Model, Hartley, 2001.

#include <fstream>

#include <asp/Sessions/RPC/RPCModelGen.h>
#include <asp/Sessions/RPC/StereoSessionRPC.h>
#include <asp/Sessions/RPC/RPCStereoModel.h>
#include <asp/Sessions/DG/XML.h>
#include <asp/Core/Macros.h>
#include <asp/Core/Common.h>
namespace po = boost::program_options;
namespace fs = boost::filesystem;

using namespace vw;
using namespace asp;
using namespace xercesc;

/// Structure for storing user options
struct RPC_gen_Options : asp::BaseOptions {
  // Input
  std::string camera_model;
  // Settings
  double penalty_weight;
  BBox3  lon_lat_height_box;
  std::string output_prefix;
};

/// Parse input arguments
void handle_arguments( int argc, char *argv[], RPC_gen_Options& opt ) {
  po::options_description general_options("");
  general_options.add_options()
    ("output-prefix",      po::value(&opt.output_prefix)->default_value(""),
             "Write diagnostic/report files to this location.  If not set then no files will be written.")
    ("penalty-weight",     po::value(&opt.penalty_weight)->default_value(0.1),
             "Penalty weight to use to keep the higher-order RPC coefficients small. Higher penalty weight results in smaller such coefficients.")
    ("lon-lat-height-box", po::value(&opt.lon_lat_height_box)->default_value(BBox3(0,0,0,0,0,0)),
             "The 3D region in which to solve for the RPC model [lon_min lat_min height_min lon_max lat_max height_max].");
  general_options.add( asp::BaseOptionsDescription(opt) );

  po::options_description positional("");
  positional.add_options()
    ("camera-model", po::value(&opt.camera_model));
  po::positional_options_description positional_desc;
  positional_desc.add("camera-model", 1);

  std::string usage("[options] <camera_model>");
  bool allow_unregistered = false;
  std::vector<std::string> unregistered;
  po::variables_map vm =
    asp::check_command_line( argc, argv, opt, general_options, general_options,
                             positional, positional_desc, usage,
                             allow_unregistered, unregistered );

  if ( !vm.count("camera-model") )
    vw_throw( ArgumentErr() << "Requires <camera_model> input in order to proceed.\n\n"
              << usage << general_options );

}

/// Print out a name followed by the vector of values
void print_vec(std::string const& name, Vector<double> const& vals){
  std::cout.precision(16);
  std::cout << name << ",";
  int len = vals.size();
  for (int i = 0; i < len - 1; i++)
    std::cout << vals[i] << ",";
  if (len > 0)
    std::cout << vals[len-1];
  std::cout << std::endl;
}

/// Dump a vector to a text file, one value per row.
void print_vec_to_file(std::string const& path, Vector<double> const& vals) {
  std::ofstream outFile(path.c_str());
  outFile.precision(16);
  int len = vals.size();
  for (int i = 0; i < len - 1; i++)
    outFile << vals[i] << std::endl;
  if (len > 0)
    outFile << vals[len-1];
  outFile.close();

}

void write_levmar_solver_results(std::string const& output_prefix, int status,
                                 Vector<double> const& initial_params,
                                 Vector<double> const& final_params,
                                 Vector<double> const& actual_observation,
                                 RpcSolveLMA const& lma_model) {

  // Compute initial and final numbers
  Vector<double> initial_projected = lma_model(initial_params);
  Vector<double> final_projected   = lma_model(final_params);
  Vector<double> initial_error     = lma_model.difference(initial_projected, actual_observation);
  Vector<double> final_error       = lma_model.difference(final_projected,   actual_observation);

  // Log the solver status
  VW_OUT(VerboseDebugMessage, "math") << "rpc_gen: levmar solver status = " << status << std::endl;
  VW_OUT(VerboseDebugMessage, "math") << "rpc_gen: levmar solver initial error norm_2 = " << norm_2(initial_error) << std::endl;
  VW_OUT(VerboseDebugMessage, "math") << "rpc_gen: levmar solver final   error norm_2 = " << norm_2(final_error  ) << std::endl;

  //// Dump the values to file
  print_vec_to_file(output_prefix + "_initial_parameters.csv", initial_params);
  print_vec_to_file(output_prefix + "_final_parameters.csv",   final_params);
  print_vec_to_file(output_prefix + "_initial_projected.csv", initial_projected);
  print_vec_to_file(output_prefix + "_final_projected.csv",   final_projected);
  print_vec_to_file(output_prefix + "_initial_error.csv",     initial_error);
  print_vec_to_file(output_prefix + "_final_error.csv",       final_error);

  //// Also add the results to the log
  //VW_OUT(VerboseDebugMessage, "math") << "LM: starting proj  " << initial_projected << std::endl;
  //VW_OUT(VerboseDebugMessage, "math") << "LM: final    proj  " << final_projected   << std::endl;
  //VW_OUT(VerboseDebugMessage, "math") << "LM: starting error " << initial_error     << std::endl;
  //VW_OUT(VerboseDebugMessage, "math") << "LM: final    error " << final_error       << std::endl;
}

/// Computes a system solution from a seed and returns the final error number.
int find_solution_from_seed(RpcSolveLMA    const& lma_model,
                            Vector<double> const& seed_params,
                            Vector<double> const& actual_observations,
                            Vector<double>      & final_params,
                            double              & norm_error) {

    // Initialize a zero vector of RPC model coefficients
    int status;

    // Use the L-M solver to optimize the RPC model coefficient values.
    const double abs_tolerance  = 1e-24;
    const double rel_tolerance  = 1e-24;
    const int    max_iterations = 2000;
    final_params = math::levenberg_marquardt( lma_model, seed_params, actual_observations, status,
                                              abs_tolerance, rel_tolerance, max_iterations );

    if (status < 1) { // This means the solver failed to converge!
      VW_OUT(DebugMessage, "math") << "rpc_gen: WARNING --> levmar solver status = " << status << std::endl;
    }

    // Otherwise the solver converged, return the final error number.
    Vector<double> final_projected = lma_model(final_params);
    Vector<double> final_error     = lma_model.difference(final_projected, actual_observations);
    norm_error = norm_2(final_error);
    return status;
}

int main( int argc, char* argv[] ) {

  RPC_gen_Options opt;
  try {
    handle_arguments( argc, argv, opt );

    VW_ASSERT( opt.penalty_weight >= 0, ArgumentErr() << "The RPC penalty weight must be non-negative.\n" );

    // Load up the Digital Globe camera model from the camera file
    XMLPlatformUtils::Initialize();
    StereoSessionDG session;
    boost::shared_ptr<camera::CameraModel> cam_dg( session.camera_model("", opt.camera_model) );

    // Load up the RPC camera model from the camera file
    RPCXML xml;
    xml.read_from_file( opt.camera_model );
    boost::shared_ptr<RPCModel> cam_rpc( new RPCModel( *xml.rpc_ptr() ) );
    Vector2 image_size = xml_image_size(opt.camera_model);

    // Normalization in the lon-lat-height and pixel domains
    // - Compute an offset and scale to place all values in the
    //   zero to one range centered on the center coordinate/pixel.
    // - The input RPC model has similar numbers loaded but we will use different
    //   values unless the input options exactly match the input XML file.
    Vector3 min_llh_coord = opt.lon_lat_height_box.min();
    Vector3 max_llh_coord = opt.lon_lat_height_box.max();

    // Use matched axis scaling for pixels so one axis does not get higher error weighting
    Vector3 llh_scale     = (max_llh_coord - min_llh_coord)/2.0; // half range
    Vector3 llh_offset    = (max_llh_coord + min_llh_coord)/2.0; // center point
    double pixel_max = vw::math::max(image_size);
    Vector2 uv_scale(pixel_max/2.0, pixel_max/2.0); // The long axis pixel is scaled to 1.0
    Vector2 uv_offset     = image_size/2.0; // center point

    // Number of points in x and y at which we will optimize the RPC
    // model. Using 10 or 20 points gives roughly similar results.
    // 20 points result in 20^3 input data for optimization, with the
    // number of variable to optimize being just 78.
    int num_pts = 20; // The number of points per axis
    int num_total_pts = num_pts*num_pts*num_pts;

    // Initialize normalized data storage
    Vector<double> normalizedGeodetics;
    normalizedGeodetics.set_size(RPCModel::GEODETIC_COORD_SIZE*num_total_pts);
    Vector<double> normalizedPixels;
    normalizedPixels.set_size(RPCModel::IMAGE_COORD_SIZE*num_total_pts + RpcSolveLMA::NUM_PENALTY_TERMS);
    for (int i = 0; i < (int)normalizedPixels.size(); i++)
      normalizedPixels[i] = 0.0; // Important: The extra penalty terms are all set to zero here.

    // Loop through all test points and generate the "correct" pairs / training data
    //  using the trusted DG camera model.
    int count = 0;
    for (int x = 0; x < num_pts; x++){
      for (int y = 0; y < num_pts; y++){
        for (int z = 0; z < num_pts; z++){

          // Test points are evenly spaced through the x/y/z -1 <> 1 range
          Vector3 U( x/(num_pts - 1.0),
                     y/(num_pts - 1.0),
                     z/(num_pts - 1.0) );
                  U = 2.0*U - Vector3(1, 1, 1); // in the box [-1, 1]^3.

          // Linear conversion from x/y/z to lat/lon/height
          Vector3 G   = elem_prod(U, llh_scale) + llh_offset; // geodetic

          // Convert from geodetic to geocentric coordinates
          Vector3 P   = cam_rpc->datum().geodetic_to_cartesian(G); // xyz

          // Project the GCC coordinate into the DG camera model
          Vector2 pxg = cam_dg->point_to_pixel(P);
          //Vector2 pxg = cam_rpc->point_to_pixel(P); // DEBUG -> Try fitting to RPC!

          // Normalize the pixel to -1 <> 1 range
          Vector2 pxn = elem_quot(pxg - uv_offset, uv_scale);

          // It is a useful exercise to compare DG and RPC cameras
          //Vector2 pxr = cam_rpc->point_to_pixel(P);
          //std::cout << U << ' ' << P << ' ' << pxg << ' ' << pxr  << ' '
          //          << norm_2(pxg-pxr)<< std::endl;

          subvector(normalizedGeodetics, RPCModel::GEODETIC_COORD_SIZE*count, RPCModel::GEODETIC_COORD_SIZE) = U;
          subvector(normalizedPixels,    RPCModel::IMAGE_COORD_SIZE   *count, RPCModel::IMAGE_COORD_SIZE   ) = pxn;
          count++;

        } // End z loop
      } // End y loop
    } // End x loop

    double penalty_weight_fraction = opt.penalty_weight; // The percentage of the error that the penalty weights should represent
    double native_penalty_fraction = (double)RpcSolveLMA::NUM_PENALTY_TERMS / (double)normalizedPixels.size(); // Fraction with no adjustment
    double penalty_adjustment      = penalty_weight_fraction / native_penalty_fraction;

    VW_OUT(DebugMessage, "math") << "rpc_gen: Computed penalty weight: " << penalty_adjustment<< std::endl;

    // Initialize a specialized least squares solver object and load the input data
    RpcSolveLMA lma_model (normalizedGeodetics, normalizedPixels, penalty_adjustment);

    int status;
    Vector<double> solution;
    double norm_error;

    // Initialize a zero vector of RPC model coefficients
    Vector<double> startZero;
    startZero.set_size(RPCModel::NUM_RPC_COEFFS);
    for (int i = 0; i < (int)startZero.size(); i++)
      startZero[i] = 0.0;

    // Use the L-M solver to optimize the RPC model coefficient values.
    //VW_OUT(DebugMessage, "math") << "rpc_gen: Solving with zero seed" << std::endl;
    status = find_solution_from_seed(lma_model, startZero, normalizedPixels, solution, norm_error);
    VW_OUT(DebugMessage, "math") << "rpc_gen: norm_error = " << norm_error << std::endl;

    // If we ever want to improve our results further we should experiment with multiple starting seeds!

    // Dump all the results to disk if the user passed in an output prefix.
    if (opt.output_prefix != "")
      write_levmar_solver_results(opt.output_prefix, status, startZero, solution, normalizedPixels, lma_model);

    // Dump the output to stdout, to be parsed by python
    RPCModel::CoeffVec line_num, line_den, samp_num, samp_den;
    unpackCoeffs(solution, line_num, line_den, samp_num, samp_den);
    print_vec("uv_scale",   uv_scale  );
    print_vec("uv_offset",  uv_offset );
    print_vec("llh_scale",  llh_scale );
    print_vec("llh_offset", llh_offset);
    print_vec("line_num",   line_num  );
    print_vec("line_den",   line_den  );
    print_vec("samp_num",   samp_num  );
    print_vec("samp_den",   samp_den  );

  } ASP_STANDARD_CATCHES;

  return 0;
}

<commit_msg>rpc_gen: Avoid printing unnecessary debug files<commit_after>// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is licensed under the Apache License, Version 2.0 (the
//  "License"); you may not use this file except in compliance with the
//  License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

/// \file rpc_gen.cc
///
/// This program will generate an RPC model from a DG model.
/// It will work by creating a 3D grid in the provided lon-lat-height box,
/// at each of those points finding the corresponding pixel value, and
/// then finding the best-fitting RPC transform. For increased
/// accuracy, both lon-lat-height and pixel values are normalized.
///
/// Reference: The Cubic Rational Polynomial Camera Model, Hartley, 2001.

#include <fstream>

#include <asp/Sessions/RPC/RPCModelGen.h>
#include <asp/Sessions/RPC/StereoSessionRPC.h>
#include <asp/Sessions/RPC/RPCStereoModel.h>
#include <asp/Sessions/DG/XML.h>
#include <asp/Core/Macros.h>
#include <asp/Core/Common.h>
namespace po = boost::program_options;
namespace fs = boost::filesystem;

using namespace vw;
using namespace asp;
using namespace xercesc;

/// Structure for storing user options
struct RPC_gen_Options : asp::BaseOptions {
  // Input
  std::string camera_model;
  // Settings
  double penalty_weight;
  BBox3  lon_lat_height_box;
  std::string output_prefix;
};

/// Parse input arguments
void handle_arguments( int argc, char *argv[], RPC_gen_Options& opt ) {
  po::options_description general_options("");
  general_options.add_options()
    ("output-prefix",      po::value(&opt.output_prefix)->default_value(""),
             "Write diagnostic/report files to this location.  If not set then no files will be written.")
    ("penalty-weight",     po::value(&opt.penalty_weight)->default_value(0.1),
             "Penalty weight to use to keep the higher-order RPC coefficients small. Higher penalty weight results in smaller such coefficients.")
    ("lon-lat-height-box", po::value(&opt.lon_lat_height_box)->default_value(BBox3(0,0,0,0,0,0)),
             "The 3D region in which to solve for the RPC model [lon_min lat_min height_min lon_max lat_max height_max].");
  general_options.add( asp::BaseOptionsDescription(opt) );

  po::options_description positional("");
  positional.add_options()
    ("camera-model", po::value(&opt.camera_model));
  po::positional_options_description positional_desc;
  positional_desc.add("camera-model", 1);

  std::string usage("[options] <camera_model>");
  bool allow_unregistered = false;
  std::vector<std::string> unregistered;
  po::variables_map vm =
    asp::check_command_line( argc, argv, opt, general_options, general_options,
                             positional, positional_desc, usage,
                             allow_unregistered, unregistered );

  if ( !vm.count("camera-model") )
    vw_throw( ArgumentErr() << "Requires <camera_model> input in order to proceed.\n\n"
              << usage << general_options );

}

/// Print out a name followed by the vector of values
void print_vec(std::string const& name, Vector<double> const& vals){
  std::cout.precision(16);
  std::cout << name << ",";
  int len = vals.size();
  for (int i = 0; i < len - 1; i++)
    std::cout << vals[i] << ",";
  if (len > 0)
    std::cout << vals[len-1];
  std::cout << std::endl;
}

/// Dump a vector to a text file, one value per row.
void print_vec_to_file(std::string const& path, Vector<double> const& vals) {
  std::ofstream outFile(path.c_str());
  outFile.precision(16);
  int len = vals.size();
  for (int i = 0; i < len - 1; i++)
    outFile << vals[i] << std::endl;
  if (len > 0)
    outFile << vals[len-1];
  outFile.close();

}

void write_levmar_solver_results(std::string const& output_prefix, int status,
                                 Vector<double> const& initial_params,
                                 Vector<double> const& final_params,
                                 Vector<double> const& actual_observation,
                                 RpcSolveLMA const& lma_model) {

  // Compute initial and final numbers
  Vector<double> initial_projected = lma_model(initial_params);
  Vector<double> final_projected   = lma_model(final_params);
  Vector<double> initial_error     = lma_model.difference(initial_projected, actual_observation);
  Vector<double> final_error       = lma_model.difference(final_projected,   actual_observation);

  // Log the solver status
  VW_OUT(VerboseDebugMessage, "math") << "rpc_gen: levmar solver status = " << status << std::endl;
  VW_OUT(VerboseDebugMessage, "math") << "rpc_gen: levmar solver initial error norm_2 = " << norm_2(initial_error) << std::endl;
  VW_OUT(VerboseDebugMessage, "math") << "rpc_gen: levmar solver final   error norm_2 = " << norm_2(final_error  ) << std::endl;

  //// Dump the values to file
  //print_vec_to_file(output_prefix + "_initial_parameters.csv", initial_params);
  //print_vec_to_file(output_prefix + "_final_parameters.csv",   final_params);
  //print_vec_to_file(output_prefix + "_initial_projected.csv", initial_projected);
  //print_vec_to_file(output_prefix + "_final_projected.csv",   final_projected);
  //print_vec_to_file(output_prefix + "_initial_error.csv",     initial_error);
  //print_vec_to_file(output_prefix + "_final_error.csv",       final_error);

  //// Also add the results to the log
  //VW_OUT(VerboseDebugMessage, "math") << "LM: starting proj  " << initial_projected << std::endl;
  //VW_OUT(VerboseDebugMessage, "math") << "LM: final    proj  " << final_projected   << std::endl;
  //VW_OUT(VerboseDebugMessage, "math") << "LM: starting error " << initial_error     << std::endl;
  //VW_OUT(VerboseDebugMessage, "math") << "LM: final    error " << final_error       << std::endl;
}

/// Computes a system solution from a seed and returns the final error number.
int find_solution_from_seed(RpcSolveLMA    const& lma_model,
                            Vector<double> const& seed_params,
                            Vector<double> const& actual_observations,
                            Vector<double>      & final_params,
                            double              & norm_error) {

    // Initialize a zero vector of RPC model coefficients
    int status;

    // Use the L-M solver to optimize the RPC model coefficient values.
    const double abs_tolerance  = 1e-24;
    const double rel_tolerance  = 1e-24;
    const int    max_iterations = 2000;
    final_params = math::levenberg_marquardt( lma_model, seed_params, actual_observations, status,
                                              abs_tolerance, rel_tolerance, max_iterations );

    if (status < 1) { // This means the solver failed to converge!
      VW_OUT(DebugMessage, "math") << "rpc_gen: WARNING --> levmar solver status = " << status << std::endl;
    }

    // Otherwise the solver converged, return the final error number.
    Vector<double> final_projected = lma_model(final_params);
    Vector<double> final_error     = lma_model.difference(final_projected, actual_observations);
    norm_error = norm_2(final_error);
    return status;
}

int main( int argc, char* argv[] ) {

  RPC_gen_Options opt;
  try {
    handle_arguments( argc, argv, opt );

    VW_ASSERT( opt.penalty_weight >= 0, ArgumentErr() << "The RPC penalty weight must be non-negative.\n" );

    // Load up the Digital Globe camera model from the camera file
    XMLPlatformUtils::Initialize();
    StereoSessionDG session;
    boost::shared_ptr<camera::CameraModel> cam_dg( session.camera_model("", opt.camera_model) );

    // Load up the RPC camera model from the camera file
    RPCXML xml;
    xml.read_from_file( opt.camera_model );
    boost::shared_ptr<RPCModel> cam_rpc( new RPCModel( *xml.rpc_ptr() ) );
    Vector2 image_size = xml_image_size(opt.camera_model);

    // Normalization in the lon-lat-height and pixel domains
    // - Compute an offset and scale to place all values in the
    //   zero to one range centered on the center coordinate/pixel.
    // - The input RPC model has similar numbers loaded but we will use different
    //   values unless the input options exactly match the input XML file.
    Vector3 min_llh_coord = opt.lon_lat_height_box.min();
    Vector3 max_llh_coord = opt.lon_lat_height_box.max();

    // Use matched axis scaling for pixels so one axis does not get higher error weighting
    Vector3 llh_scale     = (max_llh_coord - min_llh_coord)/2.0; // half range
    Vector3 llh_offset    = (max_llh_coord + min_llh_coord)/2.0; // center point
    double pixel_max = vw::math::max(image_size);
    Vector2 uv_scale(pixel_max/2.0, pixel_max/2.0); // The long axis pixel is scaled to 1.0
    Vector2 uv_offset     = image_size/2.0; // center point

    // Number of points in x and y at which we will optimize the RPC
    // model. Using 10 or 20 points gives roughly similar results.
    // 20 points result in 20^3 input data for optimization, with the
    // number of variable to optimize being just 78.
    int num_pts = 20; // The number of points per axis
    int num_total_pts = num_pts*num_pts*num_pts;

    // Initialize normalized data storage
    Vector<double> normalizedGeodetics;
    normalizedGeodetics.set_size(RPCModel::GEODETIC_COORD_SIZE*num_total_pts);
    Vector<double> normalizedPixels;
    normalizedPixels.set_size(RPCModel::IMAGE_COORD_SIZE*num_total_pts + RpcSolveLMA::NUM_PENALTY_TERMS);
    for (int i = 0; i < (int)normalizedPixels.size(); i++)
      normalizedPixels[i] = 0.0; // Important: The extra penalty terms are all set to zero here.

    // Loop through all test points and generate the "correct" pairs / training data
    //  using the trusted DG camera model.
    int count = 0;
    for (int x = 0; x < num_pts; x++){
      for (int y = 0; y < num_pts; y++){
        for (int z = 0; z < num_pts; z++){

          // Test points are evenly spaced through the x/y/z -1 <> 1 range
          Vector3 U( x/(num_pts - 1.0),
                     y/(num_pts - 1.0),
                     z/(num_pts - 1.0) );
                  U = 2.0*U - Vector3(1, 1, 1); // in the box [-1, 1]^3.

          // Linear conversion from x/y/z to lat/lon/height
          Vector3 G   = elem_prod(U, llh_scale) + llh_offset; // geodetic

          // Convert from geodetic to geocentric coordinates
          Vector3 P   = cam_rpc->datum().geodetic_to_cartesian(G); // xyz

          // Project the GCC coordinate into the DG camera model
          Vector2 pxg = cam_dg->point_to_pixel(P);
          //Vector2 pxg = cam_rpc->point_to_pixel(P); // DEBUG -> Try fitting to RPC!

          // Normalize the pixel to -1 <> 1 range
          Vector2 pxn = elem_quot(pxg - uv_offset, uv_scale);

          // It is a useful exercise to compare DG and RPC cameras
          //Vector2 pxr = cam_rpc->point_to_pixel(P);
          //std::cout << U << ' ' << P << ' ' << pxg << ' ' << pxr  << ' '
          //          << norm_2(pxg-pxr)<< std::endl;

          subvector(normalizedGeodetics, RPCModel::GEODETIC_COORD_SIZE*count, RPCModel::GEODETIC_COORD_SIZE) = U;
          subvector(normalizedPixels,    RPCModel::IMAGE_COORD_SIZE   *count, RPCModel::IMAGE_COORD_SIZE   ) = pxn;
          count++;

        } // End z loop
      } // End y loop
    } // End x loop

    double penalty_weight_fraction = opt.penalty_weight; // The percentage of the error that the penalty weights should represent
    double native_penalty_fraction = (double)RpcSolveLMA::NUM_PENALTY_TERMS / (double)normalizedPixels.size(); // Fraction with no adjustment
    double penalty_adjustment      = penalty_weight_fraction / native_penalty_fraction;

    VW_OUT(DebugMessage, "math") << "rpc_gen: Computed penalty weight: " << penalty_adjustment<< std::endl;

    // Initialize a specialized least squares solver object and load the input data
    RpcSolveLMA lma_model (normalizedGeodetics, normalizedPixels, penalty_adjustment);

    int status;
    Vector<double> solution;
    double norm_error;

    // Initialize a zero vector of RPC model coefficients
    Vector<double> startZero;
    startZero.set_size(RPCModel::NUM_RPC_COEFFS);
    for (int i = 0; i < (int)startZero.size(); i++)
      startZero[i] = 0.0;

    // Use the L-M solver to optimize the RPC model coefficient values.
    //VW_OUT(DebugMessage, "math") << "rpc_gen: Solving with zero seed" << std::endl;
    status = find_solution_from_seed(lma_model, startZero, normalizedPixels, solution, norm_error);
    VW_OUT(DebugMessage, "math") << "rpc_gen: norm_error = " << norm_error << std::endl;

    // If we ever want to improve our results further we should experiment with multiple starting seeds!

    // Dump all the results to disk if the user passed in an output prefix.
    if (opt.output_prefix != "")
      write_levmar_solver_results(opt.output_prefix, status, startZero, solution, normalizedPixels, lma_model);

    // Dump the output to stdout, to be parsed by python
    RPCModel::CoeffVec line_num, line_den, samp_num, samp_den;
    unpackCoeffs(solution, line_num, line_den, samp_num, samp_den);
    print_vec("uv_scale",   uv_scale  );
    print_vec("uv_offset",  uv_offset );
    print_vec("llh_scale",  llh_scale );
    print_vec("llh_offset", llh_offset);
    print_vec("line_num",   line_num  );
    print_vec("line_den",   line_den  );
    print_vec("samp_num",   samp_num  );
    print_vec("samp_den",   samp_den  );

  } ASP_STANDARD_CATCHES;

  return 0;
}
<|endoftext|>


<commit_before>#include <stan/services/sample/hmc_nuts_diag_e_adapt.hpp>
#include <stan/io/empty_var_context.hpp>
#include <stan/callbacks/unique_stream_writer.hpp>
#include <test/unit/util.hpp>
#include <test/test-models/good/optimization/rosenbrock.hpp>
#include <test/unit/services/instrumented_callbacks.hpp>
#include <gtest/gtest.h>
#include <iostream>

auto&& blah = stan::math::init_threadpool_tbb();

namespace stan {
namespace test {
/**
 * Read a CSV into an Eigen matrix.
 * @param in An input string stream holding the CSV
 * @param rows Number of rows
 * @param cols Number of columns.
 */
Eigen::MatrixXd read_stan_sample_csv(std::istringstream& in, int rows, int cols) {
  std::string line;
  int row = 0;
  int col = 0;
  Eigen::MatrixXd res = Eigen::MatrixXd(rows, cols);
  while (std::getline(in, line)) {
    if (line.find("#") != std::string::npos) {
      continue;
    }
    const char* ptr = line.c_str();
    int len = line.length();
    col = 0;

    const char* start = ptr;
    for (int i = 0; i < len; i++) {
      if (ptr[i] == ',') {
        res(row, col++) = atof(start);
        start = ptr + i + 1;
      }
    }
    res(row, col) = atof(start);
    row++;
  }
  return res;
}
}  // namespace test
}  // namespace stan

static constexpr size_t num_chains = 4;
class ServicesSampleHmcNutsDiagEAdaptParMatch : public testing::Test {
 public:
  ServicesSampleHmcNutsDiagEAdaptParMatch()
      : model(std::make_unique<rosenbrock_model_namespace::rosenbrock_model>(
          data_context, 0, &model_log)) {
    for (int i = 0; i < num_chains; ++i) {
      init.push_back(stan::test::unit::instrumented_writer{});
      par_parameters.emplace_back(std::make_unique<std::stringstream>(), "#");
      seq_parameters.emplace_back(std::make_unique<std::stringstream>(), "#");
      diagnostic.push_back(stan::test::unit::instrumented_writer{});
      context.push_back(std::make_shared<stan::io::empty_var_context>());
    }
  }
  stan::io::empty_var_context data_context;
  std::stringstream model_log;
  stan::test::unit::instrumented_logger logger;
  std::vector<stan::test::unit::instrumented_writer> init;
  using str_writer = stan::callbacks::unique_stream_writer<std::stringstream>;
  std::vector<str_writer> par_parameters;
  std::vector<str_writer> seq_parameters;
  std::vector<stan::test::unit::instrumented_writer> diagnostic;
  std::vector<std::shared_ptr<stan::io::empty_var_context>> context;
  std::unique_ptr<rosenbrock_model_namespace::rosenbrock_model> model;
};

TEST_F(ServicesSampleHmcNutsDiagEAdaptParMatch, single_multi_match) {
  constexpr unsigned int random_seed = 0;
  constexpr unsigned int chain = 0;
  constexpr double init_radius = 0;
  constexpr int num_warmup = 200;
  constexpr int num_samples = 400;
  constexpr int num_thin = 5;
  constexpr bool save_warmup = true;
  constexpr int refresh = 0;
  constexpr double stepsize = 0.1;
  constexpr double stepsize_jitter = 0;
  constexpr int max_depth = 8;
  constexpr double delta = .1;
  constexpr double gamma = .1;
  constexpr double kappa = .1;
  constexpr double t0 = .1;
  constexpr unsigned int init_buffer = 50;
  constexpr unsigned int term_buffer = 50;
  constexpr unsigned int window = 100;
  stan::test::unit::instrumented_interrupt interrupt;
  EXPECT_EQ(interrupt.call_count(), 0);
  int return_code = stan::services::sample::hmc_nuts_diag_e_adapt(
      *model, num_chains, context, random_seed, chain, init_radius, num_warmup,
      num_samples, num_thin, save_warmup, refresh, stepsize, stepsize_jitter,
      max_depth, delta, gamma, kappa, t0, init_buffer, term_buffer, window,
      interrupt, logger, init, par_parameters, diagnostic);

  EXPECT_EQ(0, return_code);

  int num_output_lines = (num_warmup + num_samples) / num_thin;
  EXPECT_EQ((num_warmup + num_samples) * num_chains, interrupt.call_count());
  for (int i = 0; i < num_chains; ++i) {
    stan::test::unit::instrumented_writer seq_init;
    stan::test::unit::instrumented_writer seq_diagnostic;
    return_code = stan::services::sample::hmc_nuts_diag_e_adapt(
        *model, *(context[i]), random_seed, i, init_radius, num_warmup,
        num_samples, num_thin, save_warmup, refresh, stepsize, stepsize_jitter,
        max_depth, delta, gamma, kappa, t0, init_buffer, term_buffer, window,
        interrupt, logger, seq_init, seq_parameters[i], seq_diagnostic);
    EXPECT_EQ(0, return_code);
  }
  std::vector<Eigen::MatrixXd> par_res;
  for (int i = 0; i < num_chains; ++i) {
    auto par_str = par_parameters[i].get_stream().str();
    auto sub_par_str = par_str.substr(par_str.find("Diagonal") - 1);
    std::istringstream sub_par_stream(sub_par_str);
    Eigen::MatrixXd par_mat = stan::test::read_stan_sample_csv(sub_par_stream, 80, 9);
    par_res.push_back(par_mat);
  }
  std::vector<Eigen::MatrixXd> seq_res;
  for (int i = 0; i < num_chains; ++i) {
    auto seq_str = seq_parameters[i].get_stream().str();
    auto sub_seq_str = seq_str.substr(seq_str.find("Diagonal") - 1);
    std::istringstream sub_seq_stream(sub_seq_str);
    Eigen::MatrixXd seq_mat = stan::test::read_stan_sample_csv(sub_seq_stream, 80, 9);
    seq_res.push_back(seq_mat);
  }
  for (int i = 0; i < num_chains; ++i) {
    Eigen::MatrixXd diff_res
        = (par_res[i].array() - seq_res[i].array()).matrix();
    EXPECT_MATRIX_EQ(diff_res, Eigen::MatrixXd::Zero(80, 9));
  }
}
<commit_msg>[Jenkins] auto-formatting by clang-format version 6.0.0-1ubuntu2~16.04.1 (tags/RELEASE_600/final)<commit_after>#include <stan/services/sample/hmc_nuts_diag_e_adapt.hpp>
#include <stan/io/empty_var_context.hpp>
#include <stan/callbacks/unique_stream_writer.hpp>
#include <test/unit/util.hpp>
#include <test/test-models/good/optimization/rosenbrock.hpp>
#include <test/unit/services/instrumented_callbacks.hpp>
#include <gtest/gtest.h>
#include <iostream>

auto&& blah = stan::math::init_threadpool_tbb();

namespace stan {
namespace test {
/**
 * Read a CSV into an Eigen matrix.
 * @param in An input string stream holding the CSV
 * @param rows Number of rows
 * @param cols Number of columns.
 */
Eigen::MatrixXd read_stan_sample_csv(std::istringstream& in, int rows,
                                     int cols) {
  std::string line;
  int row = 0;
  int col = 0;
  Eigen::MatrixXd res = Eigen::MatrixXd(rows, cols);
  while (std::getline(in, line)) {
    if (line.find("#") != std::string::npos) {
      continue;
    }
    const char* ptr = line.c_str();
    int len = line.length();
    col = 0;

    const char* start = ptr;
    for (int i = 0; i < len; i++) {
      if (ptr[i] == ',') {
        res(row, col++) = atof(start);
        start = ptr + i + 1;
      }
    }
    res(row, col) = atof(start);
    row++;
  }
  return res;
}
}  // namespace test
}  // namespace stan

static constexpr size_t num_chains = 4;
class ServicesSampleHmcNutsDiagEAdaptParMatch : public testing::Test {
 public:
  ServicesSampleHmcNutsDiagEAdaptParMatch()
      : model(std::make_unique<rosenbrock_model_namespace::rosenbrock_model>(
            data_context, 0, &model_log)) {
    for (int i = 0; i < num_chains; ++i) {
      init.push_back(stan::test::unit::instrumented_writer{});
      par_parameters.emplace_back(std::make_unique<std::stringstream>(), "#");
      seq_parameters.emplace_back(std::make_unique<std::stringstream>(), "#");
      diagnostic.push_back(stan::test::unit::instrumented_writer{});
      context.push_back(std::make_shared<stan::io::empty_var_context>());
    }
  }
  stan::io::empty_var_context data_context;
  std::stringstream model_log;
  stan::test::unit::instrumented_logger logger;
  std::vector<stan::test::unit::instrumented_writer> init;
  using str_writer = stan::callbacks::unique_stream_writer<std::stringstream>;
  std::vector<str_writer> par_parameters;
  std::vector<str_writer> seq_parameters;
  std::vector<stan::test::unit::instrumented_writer> diagnostic;
  std::vector<std::shared_ptr<stan::io::empty_var_context>> context;
  std::unique_ptr<rosenbrock_model_namespace::rosenbrock_model> model;
};

TEST_F(ServicesSampleHmcNutsDiagEAdaptParMatch, single_multi_match) {
  constexpr unsigned int random_seed = 0;
  constexpr unsigned int chain = 0;
  constexpr double init_radius = 0;
  constexpr int num_warmup = 200;
  constexpr int num_samples = 400;
  constexpr int num_thin = 5;
  constexpr bool save_warmup = true;
  constexpr int refresh = 0;
  constexpr double stepsize = 0.1;
  constexpr double stepsize_jitter = 0;
  constexpr int max_depth = 8;
  constexpr double delta = .1;
  constexpr double gamma = .1;
  constexpr double kappa = .1;
  constexpr double t0 = .1;
  constexpr unsigned int init_buffer = 50;
  constexpr unsigned int term_buffer = 50;
  constexpr unsigned int window = 100;
  stan::test::unit::instrumented_interrupt interrupt;
  EXPECT_EQ(interrupt.call_count(), 0);
  int return_code = stan::services::sample::hmc_nuts_diag_e_adapt(
      *model, num_chains, context, random_seed, chain, init_radius, num_warmup,
      num_samples, num_thin, save_warmup, refresh, stepsize, stepsize_jitter,
      max_depth, delta, gamma, kappa, t0, init_buffer, term_buffer, window,
      interrupt, logger, init, par_parameters, diagnostic);

  EXPECT_EQ(0, return_code);

  int num_output_lines = (num_warmup + num_samples) / num_thin;
  EXPECT_EQ((num_warmup + num_samples) * num_chains, interrupt.call_count());
  for (int i = 0; i < num_chains; ++i) {
    stan::test::unit::instrumented_writer seq_init;
    stan::test::unit::instrumented_writer seq_diagnostic;
    return_code = stan::services::sample::hmc_nuts_diag_e_adapt(
        *model, *(context[i]), random_seed, i, init_radius, num_warmup,
        num_samples, num_thin, save_warmup, refresh, stepsize, stepsize_jitter,
        max_depth, delta, gamma, kappa, t0, init_buffer, term_buffer, window,
        interrupt, logger, seq_init, seq_parameters[i], seq_diagnostic);
    EXPECT_EQ(0, return_code);
  }
  std::vector<Eigen::MatrixXd> par_res;
  for (int i = 0; i < num_chains; ++i) {
    auto par_str = par_parameters[i].get_stream().str();
    auto sub_par_str = par_str.substr(par_str.find("Diagonal") - 1);
    std::istringstream sub_par_stream(sub_par_str);
    Eigen::MatrixXd par_mat
        = stan::test::read_stan_sample_csv(sub_par_stream, 80, 9);
    par_res.push_back(par_mat);
  }
  std::vector<Eigen::MatrixXd> seq_res;
  for (int i = 0; i < num_chains; ++i) {
    auto seq_str = seq_parameters[i].get_stream().str();
    auto sub_seq_str = seq_str.substr(seq_str.find("Diagonal") - 1);
    std::istringstream sub_seq_stream(sub_seq_str);
    Eigen::MatrixXd seq_mat
        = stan::test::read_stan_sample_csv(sub_seq_stream, 80, 9);
    seq_res.push_back(seq_mat);
  }
  for (int i = 0; i < num_chains; ++i) {
    Eigen::MatrixXd diff_res
        = (par_res[i].array() - seq_res[i].array()).matrix();
    EXPECT_MATRIX_EQ(diff_res, Eigen::MatrixXd::Zero(80, 9));
  }
}
<|endoftext|>


<commit_before>/* -------------------------------------------------------------------------- *
 *                    OpenSim:  testAnalyzeTutorialOne.cpp                    *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2017 Stanford University and the Authors                *
 * Author(s): Ayman Habib                                                     *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

// INCLUDE
#include <OpenSim/Simulation/Model/Model.h>
#include <OpenSim/Simulation/StatesTrajectory.h>
#include <OpenSim/Actuators/Millard2012EquilibriumMuscle.h>
#include <OpenSim/Tools/AnalyzeTool.h>
#include <OpenSim/Auxiliary/auxiliaryTestFunctions.h>

using namespace OpenSim;
using namespace std;

void testTutorialOne();

// Test different default activations are respected when activation
// states are not provided. Note if default_activation is 1.0 it
// reproduces  the "skyscraper" bug issue #83 in OpenSim32 repo
void testTugOfWar(const string& dataFileName, const double& defaultAct);

int main()
{
    SimTK::Array_<std::string> failures;

    try { /*testTutorialOne();*/ }
    catch (const std::exception& e) {
        cout << e.what() << endl; failures.push_back("testTutorialOne");
    }
    // produce passive force-length curve
    try { testTugOfWar("Tug_of_War_ConstantVelocity.sto", 0.01); }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testTugOfWar CoordinatesOnly: default_act = 0.01");
    }
    //produced passive + active but before skyscraper bug appears (at 0.9)
    try { testTugOfWar("Tug_of_War_ConstantVelocity.sto", 0.8); }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testTugOfWar CoordinatesOnly: default_act = 0.8");
    }
    // now supply activation states which should be used instead of the default
    try { testTugOfWar("Tug_of_War_ConstantVelocity_RampActivation.sto", 0.0); }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testTugOfWar with activation state provided");
    }

    if (!failures.empty()) {
        cout << "Done, with failure(s): " << failures << endl;
        return 1;
    }

    cout << "Done" << endl;
    return 0;
}

void testTutorialOne() {
    AnalyzeTool analyze1("PlotterTool.xml");
    analyze1.getModel().print("testAnalyzeTutorialOne.osim");
    analyze1.run();
    /* Once this runs to completion we'll make the test more meaningful by comparing output
    * to a validated standard. Let's make sure we don't crash during run first! -Ayman 5/29/12 */
    Storage resultFiberLength("testPlotterTool/BothLegs__FiberLength.sto");
    Storage standardFiberLength("std_BothLegs_fiberLength.sto");
    CHECK_STORAGE_AGAINST_STANDARD(resultFiberLength, standardFiberLength,
        std::vector<double>(100, 0.0001), __FILE__, __LINE__,
        "testAnalyzeTutorialOne failed");
    // const Model& mdl = analyze1.getModel();
    //mdl.updMultibodySystem()
    analyze1.setStatesFileName("plotterGeneratedStatesHip45.sto");
    //analyze1.setModel(mdl);
    analyze1.setName("BothLegsHip45");
    analyze1.run();
    Storage resultFiberLengthHip45("testPlotterTool/BothLegsHip45__FiberLength.sto");
    Storage standardFiberLength45("std_BothLegsHip45__FiberLength.sto");
    CHECK_STORAGE_AGAINST_STANDARD(resultFiberLengthHip45,
        standardFiberLength45, std::vector<double>(100, 0.0001),
        __FILE__, __LINE__, "testAnalyzeTutorialOne at Hip45 failed");
    cout << "testAnalyzeTutorialOne passed" << endl;
}

void testTugOfWar(const string& dataFileName, const double& defaultAct) {
    AnalyzeTool analyze("Tug_of_War_Setup_Analyze.xml");
    analyze.setCoordinatesFileName("");
    analyze.setStatesFileName("");

    // Access the model and muscle being analyzed
    Model& model = analyze.getModel();
    Millard2012EquilibriumMuscle& muscle =
        static_cast<Millard2012EquilibriumMuscle&>(model.updMuscles()[0]);
    bool isCoordinatesOnly = true;
    // Load in the States used to recompute the results of the Analysis
    Storage dataStore(dataFileName);
    if (dataStore.getColumnLabels().findIndex(muscle.getName() + ".activation") > 0) {
        isCoordinatesOnly = false;
        analyze.setStatesFileName(dataFileName);
        // ramp input starts at 0.0
        muscle.set_minimum_activation(0.0);
    }
    else {
        analyze.setCoordinatesFileName(dataFileName);
    }

    // Test that the default activation is taken into consideration by
    // the Analysis and reflected in the AnalyzeTool solution
    muscle.set_default_activation(defaultAct);
    analyze.run();

    // Load the AnalyzTool results for the muscle's force through time
    TimeSeriesTable_<double> results =
        STOFileAdapter_<double>::
        read("Analyze_Tug_of_War/Tug_of_War_Millard_Iso_ForceReporter_forces.sto");
    assert(results.getNumColumns() == 1);
    SimTK::Vector forces = results.getDependentColumnAtIndex(0);

    // Load input data as StatesTrajectory used to perform the Analysis
    auto statesTraj = StatesTrajectory::createFromStatesStorage(
        model, dataStore, true, false);
    size_t nstates = statesTraj.getSize();

    // muscle active, passive, total muscle and tendon force quantities
    double af, pf, mf, tf;
    af= pf = mf = tf = SimTK::NaN;

    // Tolerance for muscle equilibrium solution
    const double equilTol = muscle.getMaxIsometricForce()*SimTK::SqrtEps;

    SimTK::State s = model.getWorkingState();
    // Independently compute the active fiber force at every state
    for (int i = 0; i < nstates; ++i) {
        s = statesTraj[i];
        // When the muscle states are not supplied in the input dataStore
        // (isCoordinatesOnly == true), then set it to its default value.
        if (isCoordinatesOnly) {
            muscle.setActivation(s, muscle.get_default_activation());
        }
        // technically, fiber lengths could be supplied, but this test case
        // (a typical use case) does not and therefore set to its default.
        muscle.setFiberLength(s, muscle.get_default_fiber_length());
        muscle.computeEquilibrium(s);
        model.realizeDynamics(s);

        if (isCoordinatesOnly) {
            // check that activation is not reset to zero or other value
            SimTK_ASSERT_ALWAYS(muscle.getActivation(s) == defaultAct,
                "Test failed to correctly use the default activation value.");
        }
        else {
            // check that activation used was that supplied by the dataStore
            SimTK_ASSERT_ALWAYS(muscle.getActivation(s) == s.getTime(),
                "Test failed to correctly use the supplied activation values.");
        }

        // get active and passive forces given the default activation
        af = muscle.getActiveFiberForceAlongTendon(s);
        pf = muscle.getPassiveFiberForceAlongTendon(s);
        // now the total muscle force is the active + passive
        mf = af + pf;
        tf = muscle.getTendonForce(s);

        // equilibrium demands tendon and muscle fiber are equivalent
        ASSERT_EQUAL<double>(tf, mf, equilTol);
        // Verify that the current computed and AnalyzeTool reported force are
        // equivalent for the provided motion file
        cout << s.getTime() << " :: muscle-fiber-force: " << mf <<
            " Analyze reported force: " << forces[i] << endl;
        ASSERT_EQUAL<double>(mf, forces[i], equilTol, __FILE__, __LINE__,
            "Total fiber force failed to match reported muscle force.");
    }
}
<commit_msg>Restore testTutorialOne() test case<commit_after>/* -------------------------------------------------------------------------- *
 *                       OpenSim:  testAnalyzeTool.cpp                        *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2017 Stanford University and the Authors                *
 * Author(s): Ayman Habib, Ajay Seth                                          *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

// INCLUDE
#include <OpenSim/Simulation/Model/Model.h>
#include <OpenSim/Simulation/StatesTrajectory.h>
#include <OpenSim/Actuators/Millard2012EquilibriumMuscle.h>
#include <OpenSim/Tools/AnalyzeTool.h>
#include <OpenSim/Auxiliary/auxiliaryTestFunctions.h>

using namespace OpenSim;
using namespace std;

void testTutorialOne();

// Test different default activations are respected when activation
// states are not provided. Note if default_activation is 1.0 it
// reproduces  the "skyscraper" bug issue #83 in OpenSim32 repo
void testTugOfWar(const string& dataFileName, const double& defaultAct);

int main()
{
    SimTK::Array_<std::string> failures;

    try { testTutorialOne(); }
    catch (const std::exception& e) {
        cout << e.what() << endl; failures.push_back("testTutorialOne");
    }
    // produce passive force-length curve
    try { testTugOfWar("Tug_of_War_ConstantVelocity.sto", 0.01); }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testTugOfWar CoordinatesOnly: default_act = 0.01");
    }
    //produced passive + active but before skyscraper bug appears (at 0.9)
    try { testTugOfWar("Tug_of_War_ConstantVelocity.sto", 0.8); }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testTugOfWar CoordinatesOnly: default_act = 0.8");
    }
    // now supply activation states which should be used instead of the default
    try { testTugOfWar("Tug_of_War_ConstantVelocity_RampActivation.sto", 0.0); }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testTugOfWar with activation state provided");
    }

    if (!failures.empty()) {
        cout << "Done, with failure(s): " << failures << endl;
        return 1;
    }

    cout << "Done" << endl;
    return 0;
}

void testTutorialOne() {
    AnalyzeTool analyze1("PlotterTool.xml");
    analyze1.getModel().print("testAnalyzeTutorialOne.osim");
    analyze1.run();
    /* Once this runs to completion we'll make the test more meaningful by comparing output
    * to a validated standard. Let's make sure we don't crash during run first! -Ayman 5/29/12 */
    Storage resultFiberLength("testPlotterTool/BothLegs__FiberLength.sto");
    Storage standardFiberLength("std_BothLegs_fiberLength.sto");
    CHECK_STORAGE_AGAINST_STANDARD(resultFiberLength, standardFiberLength,
        std::vector<double>(100, 0.0001), __FILE__, __LINE__,
        "testAnalyzeTutorialOne failed");
    // const Model& mdl = analyze1.getModel();
    //mdl.updMultibodySystem()
    analyze1.setStatesFileName("plotterGeneratedStatesHip45.sto");
    //analyze1.setModel(mdl);
    analyze1.setName("BothLegsHip45");
    analyze1.run();
    Storage resultFiberLengthHip45("testPlotterTool/BothLegsHip45__FiberLength.sto");
    Storage standardFiberLength45("std_BothLegsHip45__FiberLength.sto");
    CHECK_STORAGE_AGAINST_STANDARD(resultFiberLengthHip45,
        standardFiberLength45, std::vector<double>(100, 0.0001),
        __FILE__, __LINE__, "testAnalyzeTutorialOne at Hip45 failed");
    cout << "testAnalyzeTutorialOne passed" << endl;
}

void testTugOfWar(const string& dataFileName, const double& defaultAct) {
    AnalyzeTool analyze("Tug_of_War_Setup_Analyze.xml");
    analyze.setCoordinatesFileName("");
    analyze.setStatesFileName("");

    // Access the model and muscle being analyzed
    Model& model = analyze.getModel();
    Millard2012EquilibriumMuscle& muscle =
        static_cast<Millard2012EquilibriumMuscle&>(model.updMuscles()[0]);
    bool isCoordinatesOnly = true;
    // Load in the States used to recompute the results of the Analysis
    Storage dataStore(dataFileName);
    if (dataStore.getColumnLabels().findIndex(muscle.getName() + ".activation") > 0) {
        isCoordinatesOnly = false;
        analyze.setStatesFileName(dataFileName);
        // ramp input starts at 0.0
        muscle.set_minimum_activation(0.0);
    }
    else {
        analyze.setCoordinatesFileName(dataFileName);
    }

    // Test that the default activation is taken into consideration by
    // the Analysis and reflected in the AnalyzeTool solution
    muscle.set_default_activation(defaultAct);
    analyze.run();

    // Load the AnalyzTool results for the muscle's force through time
    TimeSeriesTable_<double> results =
        STOFileAdapter_<double>::
        read("Analyze_Tug_of_War/Tug_of_War_Millard_Iso_ForceReporter_forces.sto");
    assert(results.getNumColumns() == 1);
    SimTK::Vector forces = results.getDependentColumnAtIndex(0);

    // Load input data as StatesTrajectory used to perform the Analysis
    auto statesTraj = StatesTrajectory::createFromStatesStorage(
        model, dataStore, true, false);
    size_t nstates = statesTraj.getSize();

    // muscle active, passive, total muscle and tendon force quantities
    double af, pf, mf, tf;
    af= pf = mf = tf = SimTK::NaN;

    // Tolerance for muscle equilibrium solution
    const double equilTol = muscle.getMaxIsometricForce()*SimTK::SqrtEps;

    SimTK::State s = model.getWorkingState();
    // Independently compute the active fiber force at every state
    for (int i = 0; i < nstates; ++i) {
        s = statesTraj[i];
        // When the muscle states are not supplied in the input dataStore
        // (isCoordinatesOnly == true), then set it to its default value.
        if (isCoordinatesOnly) {
            muscle.setActivation(s, muscle.get_default_activation());
        }
        // technically, fiber lengths could be supplied, but this test case
        // (a typical use case) does not and therefore set to its default.
        muscle.setFiberLength(s, muscle.get_default_fiber_length());
        muscle.computeEquilibrium(s);
        model.realizeDynamics(s);

        if (isCoordinatesOnly) {
            // check that activation is not reset to zero or other value
            SimTK_ASSERT_ALWAYS(muscle.getActivation(s) == defaultAct,
                "Test failed to correctly use the default activation value.");
        }
        else {
            // check that activation used was that supplied by the dataStore
            SimTK_ASSERT_ALWAYS(muscle.getActivation(s) == s.getTime(),
                "Test failed to correctly use the supplied activation values.");
        }

        // get active and passive forces given the default activation
        af = muscle.getActiveFiberForceAlongTendon(s);
        pf = muscle.getPassiveFiberForceAlongTendon(s);
        // now the total muscle force is the active + passive
        mf = af + pf;
        tf = muscle.getTendonForce(s);

        // equilibrium demands tendon and muscle fiber are equivalent
        ASSERT_EQUAL<double>(tf, mf, equilTol);
        // Verify that the current computed and AnalyzeTool reported force are
        // equivalent for the provided motion file
        cout << s.getTime() << " :: muscle-fiber-force: " << mf <<
            " Analyze reported force: " << forces[i] << endl;
        ASSERT_EQUAL<double>(mf, forces[i], equilTol, __FILE__, __LINE__,
            "Total fiber force failed to match reported muscle force.");
    }
}
<|endoftext|>