C++ Vec::data方法代码示例

bool MOSEK_SolveWrapper::setup(const LP_Constraints & cstraints) //cstraints <-> constraints
{
  assert(nbParams_ == cstraints.nbParams_);

  MSK_deletetask(&task);

  int NUMVAR = cstraints.constraint_mat_.cols();
  int NUMCON = cstraints.constraint_mat_.rows();
  int NUMANZ = cstraints.constraint_mat_.cols() * cstraints.constraint_mat_.rows(); //DENSE MATRIX

  MSKrescodee r = MSK_RES_OK;
  if ( r==MSK_RES_OK )
  {
    // Create the optimization task.
    r = MSK_maketask(env,NUMCON,NUMVAR,&task);

    // Directs the log task stream to the 'printstr' function.
    if ( r==MSK_RES_OK )
      MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr);

    // Give MOSEK an estimate of the size of the input data.
    //This is done to increase the speed of inputting data.
    // However, it is optional.
    if (r == MSK_RES_OK)
      r = MSK_putmaxnumvar(task,NUMVAR);

    if (r == MSK_RES_OK)
      r = MSK_putmaxnumcon(task,NUMCON);

    if (r == MSK_RES_OK)
      r = MSK_putmaxnumanz(task,NUMANZ);

    // Append 'NUMCON' empty constraints. The constraints will initially have no bounds.
    if ( r == MSK_RES_OK )
      r = MSK_append(task,MSK_ACC_CON,NUMCON);

    // Append 'NUMVAR' variables. The variables will initially be fixed at zero (x=0).
    if ( r == MSK_RES_OK )
      r = MSK_append(task,MSK_ACC_VAR,NUMVAR);
  }

  this->nbParams_ = NUMVAR;

  if (cstraints.bminimize_) {
    r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MINIMIZE);
  }
  else  {
    r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MAXIMIZE);
  }

  // Optionally add a constant term to the objective.
  if ( r ==MSK_RES_OK )
    r = MSK_putcfix(task,0.0);

  // Add the objective function if any
  if (!cstraints.vec_cost_.empty())  {
    // Set objective
    for (size_t i = 0; i < cstraints.vec_cost_.size(); ++i)
      MSK_putcj(task, i, cstraints.vec_cost_[i]);
  }

  //Add constraint row by row
  const Mat & A = cstraints.constraint_mat_;

  for (int i=0; i<A.rows() && r == MSK_RES_OK; ++i)
  {
    std::vector<int> vec_colno(A.cols(), 0);
    for (int ii=0; ii<A.cols(); ++ii){
      vec_colno[ii] = ii;
    }

    // Insert a row
    Vec temp = A.row(i);
    r = MSK_putavec(task,
                    MSK_ACC_CON,       // Input row of A.
                    MSKidxt (i),       // Variable row index.
                    vec_colno.size(),  // Number of non-zeros in row(i) A.
                    &vec_colno[0],     // Pointer to row indexes of row i.
                    (double*)temp.data());    // Pointer to Values of row i.
  }

  //Add bound on variables:
  if (cstraints.vec_bounds_.size() == 1) {
    for (size_t i = 0; i < NUMVAR; ++i) {
      if (r == MSK_RES_OK)
        r = MSK_putbound(task,
                         MSK_ACC_VAR, // Put bounds on variables.
                         i,           // Index of variable.
                         convertSign(cstraints.vec_sign_[0]),      // Bound key.
                         cstraints.vec_bounds_[0].first,  // Numerical value of lower bound.
                         cstraints.vec_bounds_[0].second); // Numerical value of upper bound.
    }
  }
  else{
    for (size_t i = 0; i < NUMVAR; ++i) {
    // Set the bounds on variable j.
    //    lowerbound <= x_j <= upper bound
      if (r == MSK_RES_OK)
        r = MSK_putbound(task,
                         MSK_ACC_VAR, // Put bounds on variables.
//.........这里部分代码省略.........

bool OSI_X_SolverWrapper<SOLVERINTERFACE>::setup(const LP_Constraints & cstraints) //cstraints <-> constraints
{
  bool bOk = true;
  if ( si == NULL )
  {
    return false;
  }
  assert(_nbParams == cstraints._nbParams);

  
  int NUMVAR = cstraints._constraintMat.cols();
  int NUMCON = cstraints._constraintMat.rows();
  int NUMANZ = cstraints._constraintMat.cols() * cstraints._constraintMat.rows(); //DENSE MATRIX
  
  std::vector<double> col_lb(NUMVAR);//the column lower bounds
  std::vector<double> col_ub(NUMVAR);//the column upper bounds
  
  this->_nbParams = NUMVAR;

  if (cstraints._bminimize) 
  {
    si->setObjSense( 1 );
  }
  else  
  {
    si->setObjSense( -1 );
  }

  const Mat & A = cstraints._constraintMat;

  //Equality constraint will be handked by two constraintsdue to the API limitation. 
  size_t nbLine = A.rows() + std::count(cstraints._vec_sign.begin(), cstraints._vec_sign.end(), EQ);
  
  std::vector<double> row_lb(nbLine);//the row lower bounds
  std::vector<double> row_ub(nbLine);//the row upper bounds
  
  CoinPackedMatrix * matrix = new CoinPackedMatrix(false,0,0);
  matrix->setDimensions(0, NUMVAR);

  
  //-- Add row-wise constraint
  size_t indexRow = 0;
  for (int i=0; i < A.rows(); ++i) 
  {
    Vec temp = A.row(i);
    
    CoinPackedVector row;
    if ( cstraints._vec_sign[i] == EQ || cstraints._vec_sign[i] == LE )
    {
      int coef = 1;
      for ( int j = 0; j < A.cols() ; j++ )
      {
	row.insert(j, coef * temp.data()[j]);
      }
      row_lb[indexRow] = -1.0 * si->getInfinity();
      row_ub[indexRow] = coef * cstraints._Cst_objective(i);
      matrix->appendRow(row);
      indexRow++;
    }
    if ( cstraints._vec_sign[i] == EQ || cstraints._vec_sign[i] == GE )
    {
      int coef = -1;
      for ( int j = 0; j < A.cols() ; j++ )
      {
	      row.insert(j, coef * temp.data()[j]);
      }
      row_lb[indexRow] = -1.0 * si->getInfinity();
      row_ub[indexRow] = coef * cstraints._Cst_objective(i);
      matrix->appendRow(row);      
      indexRow++;
    }
  }
  
  //-- Setup bounds
  if (cstraints._vec_bounds.size() == 1) 
  {
    // Setup the same bound for all the parameter
    for (int i=0; i < this->_nbParams; ++i)
    {
      col_lb[i] = cstraints._vec_bounds[0].first;
      col_ub[i] = cstraints._vec_bounds[0].second;
    }
  }
  else  
  {

    for (int i=0; i < this->_nbParams; ++i)
    {      
      col_lb[i] = cstraints._vec_bounds[i].first;
      col_ub[i] = cstraints._vec_bounds[i].second;
    }
  }  
 
  si->loadProblem(*matrix, &col_lb[0], &col_ub[0], cstraints._vec_cost.empty() ? NULL : &cstraints._vec_cost[0], &row_lb[0], &row_ub[0] );
  
  delete matrix;
  
  return bOk;
}

bool OSI_X_SolverWrapper::setup(const LP_Constraints & cstraints) //cstraints <-> constraints
{
  if ( si == nullptr )
  {
    return false;
  }
  assert(nbParams_ == cstraints.nbParams_);

  const unsigned int NUMVAR = cstraints.constraint_mat_.cols();
  std::vector<double>
    col_lb(NUMVAR), // the column lower bounds
    col_ub(NUMVAR); // the column upper bounds

  this->nbParams_ = NUMVAR;

  si->setObjSense( ((cstraints.bminimize_) ? 1 : -1) );

  const Mat & A = cstraints.constraint_mat_;

  //Equality constraint will be done by two constraints due to the API limitation ( >= & <=).
  const size_t nbLine = A.rows() +
    std::count(cstraints.vec_sign_.begin(), cstraints.vec_sign_.end(), LP_Constraints::LP_EQUAL);

  // Define default lower and upper bound to [-inf, inf]
  std::vector<double>
    row_lb(nbLine, -si->getInfinity()), // the row lower bounds
    row_ub(nbLine, si->getInfinity()); // the row upper bounds

  std::unique_ptr<CoinPackedMatrix> matrix(new CoinPackedMatrix(false,0,0));
  matrix->setDimensions(0, NUMVAR);

  //-- Add row-wise constraint
  size_t indexRow = 0;
  for (int i=0; i < A.rows(); ++i)
  {
    const Vec temp = A.row(i);
    CoinPackedVector row;

    if (cstraints.vec_sign_[i] == LP_Constraints::LP_EQUAL ||
        cstraints.vec_sign_[i] == LP_Constraints::LP_LESS_OR_EQUAL)
    {
      const int coef = 1;
      for ( int j = 0; j < A.cols(); ++j )
      {
        row.insert(j, coef * temp.data()[j]);
      }
      row_ub[indexRow] = coef * cstraints.constraint_objective_(i);
      matrix->appendRow(row);
      ++indexRow;
    }

    if (cstraints.vec_sign_[i] == LP_Constraints::LP_EQUAL ||
        cstraints.vec_sign_[i] == LP_Constraints::LP_GREATER_OR_EQUAL)
    {
      const int coef = -1;
      for ( int j = 0; j < A.cols(); ++j )
      {
        row.insert(j, coef * temp.data()[j]);
      }
      row_ub[indexRow] = coef * cstraints.constraint_objective_(i);
      matrix->appendRow(row);
      ++indexRow;
    }
  }

  //-- Setup bounds for all the parameters
  if (cstraints.vec_bounds_.size() == 1)
  {
    // Setup the same bound for all the parameters
    std::fill(col_lb.begin(), col_lb.end(), cstraints.vec_bounds_[0].first);
    std::fill(col_ub.begin(), col_ub.end(), cstraints.vec_bounds_[0].second);
  }
  else // each parameter have its own bounds
  {
    for (int i=0; i < this->nbParams_; ++i)
    {
      col_lb[i] = cstraints.vec_bounds_[i].first;
      col_ub[i] = cstraints.vec_bounds_[i].second;
    }
  }

  si->loadProblem(*matrix, &col_lb[0], &col_ub[0], cstraints.vec_cost_.empty() ? nullptr : &cstraints.vec_cost_[0], &row_lb[0], &row_ub[0] );

  return true;
}

int main(int argc, char **argv)
{
    enum
    {
        ROBUST_RIGID_REGISTRATION = 0,
        RIGID_REGISTRATION_ALL_POINTS = 1
    };
    std::string
    sSfM_Data_Filename_In,
    sSfM_Data_Filename_Out;
    unsigned int rigid_registration_method = RIGID_REGISTRATION_ALL_POINTS;

    CmdLine cmd;
    cmd.add(make_option('i', sSfM_Data_Filename_In, "input_file"));
    cmd.add(make_option('o', sSfM_Data_Filename_Out, "output_file"));
    cmd.add(make_option('m', rigid_registration_method, "method"));

    try
    {
        if (argc == 1) throw std::string("Invalid command line parameter.");
        cmd.process(argc, argv);
    }
    catch(const std::string& s)
    {
        std::cerr
                << "Usage: " << argv[0] << '\n'
                << " GPS registration of a SfM Data scene,\n"
                << "[-i|--input_file] path to the input SfM_Data scene\n"
                << "[-o|--output_file] path to the output SfM_Data scene\n"
                << "[-m|--method] method to use for the rigid registration\n"
                << "\t0 => registration is done using a robust estimation,\n"
                << "\t1 (default)=> registration is done using all points.\n"
                << std::endl;

        std::cerr << s << std::endl;
        return EXIT_FAILURE;
    }

    if (sSfM_Data_Filename_In.empty() || sSfM_Data_Filename_Out.empty())
    {
        std::cerr << "Invalid input or output filename." << std::endl;
        return EXIT_FAILURE;
    }

    //
    // Load a SfM scene
    // For each valid view (pose & intrinsic defined)
    //  - iff a GPS position can be parsed
    //    - store corresponding camera pose & GPS position
    // - Compute the registration between the selected camera poses & GPS positions

    // Load input SfM_Data scene
    SfM_Data sfm_data;
    if (!Load(sfm_data, sSfM_Data_Filename_In, ESfM_Data(ALL)))
    {
        std::cerr
                << "\nThe input SfM_Data file \"" << sSfM_Data_Filename_In
                << "\" cannot be read." << std::endl;
        return EXIT_FAILURE;
    }

    // Init the EXIF reader (will be used for GPS data reading)
    std::unique_ptr<Exif_IO> exifReader(new Exif_IO_EasyExif);
    if (!exifReader)
    {
        std::cerr << "Cannot instantiate the EXIF metadata reader." << std::endl;
        return EXIT_FAILURE;
    }

    // List corresponding poses (SfM - GPS)
    std::vector<Vec3> vec_sfm_center, vec_gps_center;

    for (const auto & view_it : sfm_data.GetViews() )
    {
        if (!sfm_data.IsPoseAndIntrinsicDefined(view_it.second.get()))
            continue;

        const std::string view_filename =
            stlplus::create_filespec(sfm_data.s_root_path, view_it.second->s_Img_path);

        // Try to parse EXIF metada & check existence of EXIF data
        if (! (exifReader->open( view_filename ) &&
                exifReader->doesHaveExifInfo()) )
            continue;

        // Check existence of GPS coordinates
        double latitude, longitude, altitude;
        if ( exifReader->GPSLatitude( &latitude ) &&
                exifReader->GPSLongitude( &longitude ) &&
                exifReader->GPSAltitude( &altitude ) )
        {
            // Add ECEF XYZ position to the GPS position array
            vec_gps_center.push_back( lla_to_ecef( latitude, longitude, altitude ) );
            const openMVG::geometry::Pose3 pose(sfm_data.GetPoseOrDie(view_it.second.get()));
            vec_sfm_center.push_back( pose.center() );
        }
    }

    if ( vec_sfm_center.empty() )
    {
//.........这里部分代码省略.........