C++ DblMatrix类代码示例

ConvexConstraintsPtr ZMPConstraint::convex(const DblVec& x, Model* model) {
  DblVec curvals = getDblVec(x, m_vars);
  m_rad->SetDOFValues(curvals);
  DblMatrix jacmoment = DblMatrix::Zero(3, curvals.size());
  OR::Vector moment(0,0,0);
  float totalmass = 0;
  BOOST_FOREACH(const KinBody::LinkPtr& link, m_rad->GetRobot()->GetLinks()) {
    if (!link->GetGeometries().empty()) {
      OR::Vector cm = link->GetGlobalCOM();
      moment += cm * link->GetMass();
      jacmoment += m_rad->PositionJacobian(link->GetIndex(), cm) * link->GetMass();
      totalmass += link->GetMass();
    }
  }
  moment /= totalmass;
  jacmoment /= totalmass;

  AffExpr x_expr = AffExpr(moment.x) + varDot(jacmoment.row(0), m_vars) - jacmoment.row(0).dot(toVectorXd(curvals));
  AffExpr y_expr = AffExpr(moment.y) + varDot(jacmoment.row(1), m_vars) - jacmoment.row(1).dot(toVectorXd(curvals));

  ConvexConstraintsPtr out(new ConvexConstraints(model));
  for (int i=0; i < m_ab.rows(); ++i) {
    out->addIneqCnt(m_ab(i,0) * x_expr + m_ab(i,1) * y_expr + m_c(i));
  }
  return out;
}

/**
 * @param equality A matrix of equality constraints \f$A_{eq}\f$(the number of
 * columns must match number of parameters)
 *                 where \f$A_{eq} x = 0\f$
 * @param inequality A matrix of inequality constraints (the number of columns
 * must match number of parameters
 *                 where \f$A_{eq} x \geq 0\f$
 */
void AugmentedLagrangianOptimizer::checkConstraints(
    const DblMatrix &equality, const DblMatrix &inequality) {
  const size_t totalNumConstr =
      numEqualityConstraints() + numInequalityConstraints();
  if (totalNumConstr == 0)
    return;

  // Sanity checks on matrix sizes
  for (size_t i = 0; i < 2; ++i) {
    size_t ncols(0);
    std::string matrix("");
    if (i == 0) {
      ncols = equality.numCols();
      matrix = "equality";
    } else {
      ncols = inequality.numCols();
      matrix = "inequality";
    }

    if (ncols > 0 && ncols != numParameters()) {
      std::ostringstream os;
      os << "AugmentedLagrangianOptimizer::initializeConstraints - Invalid "
         << matrix << " constraint matrix. Number of columns must match number "
                      "of parameters. ncols=" << ncols
         << ", nparams=" << numParameters();
      throw std::invalid_argument(os.str());
    }
  }
}

// x given
int FitLine(const DblVector& PointsY, const DblVector& PointsX, double& a, double& b)
{
	assert(PointsX.size()==PointsY.size());
	DblMatrix S;
	S.Assign(3, 2, 0.0);
	for(unsigned int i=0; i<PointsX.size(); i++)
	{
		S[0][0] += PointsX[i] * PointsX[i];
		S[0][1] += PointsX[i];
		S[0][2] += PointsX[i] * PointsY[i];
		S[1][2] += PointsY[i]; 
	}
	S[1][1] = (double)PointsX.size();
	S.Diagonalize();
	DblVector Solutions;
	int Ret = S.SolveLinearCramer(Solutions);
	if(Ret) 
	{
		a = Solutions[0];
		b = Solutions[1];
	}
	else std::cout << "Error in line fitting" << std::endl;
	
	return Ret;
}

