C++ VectorXd::setConstant方法代碼示例

void BtCircleTrain::train2DGaussian(BFImage& bfImage, BFImage& bfHist, const BFCircle& circle, BfGaussian2DPixelClassifier& classifier) {

	assert(bfImage.getColorMode() == BF_LAB);

	std::vector<BFCoordinate<int> > innerPoints;
	BFCircle::getInnerPoints(circle, innerPoints);
	int nPoints = innerPoints.size();

	Eigen::MatrixXd xT(nPoints,2);
	cv::Mat histImg = cv::Mat::zeros(histSize,histSize,CV_8U);

	for(int i=0; i<nPoints; i++) {
		BFCoordinate<unsigned int> intPoint(static_cast<int>(innerPoints[i].getX()), static_cast<int>(innerPoints[i].getY()));

		BFColor color = bfImage.getColor(intPoint);

		int aValue = color.getChannel(1);
		int bValue = color.getChannel(2);

		assert(aValue+abRange >= 0 && aValue+abRange < 2*abRange);
		assert(bValue+abRange >= 0 && bValue+abRange < 2*abRange);

		histImg.datastart[(bValue+abRange)*histSize + aValue+abRange]++;

		xT(i,0) = static_cast<double>(aValue);
		xT(i,1) = static_cast<double>(bValue);
	}

	double meanA = xT.col(0).mean();
	double meanB = xT.col(1).mean();

	Eigen::VectorXd meanVecA;
	Eigen::VectorXd meanVecB;
	meanVecA.setConstant(nPoints, meanA);
	meanVecB.setConstant(nPoints, meanB);

	xT.col(0) -= meanVecA;
	xT.col(1) -= meanVecB;

	Eigen::Matrix2d C = xT.transpose()*xT/static_cast<double>(nPoints-1);
	Eigen::Vector2d mu;
	mu << meanA, meanB;

	bfHist.setColorMode(BF_GRAYSCALE);
	cv::Mat& histNorm = bfHist.getImageMat();

	// just for visualization
	cv::normalize(histImg,histNorm,255.0,0.0,cv::NORM_INF,-1);

	classifier.setC(C);
	classifier.setMu(mu);
	classifier.calculateEllipse();
}

void Feature05::evaluate(const Segment& segment, Eigen::VectorXd& result) const
{
    result = Eigen::VectorXd::Zero(getNDimensions());

    const size_t numPoints = segment.points.size();
    if (numPoints > 1) {
        // width
        result(0) = (segment.points.back() - segment.points.front()).norm();

        if (m_extended) {
            // min and max distance between points
            result(1) = DBL_MAX;
            result(2) = 0.0;

            for (size_t pIndex = 0; pIndex < numPoints - 1; ++pIndex) {
                double dist = (segment.points[pIndex] - segment.points[pIndex + 1]).norm();
                if (dist < result(1)) result(1) = dist;
                if (dist > result(2)) result(2) = dist;
            }

            // ratio between min and max distance between points
            if (result(2) != 0.0) {
                result(3) = result(1) / result(2);
            }
        }
    }
    else {
        result.setConstant(getNDimensions(), -1.0);
    }
}

template <typename PointSource, typename PointTarget> inline void
pcl::registration::TransformationEstimationLM<PointSource, PointTarget>::estimateRigidTransformation (
    const pcl::PointCloud<PointSource> &cloud_src,
    const std::vector<int> &indices_src,
    const pcl::PointCloud<PointTarget> &cloud_tgt,
    const std::vector<int> &indices_tgt,
    Eigen::Matrix4f &transformation_matrix)
{
  if (indices_src.size () != indices_tgt.size ())
  {
    PCL_ERROR ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), indices_tgt.size ());
    return;
  }

  if (indices_src.size () < 4)     // need at least 4 samples
  {
    PCL_ERROR ("[pcl::IterativeClosestPointNonLinear::estimateRigidTransformationLM] ");
    PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %zu points!",
               indices_src.size ());
    return;
  }

  // If no warp function has been set, use the default (WarpPointRigid6D)
  if (!warp_point_)
    warp_point_.reset (new WarpPointRigid6D<PointSource, PointTarget>);

  int n_unknowns = warp_point_->getDimension ();  // get dimension of unknown space
  Eigen::VectorXd x (n_unknowns);
  x.setConstant (n_unknowns, 0);

  // Set temporary pointers
  tmp_src_ = &cloud_src;
  tmp_tgt_ = &cloud_tgt;
  tmp_idx_src_ = &indices_src;
  tmp_idx_tgt_ = &indices_tgt;

  OptimizationFunctorWithIndices functor (static_cast<int> (indices_src.size ()), this);
  Eigen::NumericalDiff<OptimizationFunctorWithIndices> num_diff (functor);
  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithIndices> > lm (num_diff);
  int info = lm.minimize (x);

  // Compute the norm of the residuals
  PCL_DEBUG ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
  PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
  PCL_DEBUG ("Final solution: [%f", x[0]);
  for (int i = 1; i < n_unknowns; ++i) 
    PCL_DEBUG (" %f", x[i]);
  PCL_DEBUG ("]\n");

  // Return the correct transformation
  Eigen::VectorXf params = x.cast<float> ();
  warp_point_->setParam (params);
  transformation_matrix = warp_point_->getTransform ();

  tmp_src_ = NULL;
  tmp_tgt_ = NULL;
  tmp_idx_src_ = tmp_idx_tgt_ = NULL;
}

