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();
solver.compute(LaplacianM);
x = solver.solve(b);
if (solver.info() != Eigen::Success)
{
// solving failed
std::cout << "Eigen sparse LU solver fail...\n";
}
//// Add more source & sink for graph cut
for (auto it = s_it->features.begin(); it != s_it->features.end(); ++it)
{
std::vector<int> tempSrc, tempSin;
int edgeNum = 0;
int polarizedEdgeNum = 0;
// Check the fraction --- So far we don't consider "twisted polarize" case.
for (auto it2 = gcFeatures[*it].begin(); it2 != gcFeatures[*it].end(); ++it2)
{
int v1 = vertexMap[gph.es[*it2].n1];
int v2 = vertexMap[gph.es[*it2].n2];
// At least one of v1 and v2 is not in this round's sub set
if (v1 == -1 || v2 == -1) continue;
// v1 is source
if (x[v1] > x[v2])
{
tempSrc.push_back(v1);
tempSin.push_back(v2);
}
// v2 is source
else
{
tempSrc.push_back(v2);
tempSin.push_back(v1);
}
// Accumulate edge
++edgeNum;
if (CheckPolarized(x[v1], x[v2], halfVal, myPolarizedThreshold))
{
// Accumulate polarized edge
++polarizedEdgeNum;
}
}
// Add the entire "polarized" feature
// For now, we consider the feature whose edges are all polarized to be polarized
if (polarizedEdgeNum == edgeNum)
{
for (int i = 0; i < (int)tempSrc.size(); ++i)
{
g->add_tweights(tempSrc[i], sm_largeDouble, 0);
g->add_tweights(tempSin[i], 0, sm_largeDouble);
}
}
} // end this feature
}
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);
}