// x given 
int FitParabola(const DblVector& PointsY, const DblVector& PointsX, double& a, double& b, double& c)
{
	// fit parabola
	assert(PointsX.size()==PointsY.size());
	DblMatrix S;
	S.Assign(4, 3, 0.0);
	for(unsigned int i=0; i<PointsX.size(); i++)
	{
		S[0][0] += PointsX[i] * PointsX[i] * PointsX[i] * PointsX[i];
		S[0][1] += PointsX[i] * PointsX[i] * PointsX[i];
		S[0][2] += PointsX[i] * PointsX[i];
		S[0][3] += PointsX[i] * PointsX[i] * PointsY[i];
		S[1][2] += PointsX[i];
		S[1][3] += PointsX[i] * PointsY[i];
		S[2][3] += PointsY[i];
		
	}
	S[1][1] = S[0][2];
	S[2][2] = (double)PointsX.size();
	S.Diagonalize();
	DblVector Solutions;
	int Ret = S.SolveLinearCramer(Solutions);
	if(Ret==-1) return -1;
	else
	{
		a = Solutions[0];
		b = Solutions[1];
		c = Solutions[2];
	}
	return Ret;
}

/** Sets the U matrix
  @param newU :: the new U matrix
  @param force :: If true, do not check that U matrix is valid
  */
void OrientedLattice::setU(const DblMatrix &newU, const bool force) {
  // determinant ==1 or (determinant == +/-1 and force)
  if (newU.isRotation() || (force && newU.isOrthogonal())) {
    U = newU;
    UB = U * getB();
  } else
    throw std::invalid_argument("U is not a proper rotation");
}

int DblMatrix::qtClustering(double thresh, std::vector<DblMatrix>& clusters)
{
    /// initialize the members
    IntMatrix members;
    members.assign(size(), IntVector());

    ///// each entry is a neighbor of itself
    for(unsigned int i=0; i<size(); i++)
        members[i].push_back(i);

    /// get the other members
    for(unsigned int j=0; j<size()-1; j++)
    {
        for(unsigned int k=j+1; k<size(); k++)
        {
            double diff = (*this)[j].EuclDist((*this)[k]);
            if(diff <= thresh)
            {
                members[j].push_back(k);
                members[k].push_back(j);
            }
        }
    }

    /// QT clustering
    std::sort(members.begin(), members.end(), IntVectorGreater);
    IntMatrix::iterator it1;
    int n=0;
    for(it1=members.begin(); it1!=members.end(); it1++, n++)
    {
        for(unsigned int j=1; j<it1->size(); j++)
        {
            IntMatrix::iterator it2=it1;
            it2++;
            for(; it2!=members.end();)
            {
                if((*it1)[j]==it2->front()) it2=members.erase(it2);
                else it2++;
            }
        }
    }

    /// build groups
    clusters.clear();
    for(unsigned int i=0; i<members.size(); i++)
    {
        DblMatrix cluster;
        for(unsigned int j=0; j<members[i].size(); j++)
        {
            cluster.push_back((*this)[members[i][j]]);
        }
        clusters.push_back(cluster);
    }

    return 0;
}

// fit a two dimensional quadratic surface
int FitQuadraticSurface(const DblVector& PX, const DblVector& PY,
							const DblVector& PZ, DblVector& Parameters)
{
	// fit parabola
	assert(PX.size()==PY.size());
	DblMatrix S;
	S.Assign(7, 6, 0.0);
	for(unsigned int i=0; i<PX.size(); i++)
	{
		double X2 = PX[i] * PX[i];
		double Y2 = PY[i] * PY[i];
		double X3 = X2 * PX[i];
		double Y3 = Y2 * PY[i];
		double X4 = X3 * PX[i];
		double Y4 = Y3 * PY[i];
		
		S[0][1] += PY[i];
		S[1][1] += Y2;
		
		S[0][2] += PX[i];
		S[1][2] += PX[i]*PY[i];
		S[2][2] += X2;

		S[0][3] += PX[i]*PY[i];
		S[1][3] += PX[i]*Y2;
		S[2][3] += X2*PY[i];
		S[3][3] += X2*Y2;

		S[0][4] += Y2;
		S[1][4] += Y3;
		S[2][4] += PX[i]*Y2;
		S[3][4] += PX[i]*Y3;
		S[4][4] += Y4;

		S[0][5] += X2;
		S[1][5] += X2*PY[i];
		S[2][5] += X3;
		S[3][5] += X3*PY[i];
		S[4][5] += X2*Y2;
		S[5][5] += X4;

		// function values
		S[0][6]+=PZ[i];
		S[1][6]+=PY[i]*PZ[i];
		S[2][6]+=PX[i]*PZ[i];
		S[3][6]+=PX[i]*PY[i]*PZ[i];
		S[4][6]+=Y2*PZ[i];
		S[5][6]+=X2*PZ[i];

	}
	S[0][0] = (double)PX.size();
	S.Diagonalize();
	return S.SolveLinearCramer(Parameters);
}