int main(int argc, char** argv)
{
	Eigen::IOFormat HeavyFmt(Eigen::FullPrecision);
	
	DMP dmp_example(10);
	dmp_example.reset_state(0);
	dmp_example.set_goal(1,1);
	
	Eigen::VectorXd cw;
	cw.setConstant(10,-1);
	
	std::vector< Eigen::VectorXd > test = dmp_example.run(1, 0.01, 0, 0, 1, 1, cw);
	
	
	Eigen::MatrixXd read_T= dmp_example.readMatrix("/home/zengzhen/Desktop/T.txt");
// 	std::cout << "read file T = " << read_T << std::endl; 
	
	Eigen::Map<Eigen::VectorXd> T(read_T.data(),read_T.cols()*read_T.rows(),1);
// 	std::cout << "target T is " << T << std::endl;
	
	dmp_example.batch_fit(3.0, 0.01, T);
	
	return 0;
}

void MGraphCut::FeatureEdgeExtention(MGraph& gph, std::vector<std::vector<unsigned> >& gcFeatures, std::vector<unsigned> & features, std::vector<int> & vertexMap, std::vector<int> & featureFlagMap, SGIter & s_it, myLinearSolver & solver, GraphType *g, double myPolarizedThreshold)
{
    // Source and sink value
    double srcVal  = 1.0;
    double sinVal  = 0.0;
    double halfVal = 0.5; // 0.5*(srcVal + sinVal)

    // build a flag map to indicate which vertices are the current round's source (0) and sink (1)
    for (unsigned i = 0; i < features.size(); i++)
    {
        int vsrc = gph.es[features[i]].n1;
        int vsin = gph.es[features[i]].n2;
        if (!gph.es[features[i]].ort)
        {
            vsrc = gph.es[features[i]].n2;
            vsin = gph.es[features[i]].n1;
        }

        if (vertexMap[vsrc] == -1 || vertexMap[vsin] == -1) continue;

        featureFlagMap[vsrc] = 0;
        featureFlagMap[vsin] = 1;
    }

    // Build affinity matrix: -Affinity
    std::vector< Eigen::Triplet<double> > tripletList;
    // Preallocate space for non zero off diagonals and diagonals
    tripletList.reserve(2 * s_it->edges.size() + s_it->vers.size());

    for (auto it = s_it->edges.begin(); it != s_it->edges.end(); ++it)
    {
        // The row of source or sink need to have 1 on diagonal and 0 elsewhere
        if (gph.es[*it].w != 0)
        {
            // n1 is not a source or sink
            if (featureFlagMap[gph.es[*it].n1] == -1)
            {
                tripletList.push_back(Eigen::Triplet<double>(vertexMap[gph.es[*it].n1], vertexMap[gph.es[*it].n2], -gph.es[*it].w));
            }
            // n2 is not a source or sink
            if (featureFlagMap[gph.es[*it].n2] == -1)
            {
                tripletList.push_back(Eigen::Triplet<double>(vertexMap[gph.es[*it].n2], vertexMap[gph.es[*it].n1], -gph.es[*it].w));
            }
        }
    }

    // Fill in 1 for source and sinks and some dummy values for diagonal
    // In the mean time, build "b" for Ax=b
    Eigen::VectorXd b;
    b.setConstant(s_it->vers.size(), 0);
    for (auto it = s_it->vers.begin(); it != s_it->vers.end(); ++it)
    {
        // This vertex is a source
        if (featureFlagMap[*it] == 0)
        {
            b[vertexMap[*it]] = srcVal;
            tripletList.push_back(Eigen::Triplet<double>(vertexMap[*it], vertexMap[*it], -1.0));
        }
        // This vertex is a sink
        else if (featureFlagMap[*it] == 1)
        {
            b[vertexMap[*it]] = sinVal;
            tripletList.push_back(Eigen::Triplet<double>(vertexMap[*it], vertexMap[*it], -1.0));
        }
        // This is just a vertex
        else
        {
            tripletList.push_back(Eigen::Triplet<double>(vertexMap[*it], vertexMap[*it], 0.0));
        }
    }

    // Setup the final affinity sparse matrix
    Eigen::SparseMatrix<double, Eigen::ColMajor> LaplacianM(s_it->vers.size(), s_it->vers.size());
    LaplacianM.setFromTriplets(tripletList.begin(), tripletList.end());

    // Build sparse Laplacian matrix and in the mean time modify Laplacian matrix to build matrix "A"
    // Compute the sum on diagonal looped by column
    std::vector<double> tempDiagonal(LaplacianM.cols(), 0);
    for (int k = 0; k < LaplacianM.outerSize(); ++k)
    {
        double tempColSum = 0;
        for (Eigen::SparseMatrix<double, Eigen::ColMajor>::InnerIterator it(LaplacianM, k); it; ++it)
        {
            // We stored -w_ij and -1 in "-Affinity"
            tempDiagonal[it.row()] -= it.value();
        }
    }

    // Add the diagonal to build "A"
    for (int i = 0; i < LaplacianM.cols(); ++i)
    {
        LaplacianM.coeffRef(i, i) = tempDiagonal[i];
    }

    // Set up "x" for Ax=b
    Eigen::VectorXd x(s_it->vers.size());

    // Solve for Ax=b
    LaplacianM.makeCompressed();
//.........這裏部分代碼省略.........

static void omxRowFitFunctionSingleIteration(omxFitFunction *localobj, omxFitFunction *sharedobj, int rowbegin, int rowcount,
					     FitContext *fc) {

    omxRowFitFunction* oro = ((omxRowFitFunction*) localobj->argStruct);
    omxRowFitFunction* shared_oro = ((omxRowFitFunction*) sharedobj->argStruct);

    omxMatrix *rowAlgebra, *rowResults;
    omxMatrix *filteredDataRow, *dataRow, *existenceVector;
    omxMatrix *dataColumns;
	omxData *data;
	int isContiguous, contiguousStart, contiguousLength;
    int numCols, numRemoves;

	rowAlgebra	    = oro->rowAlgebra;
	rowResults	    = shared_oro->rowResults;
	data		    = oro->data;
    dataColumns     = oro->dataColumns;
    dataRow         = oro->dataRow;
    filteredDataRow = oro->filteredDataRow;
    existenceVector = oro->existenceVector;
    
    isContiguous    = oro->contiguous.isContiguous;
	contiguousStart = oro->contiguous.start;
	contiguousLength = oro->contiguous.length;

	Eigen::VectorXd oldDefs;
	oldDefs.resize(data->defVars.size());
	oldDefs.setConstant(NA_REAL);

	numCols = dataColumns->cols;
	int *toRemove = (int*) malloc(sizeof(int) * dataColumns->cols);
	int *zeros = (int*) calloc(dataColumns->cols, sizeof(int));

	for(int row = rowbegin; row < data->rows && (row - rowbegin) < rowcount; row++) {

		data->handleDefinitionVarList(localobj->matrix->currentState, row, oldDefs.data());

		omxStateNextRow(localobj->matrix->currentState);						// Advance row
		
        // Populate data row
		numRemoves = 0;
	
		if (isContiguous) {
			omxContiguousDataRow(data, row, contiguousStart, contiguousLength, dataRow);
		} else {
			omxDataRow(data, row, dataColumns, dataRow);	// Populate data row
		}

		markDataRowDependencies(localobj->matrix->currentState, oro);
		
		for(int j = 0; j < dataColumns->cols; j++) {
			double dataValue = omxVectorElement(dataRow, j);
			if(std::isnan(dataValue)) {
				numRemoves++;
				toRemove[j] = 1;
                omxSetVectorElement(existenceVector, j, 0);
			} else {
			    toRemove[j] = 0;
                omxSetVectorElement(existenceVector, j, 1);
			}
		}		
		// TODO: Determine if this is the correct response.
		
		if(numRemoves == numCols) {
			char *errstr = (char*) calloc(250, sizeof(char));
			sprintf(errstr, "Row %d completely missing.  omxRowFitFunction cannot have completely missing rows.", omxDataIndex(data, row));
			omxRaiseError(errstr);
			free(errstr);
			continue;
		}

		omxCopyMatrix(filteredDataRow, dataRow);
		omxRemoveRowsAndColumns(filteredDataRow, 0, numRemoves, zeros, toRemove);

		omxRecompute(rowAlgebra, fc);

		omxCopyMatrixToRow(rowAlgebra, omxDataIndex(data, row), rowResults);
	}
	free(toRemove);
	free(zeros);
}

void Model::construct_weight_vector(Eigen::VectorXd &W) {

  W.setConstant(1.0 / n_dimensions);
}