Example #1
0
void doLsqr( int m,
            int n,
            double damp,
            void   *UsrWrk,
            double u[],    // len = m
            double v[],    // len = n
            double w[],    // len = n
            double x[],    // len = n
            FILE   *nout,
            // The remaining variables are output only.
            int    *istop_out,
            int    *itn_out,
            double *anorm_out,
            double *acond_out,
            double *rnorm_out,
            double *arnorm_out,
            double *xnorm_out )
{
    lsqr(  m,
          n,
         sparseMatrixVectorProduct,
         damp,
         UsrWrk,
         u,    // len = m
         v,    // len = n
         w,    // len = n
         x,    // len = n
         NULL,   // len = *
         1.0e-12,
         1.0e-12,
         10e6,
         1e7,
         nout,
         // The remaining variables are output only.
         istop_out,
         itn_out,
         anorm_out,
         acond_out,
         rnorm_out,
         arnorm_out,
         xnorm_out );
}
Example #2
0
int main(int argc, char *argv[])
{
    struct matrix_t *matrixA;
    struct sparse_matrix_t *sparseA;
    struct vector_t *x, *b;

    lsqr_input *input;
    lsqr_output *output;
    lsqr_work *work;            /* zone temoraire de travail */
    lsqr_func *func;            /* func->mat_vec_prod -> APROD */

    /* cmd line arg */
    char *matrix_filename = NULL;
    char *vector_filename = NULL;
    char *sol_filename = NULL;
    int max_iter = -1;
    float damping = 0;

    if (argc != 4) {
        fprintf(stderr, "%s matrixfile vectorfile solutionfile\n",
                argv[0]);
        exit(1);
    }
    matrix_filename = strdup(argv[1]);
    vector_filename = strdup(argv[2]);
    sol_filename = strdup(argv[3]);

    /* read the  matrix */
    matrixA = read_matrix(matrix_filename);
    fprintf(stderr, "read*matrix: ok (size=%ldx%ld, %ld elements)\n",
            matrixA->nb_line, matrixA->nb_col,
            matrixA->nb_line * matrixA->nb_col);

    sparseA = sparsify(matrixA, SPARSE_COL_LINK);
    b = read_simple_vector(vector_filename);

        /*************************************************/
    /* check compatibility between matrix and vector */

        /*************************************************/
    if (sparseA->nb_line != b->length) {
        fprintf(stderr,
                "Error, check your matrix/vector sizes (%ld/%ld)\n",
                sparseA->nb_line, b->length);
        exit(1);
    }
    /* init vector solution to zero */
    x = new_vector(sparseA->nb_col);

    /* catch Ctrl-C signal */
    signal(SIGINT, emergency_halt);

        /*************************************************************/
    /* solve A.x = B                                             */

        /*************************************************************/

    /* LSQR alloc */
    alloc_lsqr_mem(&input, &output, &work, &func,
                   sparseA->nb_line, sparseA->nb_col);

    fprintf(stderr, "alloc_lsqr_mem : ok\n");

    /* defines the routine Mat.Vect to use */
    func->mat_vec_prod = sparseMATRIXxVECTOR;

    /* Set the input parameters for LSQR */
    input->num_rows = sparseA->nb_line;
    input->num_cols = sparseA->nb_col;
    input->rel_mat_err = .0;
    input->rel_rhs_err = .0;
    input->cond_lim = .0;
    input->lsqr_fp_out = stdout;
    input->rhs_vec = (dvec *) b;
    input->sol_vec = (dvec *) x;        /* initial guess */
    input->damp_val = damping;
    if (max_iter == -1) {
        input->max_iter = 4 * (sparseA->nb_col);
    } else {
        input->max_iter = max_iter;
    }

    /* resolution du systeme Ax=b */
    lsqr(input, output, work, func, sparseA);
    write_vector((struct vector_t *) output->sol_vec, sol_filename);
    free_lsqr_mem(input, output, work, func);
    free_matrix(matrixA);

    /* check A^t.A */
    /*
     * { struct sparse_matrix_t *AtA; AtA = AtransA (sparseA);
     * write_sparse_matrix(AtA, "AtA"); write_sparse_matrix(sparseA,
     * "A"); free_sparse_matrix (AtA);
     * 
     * } */

    free_sparse_matrix(sparseA);
    return (1);
}
Example #3
0
int main(int argc, char* argv[])
{
  char *A_matrix_file = NULL;
  char *AT_matrix_file = NULL;
  char *vector_file = NULL;
  char *result_file = NULL;

  // input checks
  if (argc <= 4)
  {
    std::cout << "usage: " << argv[0] << " <A matrix file> <AT matrix file>"
              << " <rhs vector file> <result file>" << std::endl;
    return -1;
  } else {
    A_matrix_file = argv[1];
    AT_matrix_file = argv[2];
    vector_file = argv[3];
    result_file = argv[4]; 
  }

  // init the network
  agile::NetworkEnvironment environment(argc, argv);
  
  // allocate a GPU
  typedef agile::GPUCommunicator<unsigned, float, float> CommunicatorType;
  CommunicatorType com;
  com.allocateGPU();

  bool success;

  typedef std::vector<std::complex<float> > cpu_vector_type;

  // read in crs matrix A
  //--------------------------------------------------------------------------
  unsigned A_num_rows, A_num_columns;
  std::vector<unsigned> A_row_nnz;
  std::vector<unsigned> A_column_index;
  cpu_vector_type A_data;
  success = agile::readCSMatrixFile(A_matrix_file,
                                      A_num_rows, A_num_columns,
                                      A_row_nnz, A_column_index,
                                      A_data);
  if (!success)
  {
    std::cerr << "error: not able to load matrix A: " << A_matrix_file << std::endl;
    exit(-1);
  }

  // read in crs matrix A'
  //--------------------------------------------------------------------------
  unsigned AT_num_rows, AT_num_columns;
  std::vector<unsigned> AT_row_nnz;
  std::vector<unsigned> AT_column_index;
  cpu_vector_type AT_data;
  success = agile::readCSMatrixFile(AT_matrix_file,
                                      AT_num_rows, AT_num_columns,
                                      AT_row_nnz, AT_column_index,
                                      AT_data);
  if (!success)
  {
    std::cerr << "error: not able to load matrix A': " << AT_matrix_file << std::endl;
    exit(-1);
  }

  // read in vector
  //--------------------------------------------------------------------------
  cpu_vector_type y_host;
  success = agile::readVectorFile(vector_file, y_host);
  if (!success)
  {
    std::cerr << "error: not able to load vector: " << vector_file << std::endl;
    exit(-1);
  }
  
  // dimension check
  //--------------------------------------------------------------------------
  if (A_num_rows != AT_num_columns || A_num_columns != AT_num_rows) {
    std::cerr << "error: incompatible matrix dimensions " << std::endl
              << "       A: " << A_num_rows << "x" << A_num_columns
              << ", AT: " << AT_num_rows << "x" << AT_num_columns << std::endl;
    exit(-1);
  }
  if (y_host.size() != A_num_rows) {
    std::cerr << "error: incompatible dimensions" << std::endl;
  }

  // init gpu matrix and vector
  typedef agile::GPUCSMatrix<std::complex<float> > GPUCSMatrixType;
  GPUCSMatrixType A(A_row_nnz, A_column_index, A_data);
  typedef agile::GPUCSMatrix<std::complex<float>, true>
    GPUCSAdjointMatrixType;
  GPUCSAdjointMatrixType AT(AT_row_nnz, AT_column_index, AT_data);

  typedef agile::GPUVector<std::complex<float> > gpu_vector_type;
  gpu_vector_type y(A_num_rows);
  y.assignFromHost(y_host.begin(), y_host.end());

  // generate a forward operator
  typedef agile::ForwardMatrixWithAdjoint<CommunicatorType, GPUCSMatrixType,
    GPUCSAdjointMatrixType> ForwardType;
  ForwardType forward(com, A, AT);

  // generate a binary measure
  typedef agile::ScalarProductMeasure<CommunicatorType> MeasureType;
  MeasureType scalar_product(com);

  // init lsqr operator
  agile::LSQR<CommunicatorType, ForwardType, MeasureType> lsqr(
    com, forward, scalar_product, LSQR_ABS_TOLERANCE, LSQR_MAX_ITERATIONS);

  // init result vector on gpu
  gpu_vector_type x(A_num_columns);

#if WITH_TIMER
  struct timeval st, et;
  gettimeofday(&st, NULL);
#endif

  // do lsqr inverse computation
  lsqr(y, x);

#if WITH_TIMER
  cudaThreadSynchronize();
  gettimeofday(&et, NULL);
  float elapsed_time = ((et.tv_sec-st.tv_sec)*1000.0
    + (et.tv_usec - st.tv_usec)/1000.0);
  std::cout << "lsqr (gpu):  " << std::setprecision(5.9)
            << elapsed_time << "ms" << std::endl;
#endif

#if SHOW_LSQR_DETAILS
  std::cout << "iterations: " << lsqr.getIteration() << std::endl;
  std::cout << "final residual: " << lsqr.getRho() << std::endl;
#endif

  // transfer result from gpu to cpu
  cpu_vector_type x_host;
  x.copyToHost(x_host);

  // write result to file
  agile::writeVectorFile(result_file, x_host);
  return 0;
}
Example #4
0
int main(int argc, char *argv[])
{
    struct sparse_matrix_t *sparseA = NULL;
    struct vector_t *b = NULL;
    struct vector_t *x;
    struct mesh_t *mesh;
    char *xml_output;

    long int *compress2fat = NULL;
    struct vector_t *solution;
    struct vector_t *std_error_sol;
    long int fat_sol_nb_col;

    lsqr_input *input;
    lsqr_output *output;
    lsqr_work *work;            /* zone temoraire de travail */
    lsqr_func *func;            /* func->mat_vec_prod -> APROD */

    /* cmd line arg */
    char *mesh_filename = NULL;
    char *importfilename = NULL;
    char *output_filename = NULL;
    char *sol_error_filename = NULL;
    char *log_filename = NULL;
    char *output_type = NULL;
    int max_iter;
    double damping, grad_damping;
    int use_ach = 0;            /* ACH : tele-seismic inversion tomography */
    int check_sparse = 0;       /* check sparse matrix disable by default  */

    /* velocity model */
    char *vmodel = NULL;
    struct velocity_model_t *vm = NULL;

    struct mesh_t **imported_mesh = NULL;
    char **xmlfilelist = NULL;
    int nb_xmlfile = 0;

    int i, j;

    int nb_irm = 0;
    struct irm_t **irm = NULL;
    int *nb_metacell = NULL;

    FILE *logfd;

    /*************************************************************/
    parse_command_line(argc, argv,
                       &mesh_filename,
                       &vmodel,
                       &importfilename,
                       &log_filename,
                       &output_filename, &output_type,
                       &max_iter,
                       &damping, &grad_damping, &use_ach, &check_sparse);

    if (use_ach) {
        fprintf(stderr, "Using ACH tomographic inversion\n");
    } else {
        fprintf(stderr, "Using STANDARD tomographic inversion\n");
    }

    /* load the velocity model */
    if (vmodel) {
        char *myfile;
        vm = load_velocity_model(vmodel);
        if (!vm) {
            fprintf(stderr, "Can not initialize velocity model '%s'\n",
                    vmodel);
            exit(1);
        }
        myfile = strdup(vmodel);
        fprintf(stderr, "Velocity model '%s' loaded\n", basename(myfile));
        free(myfile);
    } else {
        vm = NULL;
    }

    /* Open log file */
    if (!log_filename) {
        logfd = stdout;
    } else {
        if (!(logfd = fopen(log_filename, "w"))) {
            perror(log_filename);
            exit(1);
        }
    }

    /*check_write_access (output_filename); */

    /**************************************/
    /* test if we can open file to import */
    /**************************************/
    if (importfilename) {
        xmlfilelist = parse_separated_list(importfilename, ",");
        nb_xmlfile = 0;
        while (xmlfilelist[nb_xmlfile]) {
            if (access(xmlfilelist[nb_xmlfile], R_OK) == -1) {
                perror(xmlfilelist[nb_xmlfile]);
                exit(1);
            }
            nb_xmlfile++;
        }
    } else {
        fprintf(stderr, "No file to import ... exiting\n");
        exit(0);
    }

    /****************************/
    /* main mesh initialization */
    /****************************/
    mesh = mesh_init_from_file(mesh_filename);
    if (!mesh) {
        fprintf(stderr, "Error decoding %s.\n", mesh_filename);
        exit(1);
    }
    fprintf(stderr, "read %s ok\n", mesh_filename);

    /*****************************************/
    /* check and initialize slice xml files  */
    /*****************************************/
    if (nb_xmlfile) {
        int nb_sparse = 0;
        int nb_res = 0;
        int f;

        imported_mesh =
            (struct mesh_t **) malloc(sizeof(struct mesh_t *) *
                                      nb_xmlfile);
        assert(imported_mesh);

        for (i = 0; i < nb_xmlfile; i++) {
            imported_mesh[i] = mesh_init_from_file(xmlfilelist[i]);
            if (!imported_mesh[i]) {
                fprintf(stderr, "Error decoding %s.\n", mesh_filename);
                exit(1);
            }
            for (f = 0; f < NB_MESH_FILE_FORMAT; f++) {
                /* mandatory field : res, sparse, and irm if provided */
                if (f == RES || f == SPARSE || f == IRM) {
                    check_files_access(f, imported_mesh[i]->data[f],
                                       xmlfilelist[i]);
                }
            }
            if (imported_mesh[i]->data[SPARSE]) {
                nb_sparse += imported_mesh[i]->data[SPARSE]->ndatafile;
            }

            if (imported_mesh[i]->data[RES]) {
                nb_res += imported_mesh[i]->data[RES]->ndatafile;
            }

            if (imported_mesh[i]->data[IRM]) {
                nb_irm += imported_mesh[i]->data[IRM]->ndatafile;
            }

        }

        if (!nb_sparse || !nb_res) {
            fprintf(stderr, "Error no sparse or res file available !\n");
            exit(0);
        }
    }

    /*********************************************/
    /* read and import the sparse(s) matrix(ces) */
    /*********************************************/
    for (i = 0; i < nb_xmlfile; i++) {
        if (!imported_mesh[i]->data[SPARSE]) {
            continue;
        }

        for (j = 0; j < imported_mesh[i]->data[SPARSE]->ndatafile; j++) {
            sparseA = import_sparse_matrix(sparseA,
                                           imported_mesh[i]->data[SPARSE]->
                                           filename[j]);
        }
    }

    if (check_sparse) {
        if (check_sparse_matrix(sparseA)) {
            exit(1);
        }
    }

    /*sparse_compute_length(sparseA, "length1.txt"); */
    fat_sol_nb_col = sparseA->nb_col;
    show_sparse_stats(sparseA);

    /*********************************************/
    /* read and import the residual time vector  */
    /*********************************************/
    for (i = 0; i < nb_xmlfile; i++) {
        if (!imported_mesh[i]->data[RES]) {
            continue;
        }

        for (j = 0; j < imported_mesh[i]->data[RES]->ndatafile; j++) {
            b = import_vector(b, imported_mesh[i]->data[RES]->filename[j]);
        }
    }

    /*************************************************/
    /* check compatibility between matrix and vector */
    /*************************************************/
    if (sparseA->nb_line != b->length) {
        fprintf(stderr,
                "Error, check your matrix/vector sizes (%ld/%ld)\n",
                sparseA->nb_line, b->length);
        exit(1);
    }

    /********************/
    /* show memory used */
    /********************/
#ifdef __APPLE__
    {
        struct mstats memusage;

        memusage = mstats();
        fprintf(stderr, "Memory used: %.2f MBytes\n",
                (float) (memusage.bytes_used) / (1024. * 1024));
    }
#else
    {
        struct mallinfo m_info;

        m_info = mallinfo();
        fprintf(stderr, "Memory used: %.2f MBytes\n",
                (float) (m_info.uordblks +
                         m_info.usmblks) / (1024. * 1024.));
    }
#endif

    /**************************************/
    /* relative traveltime mode           */
    /**************************************/
    if (use_ach) {
        int nb_evt_imported = 0;

        for (i = 0; i < nb_xmlfile; i++) {
            if (!imported_mesh[i]->data[EVT]) {
                continue;
            }

            for (j = 0; j < imported_mesh[i]->data[EVT]->ndatafile; j++) {
                relative_tt(sparseA, b,
                            imported_mesh[i]->data[EVT]->filename[j]);
                nb_evt_imported++;
            }
        }

        if (!nb_evt_imported) {
            fprintf(stderr,
                    "Error in ACH mode, can not import any .evt file !\n");
            exit(1);
        }
    }

    /************************************************/
    /* read the irregular mesh definition if needed */
    /* one by layer                                 */
    /************************************************/
    if (nb_irm) {
        int cpt = 0;
        struct mesh_offset_t **offset;
        int l;

        irm = (struct irm_t **) malloc(nb_irm * sizeof(struct irm_t *));
        assert(irm);
        nb_metacell = (int *) calloc(nb_irm, sizeof(int));
        assert(nb_metacell);

        make_mesh(mesh);

        for (i = 0; i < nb_xmlfile; i++) {
            if (!imported_mesh[i]->data[IRM]) {
                continue;
            }

            /* offset between meshes */
            offset = compute_mesh_offset(mesh, imported_mesh[i]);
            for (l = 0; l < mesh->nlayers; l++) {
                if (!offset[l])
                    continue;
                fprintf(stderr,
                        "\t%s, [%s] offset[layer=%d] : lat=%d lon=%d z=%d\n",
                        xmlfilelist[i], MESH_FILE_FORMAT[IRM], l,
                        offset[l]->lat, offset[l]->lon, offset[l]->z);
            }

            for (j = 0; j < imported_mesh[i]->data[IRM]->ndatafile; j++) {
                /* FIXME: read only once the irm file */
                irm[cpt] =
                    read_irm(imported_mesh[i]->data[IRM]->filename[j],
                             &(nb_metacell[cpt]));
                import2mesh_irm_file(mesh,
                                     imported_mesh[i]->data[IRM]->
                                     filename[j], offset);
                cpt++;
            }

            for (l = 0; l < mesh->nlayers; l++) {
                if (offset[l])
                    free(offset[l]);
            }
            free(offset);
        }
        metacell_find_neighbourhood(mesh);
    }

    /*sparse_compute_length(sparseA, "length1.txt"); */
    fat_sol_nb_col = sparseA->nb_col;
    show_sparse_stats(sparseA);

    /***********************/
    /* remove empty column */
    /***********************/
    fprintf(stderr, "starting compression ...\n");
    sparse_compress_column(mesh, sparseA, &compress2fat);
    if (check_sparse) {
        if (check_sparse_matrix(sparseA)) {
            exit(1);
        }
    }
    show_sparse_stats(sparseA);

    /***************************************/
    /* add gradient damping regularisation */
    /***************************************/
    if (fabs(grad_damping) > 1.e-6) {
        int nb_faces = 6;       /* 1 cell may have 6 neighbours */
        long int nb_lines = 0;
        char *regul_name;

        fprintf(stdout, "using gradient damping : %f\n", grad_damping);

        /* tmp file name */
        regul_name = tempnam("/tmp", "regul");
        if (!regul_name) {
            perror("lsqrsolve: ");
            exit(1);
        }

        if (nb_irm) {
            create_regul_DtD_irm(sparseA, compress2fat,
                                 mesh, regul_name, nb_faces,
                                 grad_damping, &nb_lines);
        } else {
            create_regul_DtD(sparseA, compress2fat,
                             mesh, regul_name, nb_faces,
                             grad_damping, &nb_lines);
        }

        sparse_matrix_resize(sparseA,
                             sparseA->nb_line + sparseA->nb_col,
                             sparseA->nb_col);

        sparseA = import_sparse_matrix(sparseA, regul_name);
        if (check_sparse) {
            if (check_sparse_matrix(sparseA)) {
                exit(1);
            }
        }
        vector_resize(b, sparseA->nb_line);
        unlink(regul_name);

        show_sparse_stats(sparseA);
    }

    /*********************************/
    /* the real mesh is no more used */
    /* keep only the light mesh      */
    /*********************************/
    fprintf(stdout,
            "Time to free the real mesh and keep only the light structure\n");
    free_mesh(mesh);
    mesh = mesh_init_from_file(mesh_filename);
    if (!mesh) {
        fprintf(stderr, "Error decoding %s.\n", mesh_filename);
        exit(1);
    }
    fprintf(stderr, "read %s ok\n", mesh_filename);

    /********************************/
    /* init vector solution to zero */
    /********************************/
    x = new_vector(sparseA->nb_col);

    /*************************************************************/
    /* solve A.x = B                                             */
    /* A = ray length in the cells                               */
    /* B = residual travel time observed - computed              */
    /* x solution to satisfy the lsqr problem                    */
    /*************************************************************/

    /* LSQR alloc */
    alloc_lsqr_mem(&input, &output, &work, &func, sparseA->nb_line,
                   sparseA->nb_col);

    fprintf(stderr, "alloc_lsqr_mem : ok\n");

    /* defines the routine Mat.Vect to use */
    func->mat_vec_prod = sparseMATRIXxVECTOR;

    /* Set the input parameters for LSQR */
    input->num_rows = sparseA->nb_line;
    input->num_cols = sparseA->nb_col;
    input->rel_mat_err = 1.0e-3;        /* in km */
    input->rel_rhs_err = 1.0e-2;        /* in seconde */
    /*input->rel_mat_err = 0.;
       input->rel_rhs_err = 0.; */
    input->cond_lim = .0;
    input->lsqr_fp_out = logfd;
    /* input->rhs_vec = (dvec *) b; */
    dvec_copy((dvec *) b, input->rhs_vec);
    input->sol_vec = (dvec *) x;        /* initial guess */
    input->damp_val = damping;
    if (max_iter == -1) {
        input->max_iter = 4 * (sparseA->nb_col);
    } else {
        input->max_iter = max_iter;
    }

    /* catch Ctrl-C signal */
    signal(SIGINT, emergency_halt);

    /******************************/
    /* resolution du systeme Ax=b */

    /******************************/
    lsqr(input, output, work, func, sparseA);
    fprintf(stderr, "*** lsqr ended (%ld iter) : %s\n",
            output->num_iters, lsqr_msg[output->term_flag]);
    if (output->term_flag == 0) {       /* solution x=x0 */
        exit(0);
    }

    /* uncompress the solution  */
    solution = uncompress_column((struct vector_t *) output->sol_vec,
                                 compress2fat, fat_sol_nb_col);

    /* uncompress the standard error on solution  */
    std_error_sol =
        uncompress_column((struct vector_t *) output->std_err_vec,
                          compress2fat, fat_sol_nb_col);

    /* if irm file was provided, set the right value to each cell 
     * from a given metacell 
     */
    if (irm) {
        irm_update(solution, irm, nb_metacell, nb_irm, mesh);
        free_irm(irm, nb_irm);
        free(nb_metacell);
    }

    /* write solution */
    if (strchr(output_type, 'm')) {
        export2matlab(solution, output_filename, mesh, vm,
                      output->num_iters,
                      input->damp_val, grad_damping, use_ach);
    }

    if (strchr(output_type, 's')) {
        export2sco(solution, output_filename, mesh, vm,
                   output->num_iters,
                   input->damp_val, grad_damping, use_ach);
    }

    if (strchr(output_type, 'g')) {
        /* solution */
        export2gmt(solution, output_filename, mesh, vm,
                   output->num_iters,
                   input->damp_val, grad_damping, use_ach);

        /* error on solution */
        sol_error_filename = (char *)
            malloc(sizeof(char) *
                   (strlen(output_filename) + strlen(".err") + 1));
        sprintf(sol_error_filename, "%s.err", output_filename);
        export2gmt(std_error_sol, sol_error_filename, mesh, vm,
                   output->num_iters,
                   input->damp_val, grad_damping, use_ach);
        free(sol_error_filename);

    }

    /* save the xml enrichied with sections */
    xml_output = (char *)
        malloc((strlen(output_filename) + strlen(".xml") +
                1) * sizeof(char));
    assert(xml_output);
    sprintf(xml_output, "%s.xml", output_filename);
    mesh2xml(mesh, xml_output);
    free(xml_output);

    /******************************************************/
    /* variance reduction, ie how the model fits the data */
    /* X = the final solution                             */
    /*                                                    */
    /*                 ||b-AX||²                          */
    /*         VR= 1 - --------                           */
    /*                  ||b||²                            */
    /*                                                    */
    /******************************************************/
    {
        double norm_b;
        double norm_b_AX;
        double VR;              /* variance reduction */

        struct vector_t *rhs;   /* right hand side */
        rhs = new_vector(sparseA->nb_line);

        /* use copy */
        dvec_copy((dvec *) b, (dvec *) rhs);

        norm_b = dvec_norm2((dvec *) rhs);

        /* does rhs = rhs + sparseA . output->sol_vec */
        /* here  rhs is overwritten */
        dvec_scale((-1.0), (dvec *) rhs);
        sparseMATRIXxVECTOR(0, output->sol_vec, (dvec *) rhs, sparseA);
        dvec_scale((-1.0), (dvec *) rhs);

        norm_b_AX = dvec_norm2((dvec *) rhs);

        VR = 1 - (norm_b_AX * norm_b_AX) / (norm_b * norm_b);
        fprintf(stdout, "Variance reduction = %.2f%%\n", VR * 100);
        free_vector(rhs);
    }

    /********/
    /* free */
    /********/
    if (vm) {
        free_velocity_model(vm);
    }
    free_mesh(mesh);
    free_sparse_matrix(sparseA);
    free_lsqr_mem(input, output, work, func);

    free_vector(solution);
    free_vector(std_error_sol);
    free(compress2fat);

    for (i = 0; i < nb_xmlfile; i++) {
        free(xmlfilelist[i]);
        free_mesh(imported_mesh[i]);
    }
    free(xmlfilelist);
    free(imported_mesh);
    return (0);
}
Example #5
0
float
optimize_smoothness(WPt& worlds_pts, const IntensityPerImage& left_intensities, const IntensityPerImage& right_intensities)
{
	// copy to globals
	//assert(fromVector.size() == toVector.size());
	//assert(fromVector.size() >= 3);
	//_fromVector = fromVector;
//	worlds_pts.resize(3);

	if (worlds_pts.size() < 3) {
		// too little points to opitimize
		return -1;
	}

	double max_z = -DBL_MAX;
	for (auto w = 0; w < worlds_pts.size();++w) {
		max_z = std::max(max_z, worlds_pts[w][2]);
//		max_z = std::max(max_z, (worlds_pts[w][1]));
	}

	double adjustment_rate = 1;

	std::vector<double> lambdas;
	lambdas.resize(worlds_pts.size() - 2);
	for (auto w = 0; w < worlds_pts.size();++w) {
		if (((w - 1) >= 0) && ((w) < (worlds_pts.size() - 1))) {
			double z_top = worlds_pts[w - 1][2];
			double z_bottom = worlds_pts[w + 1][2];
			double z_0 = worlds_pts[w][2];
//			double z_top = worlds_pts[w - 1][1];
//			double z_bottom = worlds_pts[w + 1][1];
//			double z_0 = worlds_pts[w][1];
//			double delta_z = ((z_top - z_bottom) + (z_0 - z_top) - (z_bottom - z_0)) / max_z;
			double delta_z = ((z_0 - z_top) - (z_bottom - z_0)) / max_z;
//			double delta_z = 0;

			double omega_i = std::min(left_intensities[w], right_intensities[w]);
			omega_i /= 255.0;
//			omega_i = 1.0;

//			double lambda_i = (1.0 - delta_z) * omega_i * adjustment_rate;
			double lambda_i = 0.99;
			lambdas[w - 1] = lambda_i;


			double y_top = worlds_pts[w - 1][1];
			double y_bottom = worlds_pts[w + 1][1];
			double y_0 = worlds_pts[w][1];
			
			double y = worlds_pts[w][1];
			double y_prime = w;
		}
	}

#ifdef DEBUG
	for (auto i = 0u; i < lambdas.size(); ++i) {
		std::cout << std::setprecision(15) << lambdas[i] << std::endl;
	}
#endif
	
	// allocate to globals
	lambdas_g = lambdas;

	// allocate structures for sparse linear least squares
	//printf("\tallocating for sparse linear least squares "
			 //"(%i vectors)...\n", fromVector.size());
	int num_rows = worlds_pts.size() - 2;
	int num_cols = worlds_pts.size();

	lsqr_input *input = NULL;
	lsqr_output *output = NULL;
	lsqr_work *work = NULL;
	lsqr_func *func = NULL;
	alloc_lsqr_mem(&input, &output, &work, &func, num_rows, num_cols);
	input->num_rows = num_rows;
	input->num_cols = num_cols;
	input->damp_val = 0.0;
	input->rel_mat_err = 0.0;
	input->rel_rhs_err = 0.0;
	input->cond_lim = 0.0;
	input->max_iter = 10*input->num_cols;
	input->lsqr_fp_out = NULL;
	func->mat_vec_prod = lsqr_eval_for_opt;

	// set rhs vec
	for (auto j = 0; j < num_rows; ++j) {
//	   input->rhs_vec->elements[j] = (1.0 - lambdas[j]) * worlds_pts[j+1][1];
	   input->rhs_vec->elements[j] = (1.0 - lambdas[j]) * worlds_pts[j+1][2];
	}

	// set initial sol vec
	for (auto i = 0u; i<num_cols; i++) {
//		input->sol_vec->elements[i] = worlds_pts[i][1];
		input->sol_vec->elements[i] = 0;
	}

	// call sparse linear least squares!
	printf("\t\tstarting (rows=%i, cols=%i)...\n", num_rows, num_cols);
	lsqr(input, output, work, func, NULL);
	double error = output->resid_norm;
	printf("\t\ttermination reason = %i\n", output->term_flag);
	printf("\t\tnum function calls = %i\n", output->num_iters);
	printf("\t\tremaining error = %lf\n", error);

	if (worlds_pts.size() > 0) {
//		double y_prev = worlds_pts[0][1];
		double y_prev = worlds_pts[0][2];
		for (auto i = 1u; i < worlds_pts.size() - 1; ++i) {
//			auto& y = worlds_pts[i][1];
			auto& y = worlds_pts[i][2];
			y = output->sol_vec->elements[i];
			double y_diff = y - y_prev;
			std::cout << "y difference " << i << " : "<< y_diff << std::endl;
			y_prev = y;
		}
		auto i = worlds_pts.size() - 1;
//		auto& y = worlds_pts[i][1];
		auto& y = worlds_pts[i][2];
		double y_diff = y - y_prev;
		std::cout << "y difference " << i << " : "<< y_diff << std::endl;
	}

	// free memory
   free_lsqr_mem(input, output, work, func);

	return (error);
}
Example #6
0
int
LEVMAR( // functional relation describing measurements. A p \in R^m yields a \hat{x} \in  R^n
        void (*func)( float* p, float* hx, int r, int c, void* adata ),
        // function to evaluate the Jacobian \part x / \part p
        void (*jacf)( float* p, SparseMatrix* j, int r, int c, void* adata ),
        float* p,         // I/O: initial parameter estimates. On output has the estimated solution
        float* x,         // I:   measurement vector. NULL implies a zero vector
        int r,            // I:   measurement vector dimension
        int c,            // I:   parameter vector dimension (i.e. #unknowns)
        int itmax,        // I:   maximum number of iterations
        float opts[4],    /* I:   minim. options [\mu, \epsilon1, \epsilon2, \epsilon3].
                                  Respectively the scale factor for initial \mu,
                                  stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2.
                                  Set to NULL for defaults to be used.  */
        float info[LM_INFO_SZ],
                          /* O:  information regarding the minimization. Set to NULL if don't care
                             info[0]  = ||e||_2, at initial p.
                             info[1]  = ||e||_2,            at estimated p.
                             info[2]  = ||J^T e||_inf,      at estimated p.
                             info[3]  = ||Dp||_2,           at estimated p.
                             info[4]  = mu/max[J^T * J]_ii, at estimated p.
                             info[5]  = # iterations,
                             info[6]  = reason for terminating:
                             1 - stopped by small gradient J^T e
                             2 - stopped by small Dp
                             3 - stopped by itmax
                             4 - singular matrix. Restart from current p with increased mu 
                             5 - no further error reduction is possible. Restart with increased mu
                             6 - stopped by small ||e||_2
                             7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error.
                             info[7]  = # function evaluations
                             info[8]  = # Jacobian evaluations
                             info[9]  = # linear systems solved, i.e. # attempts for reducing error.  */
        void* adata,      //  pointer to possibly additional data, passed uninterpreted to func & jacf.
                          //  Set to NULL if not needed.
        FILE* dout )
{
    SparseMatrix JAC;		// sparse jac
    SparseMatrix JTJ;		// sparse jac^T \times jac

    // temp work arrays
    float* epsilon_p;		//  r x 1
    float* hx;			//  r x 1   \hat{x}_i
    float* jacTe;		//  c x 1   J^T * e_i
    float* Dp;			//  c x 1
    float* diag_jacTjac;	//  c x 1   diagonal of [ J^T * J ]
    float* p_new;		//  c x 1   p + Dp

    float mu        = 0.0f;	//  damping constant
    float tmp       = 0.0f;	//  mainly used in matrix & vector multiplications
    float p_eL2     = 0.0f;	//  ||   e(p)  ||_2
    float jacTe_inf = 0.0f;	//  ||  J^T e  ||_inf
    float pDp_eL2   = 0.0f;	//  || e(p+Dp) ||_2
    float p_L2      = 0.0f;
    float Dp_L2     = FLT_MAX;
    float dF	    = 0.0f;
    float dL        = 0.0f;
    float tau       = LM_INIT_MU;
    float eps1      = LM_STOP_THRESH;
    float eps2      = LM_STOP_THRESH;
    float eps3      = LM_STOP_THRESH;
    float eps2_sq   = LM_STOP_THRESH * LM_STOP_THRESH;
    float init_p_eL2= 0.0f;

    int i, k;
    int nu = 2, nu2 = 0, stop = 0;
    int nfev = 0, njev = 0, nlss = 0;

    gettimeofday( &startTime, NULL );

    if ( r < c )
    {
        fprintf( dout, "LEVMAR(): cannot solve a problem with fewer measurements [%d] than unknowns [%d]\n", r, c );
        return LM_ERROR*1;
    }

    if ( !jacf )
    {
        fprintf( dout, "No function specified for computing the Jacobian in LEVMAR()\n" );
        return LM_ERROR*2;
    }

    if ( opts )
    {
        tau  = opts[0];
        eps1 = opts[1];
        eps2 = opts[2];
        eps3 = opts[3];
        eps2_sq = eps2 * eps2;
    }

    // setup indices of JTJ, they're constant for all iterations.
    (*jacf)( p, &JAC, r, c, adata ); njev++;
    JAC.prepare_transpose_multiply( JTJ );

    // allocate 2 * r + 4 * m floats;
    size_t total = LM_DER_WORKSZ( c, r );
    if ( NULL == ( epsilon_p = (float*) malloc( total * sizeof(float) ) ) )
    {
        fprintf( dout, "LEVMAR(): memory allocation request failed\n" );
        return LM_ERROR*3;
    }

    /* Internal solver memory pointer pt,                  */
    /* 32-bit: int pt[64]; 64-bit: long int pt[64]         */
    /* or void *pt[64] should be OK on both architectures  */ 
    void    *pt[64]; 

    /* Pardiso control parameters. */
    double   dparm[64];
    int      iparm[64];
    int      error, solver, mtype = -2;        /* Real symmetric matrix */

/* -------------------------------------------------------------------- */
/* ..  Setup Pardiso control parameters.                                */
/* -------------------------------------------------------------------- */

    error = 0;
    solver = 0; /* use sparse direct solver */
    F77_FUNC( pardisoinit )( pt,  &mtype, &solver, iparm, dparm, &error );

    if ( error != 0 )
    {
        if (error == -10 ) fprintf( dout, "No license file found \n"     );
        if (error == -11 ) fprintf( dout, "License is expired \n"        );
        if (error == -12 ) fprintf( dout, "Wrong username or hostname \n");
        return 0;
    }
    else fprintf( dout, "PARDISO license check was successful ... \n");

    /* set up work arrays */
    hx           = epsilon_p + r;
    jacTe        = hx + r;
    Dp           = jacTe + c;
    diag_jacTjac = Dp + c;
    p_new        = diag_jacTjac + c;

    /* compute epsilon_p = x - f(p) and its L2 norm */
    (*func)( p, hx, r, c, adata ); nfev++;

    /* ### epsilon_p = x - hx, p_eL2 = ||epsilon_p|| */

    for( i = 0, p_eL2 = 0.0f; i < r; ++i )
    {
        tmp = -hx[i];
        epsilon_p[i] = tmp;
        p_eL2 += tmp * tmp;
    }

    init_p_eL2 = p_eL2;
    if ( !finite( p_eL2 ) ) stop = 7;

    for ( k = 0; k < itmax && !stop; ++k )
    {
        // Note that p and epsilon_p have been updated at a previous iteration

        if ( p_eL2 <= eps3 )
        { // error is small
            stop = 6;
            break;
        }

        // Compute the Jacobian J at p,
        // [J^T \times J],
        // [J^T \times epsilon_p],
        // ||[J^T \times epsilon_p]||_inf and ||p||^2.

        (*jacf)( p, &JAC, r, c, adata ); njev++;

#if DEBUG
        if ( c < 101 ) JAC.dump( true );
#endif
        // J^T \times J, J^T \times epsilon_p
        JAC.compute_transpose_multiply( JTJ );

#if DEBUG
        if ( c < 101 ) JTJ.dump();
#endif

        bzero( jacTe, c * sizeof( float ) );

        for( i = 0; i < r; ++i )
        {
            tmp = epsilon_p[ i ];
            for ( int k = JAC.I_[ i ]; k < JAC.I_[ i + 1 ]; ++k )
            {
                jacTe[ JAC.J_[ k ] ] += JAC.A_[ k ] * tmp;
            }
        }
#if DEBUG
        if ( c < 101 )
        {
            fprintf( dout, "VECTOR epsilon_p:\n" );
            for ( i = 0; i < r; ++i )
            {
                fprintf( dout, "%4d: %7.3f\n", i, epsilon_p[ i ] );
            }
            fprintf( dout, "VECTOR jacTe:\n" );
            for ( i = 0; i < c; ++i )
            {
                fprintf( dout, "%4d: %7.3f\n", i, jacTe[ i ] );
            }
        }
#endif
        // Compute ||J^T \times epsilon_p||_inf and ||p||^2
        for ( i = 0, p_L2 = jacTe_inf = 0.0f; i < c; ++i )
        {
            tmp = FABS( jacTe[ i ] );
            if ( jacTe_inf < tmp )
            {
                jacTe_inf = tmp;
            }
            // save diagonal entries so that augmentation can be later canceled
            diag_jacTjac[ i ] = JTJ.A_[ JTJ.I_[ i ] ];
            p_L2 += p[ i ] * p[ i ];
        }

        // check for convergence
        if ( jacTe_inf <= eps1 )
        {
            Dp_L2 = 0.0f; // no increment for p in this case
            stop = 1;
            break;
        }

        // compute initial damping factor
        if ( k == 0 )
        {
            tmp = -FLT_MAX;
            // find max diagonal element
            for ( i = 0; i < c; ++i )
            {
                if ( diag_jacTjac[i] > tmp )
                {
                    tmp = diag_jacTjac[ i ];
                }
            }
            mu = tau * tmp;
        }

        // determine increment using adaptive damping
        while ( 1 )
        {
            // augment normal equations
            for ( i = 0; i < c; ++i )
            {
                JTJ.A_[ JTJ.I_[ i ] ] += mu;
            }
#if 0
            // solve augmented equations
#if DEBUG
            float t1 = currentTime();
#endif
            pardiso_symmetric( JTJ, jacTe, Dp, dout, pt, iparm, dparm );
            nlss++;
#if DEBUG
            float t2 = currentTime();
            fprintf( dout, "PARDISO time %f us\n", (t2 - t1) );
            if ( c < 201 ) check_solution( JTJ, jacTe, Dp );
#endif
#else
            {
                int istop, itn;
                float E_ = 1.0e-6f;
                float F_ = 1.0f / ( 10.0f * sqrtf( 1.0e-7f ) );
                float an, ac, rn, ar, xn;
                float* v = (float*) malloc( c * 2 * sizeof(float) );
                float* w = v + c;
                float t1 = currentTime();
                lsqr( c, c, LSQRAPROD, 0, &JTJ, jacTe, v, w, Dp, 0, E_, E_, F_, 100, dout, &istop, &itn, &an, &ac, &rn, &ar, &xn );
                nlss++;
                float t2 = currentTime();
                fprintf( dout, "LSQR solver time %f us\n", (t2 - t1) );
                if ( c < 201 ) check_solution( JTJ, jacTe, Dp );
                free( v );
            }
#endif
            // compute p's new estimate and ||Dp||^2
            for( i = 0, Dp_L2 = 0.0f; i < c; ++i )
            {
                p_new[i] = p[i] + ( tmp = Dp[i] );
                Dp_L2 += tmp * tmp;
            }
            // Dp_L2 = sqrt( Dp_L2 );

            if ( Dp_L2 <= eps2_sq * p_L2 )
            { // relative change in p is small
                stop = 2;
                break;
            }

            if ( Dp_L2 >= (p_L2 + eps2) / LM_EPSILON )
            { // almost singular
                stop = 4;
                break;
            }
            // evaluate function at p + Dp
            (*func)( p_new, hx, r, c, adata ); nfev++;

            // compute ||e(p_new)||_2
            // ### hx=x-hx, pDp_eL2=||hx||

            for( i = 0, pDp_eL2 = 0.0; i < r; ++i )
            {
                tmp = -hx[ i ];
                hx[ i ] = tmp;
                pDp_eL2 += tmp * tmp;
            }

            if ( !finite( pDp_eL2 ) )
            {
                // sum of squares is not finite, most probably due to a user error.
                // This check makes sure that the inner loop does not run indefinitely.
                stop = 7;
                break;
            }

            for ( i = 0, dL = 0.0f; i < c; ++i )
            {
                dL += Dp[ i ] * ( mu * Dp[ i ] + jacTe[ i ] );
            }
            dF = p_eL2 - pDp_eL2;

            // reduction in error, increment is accepted
            if ( dL > 0.0f && dF > 0.0f )
            {
                tmp = ( 2.0f * dF / dL - 1.0f );
                tmp = 1.0f - tmp * tmp * tmp;
                mu = mu * ( ( tmp >= LM_1_THIRD ) ? tmp : LM_1_THIRD );
                nu = 2;

                // update p's estimate
                bcopy( p_new, p, c * sizeof(float) );
                // update e and ||e||_2
                bcopy(  hx, epsilon_p, r * sizeof(float) );
                p_eL2 = pDp_eL2;
                break;
            }

            // if this point is reached the error did not reduce; the increment must be rejected

            mu *= nu;
            nu2 = nu << 1; // 2 * nu;
            if( nu2 <= nu )
            { // nu has wrapped around (overflown)
                stop = 5;
                break;
            }
            nu = nu2;
            // restore diagonal J^T J entries
            for ( i = 0; i < c; ++i )
            {
                JTJ.A_[ JTJ.I_[ i ] ] = diag_jacTjac[ i ];
            }
        } // inner loop
    }

    if ( k >= itmax ) stop = 3;

    if ( info )
    {
        for ( i = 1, tmp = diag_jacTjac[ 0 ]; i < c; ++i )
        {
            if ( tmp < diag_jacTjac[ i ] )
            {
                tmp = diag_jacTjac[ i ];
            }
        }
        info[0] = init_p_eL2;
        info[1] = p_eL2;
        info[2] = jacTe_inf;
        info[3] = Dp_L2;
        info[4] = mu / tmp;
        info[5] = (float)k;
        info[6] = (float)stop;
        info[7] = (float)nfev;
        info[8] = (float)njev;
        info[9] = (float)nlss;
    }

    free( epsilon_p );

    return ( stop != 4 && stop != 7 ) ?  k : LM_ERROR*5;
}
Example #7
0
double find_optimal_edge_zero_crossing(std::vector<cv::Point2f>& crossing_points) 
{
	// copy to globals
	//assert(fromVector.size() == toVector.size());
	//assert(fromVector.size() >= 3);
	//_fromVector = fromVector;
//	worlds_pts.resize(3);

	if (crossing_points.size() < 3) {
		// too little points to opitimize
		return -1;
	}



#ifdef DEBUG
	//for (auto i = 0u; i < lambdas.size(); ++i) {
	//	std::cout << std::setprecision(15) << lambdas[i] << std::endl;
	//}
#endif
	
	// allocate to globals
	crossing_points_g = crossing_points;

	// allocate structures for sparse linear least squares
	//printf("\tallocating for sparse linear least squares "
			 //"(%i vectors)...\n", fromVector.size());
	int num_rows = crossing_points.size();
	int num_cols = 3;

	lsqr_input *input = NULL;
	lsqr_output *output = NULL;
	lsqr_work *work = NULL;
	lsqr_func *func = NULL;
	alloc_lsqr_mem(&input, &output, &work, &func, num_rows, num_cols);
	input->num_rows = num_rows;
	input->num_cols = num_cols;
	input->damp_val = 0.0;
	input->rel_mat_err = 0.0;
	input->rel_rhs_err = 0.0;
	input->cond_lim = 0.0;
	input->max_iter = 10*input->num_cols;
	input->lsqr_fp_out = NULL;
	func->mat_vec_prod = lsqr_eval_for_opt_;

	// set rhs vec
	for (auto j = 0; j < num_rows; ++j) {
//	   input->rhs_vec->elements[j] = (1.0 - lambdas[j]) * worlds_pts[j+1][1];
	   input->rhs_vec->elements[j] = crossing_points[j].y;
	}

	// set initial sol vec
	for (auto i = 0u; i<num_cols; i++) {
//		input->sol_vec->elements[i] = worlds_pts[i][1];
		input->sol_vec->elements[i] = 0;
	}

	// call sparse linear least squares!
	//printf("\t\tstarting (rows=%i, cols=%i)...\n", num_rows, num_cols);
	lsqr(input, output, work, func, NULL);
	double error = output->resid_norm;
	//printf("\t\ttermination reason = %i\n", output->term_flag);
	//printf("\t\tnum function calls = %i\n", output->num_iters);
	//printf("\t\tremaining error = %lf\n", error);

	double a = input->sol_vec->elements[0];
	double b = input->sol_vec->elements[1];
	double c = input->sol_vec->elements[2];

	// solving for y = 0

	double solution = 0.0;
	double discriminant = std::pow(b, 2) - (4 * a * c);
	if (discriminant < 0) {
		// something went wrong
		solution = -1;
	} else {
		double delta = std::sqrt(discriminant);
		double sol_1 = ((-1 * b) + delta) / (2 * a);
		double sol_2 = ((-1 * b) - delta) / (2 * a);

		if (sol_1 <= crossing_points[crossing_points.size() - 1].x &&
			sol_1 >= crossing_points[0].x) {
			solution = sol_1;
		} else if (sol_2 <= crossing_points[crossing_points.size() - 1].x &&
			sol_2 >= crossing_points[0].x) {
			solution = sol_2;
		} else {
			// something wrong happened
			solution = -1;
		}

	}


	// free memory
   free_lsqr_mem(input, output, work, func);

	return (solution);
}