/** Sets the UB matrix and recalculates lattice parameters
  @param newUB :: the new UB matrix*/
void OrientedLattice::setUB(const DblMatrix &newUB) {
  // check if determinant is close to 0. The 1e-10 value is arbitrary
  if (std::fabs(newUB.determinant()) > 1e-10) {
    UB = newUB;
    DblMatrix newGstar, B;
    newGstar = newUB.Tprime() * newUB;
    this->recalculateFromGstar(newGstar);
    B = this->getB();
    B.Invert();
    U = newUB * B;
  } else
    throw std::invalid_argument("determinant of UB is too close to 0");
}

void SharedImageSequence::ExportSequenceAsPointClouds(const std::string& Name)
{
	SharedImageSequence::iterator it;
	int i=0;
	for(it=begin(); it!=end(); it++)
	{
		std::stringstream FileNameStream;
		FileNameStream << Name << i << m_PCloudAttachement;
		DblMatrix M;
		it->ExportToPointCloud(M);
		M.Save(FileNameStream.str());
		i++;
	}
}

/** Default constructor
@param Umatrix :: orientation matrix U. By default this will be identity matrix
*/
NiggliCell::NiggliCell(const DblMatrix &Umatrix) : UnitCell() {
  if (Umatrix.isRotation() == true) {
    U = Umatrix;
    UB = U * getB();
  } else
    throw std::invalid_argument("U is not a proper rotation");
}

/** Sets the U matrix
  @param newU :: the new U matrix
  @param force :: If true, do not check that U matrix is valid
  */
void OrientedLattice::setU(const DblMatrix &newU, const bool force) {
  if (force || newU.isRotation()) {
    U = newU;
    UB = U * getB();
  } else
    throw std::invalid_argument("U is not a proper rotation");
}

/** Default constructor
@param Umatrix :: orientation matrix U. By default this will be identity matrix
*/
OrientedLattice::OrientedLattice(const DblMatrix &Umatrix) : UnitCell() {
  if (Umatrix.isRotation()) {
    U = Umatrix;
    UB = U * getB();
  } else
    throw std::invalid_argument("U is not a proper rotation");
}

/**
  Get the UB matrix corresponding to the real space edge vectors a,b,c.
  The inverse of the matrix with vectors a,b,c as rows will be stored in UB.

  @param  UB      A 3x3 matrix that will be set to the UB matrix.
  @param  a_dir   The real space edge vector for side a of the unit cell
  @param  b_dir   The real space edge vector for side b of the unit cell
  @param  c_dir   The real space edge vector for side c of the unit cell

  @return true if UB was set to the new matrix and false if UB could not be
          set since the matrix with a,b,c as rows could not be inverted.
 */
bool OrientedLattice::GetUB(DblMatrix &UB, const V3D &a_dir, const V3D &b_dir,
                            const V3D &c_dir) {
  if (UB.numRows() != 3 || UB.numCols() != 3) {
    throw std::invalid_argument("Find_UB(): UB matrix NULL or not 3X3");
  }

  UB.setRow(0, a_dir);
  UB.setRow(1, b_dir);
  UB.setRow(2, c_dir);
  try {
    UB.Invert();
  } catch (...) {
    return false;
  }
  return true;
}

/**
  @param  inname       Name of workspace containing peaks
  @param  params       optimized cell parameters
  @param  out          residuals from optimization
*/
void OptimizeLatticeForCellType::optLattice(std::string inname,
                                            std::vector<double> &params,
                                            double *out) {
  PeaksWorkspace_sptr ws = boost::dynamic_pointer_cast<PeaksWorkspace>(
      AnalysisDataService::Instance().retrieve(inname));
  const std::vector<Peak> &peaks = ws->getPeaks();
  size_t n_peaks = ws->getNumberPeaks();
  std::vector<V3D> q_vector;
  std::vector<V3D> hkl_vector;

  for (size_t i = 0; i < params.size(); i++)
    params[i] = std::abs(params[i]);
  for (size_t i = 0; i < n_peaks; i++) {
    q_vector.push_back(peaks[i].getQSampleFrame());
    hkl_vector.push_back(peaks[i].getHKL());
  }

  Mantid::API::IAlgorithm_sptr alg = createChildAlgorithm("CalculateUMatrix");
  alg->setPropertyValue("PeaksWorkspace", inname);
  alg->setProperty("a", params[0]);
  alg->setProperty("b", params[1]);
  alg->setProperty("c", params[2]);
  alg->setProperty("alpha", params[3]);
  alg->setProperty("beta", params[4]);
  alg->setProperty("gamma", params[5]);
  alg->executeAsChildAlg();

  ws = alg->getProperty("PeaksWorkspace");
  OrientedLattice latt = ws->mutableSample().getOrientedLattice();
  DblMatrix UB = latt.getUB();
  DblMatrix A = aMatrix(params);
  DblMatrix Bc = A;
  Bc.Invert();
  DblMatrix U1_B1 = UB * A;
  OrientedLattice o_lattice;
  o_lattice.setUB(U1_B1);
  DblMatrix U1 = o_lattice.getU();
  DblMatrix U1_Bc = U1 * Bc;

  for (size_t i = 0; i < hkl_vector.size(); i++) {
    V3D error = U1_Bc * hkl_vector[i] - q_vector[i] / (2.0 * M_PI);
    out[i] = error.norm2();
  }

  return;
}

/**
  Get the real space edge vectors a,b,c corresponding to the UB matrix.
  The rows of the inverse of the matrix with will be stored in a_dir,
  b_dir, c_dir.

  @param  UB      A 3x3 matrix containing a UB matrix.
  @param  a_dir   Will be set to the real space edge vector for side a
                  of the unit cell
  @param  b_dir   Will be set to the real space edge vector for side b
                  of the unit cell
  @param  c_dir   Will be set to the real space edge vector for side c
                  of the unit cell

  @return true if the inverse of the matrix UB could be found and the
          a_dir, b_dir and c_dir vectors have been set to the rows of
          UB inverse.
 */
bool OrientedLattice::GetABC(const DblMatrix &UB, V3D &a_dir, V3D &b_dir,
                             V3D &c_dir) {
  if (UB.numRows() != 3 || UB.numCols() != 3) {
    throw std::invalid_argument("GetABC(): UB matrix NULL or not 3X3");
  }

  DblMatrix UB_inverse(UB);
  try {
    UB_inverse.Invert();
  } catch (...) {
    return false;
  }
  a_dir(UB_inverse[0][0], UB_inverse[0][1], UB_inverse[0][2]);
  b_dir(UB_inverse[1][0], UB_inverse[1][1], UB_inverse[1][2]);
  c_dir(UB_inverse[2][0], UB_inverse[2][1], UB_inverse[2][2]);

  return true;
}