rk_result_t Robot::dampedLeastSquaresIK_linkage(const string linkageName, VectorXd &jointValues,
const Isometry3d &target, const Isometry3d &finalTF)
{
vector<size_t> jointIndices;
jointIndices.resize(linkage(linkageName).joints_.size());
for(size_t i=0; i<linkage(linkageName).joints_.size(); i++)
jointIndices[i] = linkage(linkageName).joints_[i]->id();
Isometry3d linkageFinalTF;
linkageFinalTF = linkage(linkageName).tool().respectToFixed()*finalTF;
return dampedLeastSquaresIK_chain(jointIndices, jointValues, target, linkageFinalTF);
}
extern PyObject *linkage_wrap(PyObject *self, PyObject *args) {
int method, n;
PyArrayObject *dm, *Z;
distfunc *df;
if (!PyArg_ParseTuple(args, "O!O!ii",
&PyArray_Type, &dm,
&PyArray_Type, &Z,
&n,
&method)) {
return 0;
}
else {
switch (method) {
case CPY_LINKAGE_SINGLE:
df = dist_single;
break;
case CPY_LINKAGE_COMPLETE:
df = dist_complete;
break;
case CPY_LINKAGE_AVERAGE:
df = dist_average;
break;
case CPY_LINKAGE_WEIGHTED:
df = dist_weighted;
break;
default:
/** Report an error. */
df = 0;
break;
}
linkage((double*)dm->data, (double*)Z->data, 0, 0, n, 0, 0, df, method);
}
return Py_BuildValue("d", 0.0);
}
bool X86_64TargetABI::returnInArg(TypeFunction* tf) {
assert(linkage() == tf->linkage);
Type* rt = tf->next->toBasetype();
if (tf->linkage == LINKd) {
#if DMDV2
if (tf->isref)
return false;
#endif
// All non-structs can be returned in registers.
return (rt->ty == Tstruct);
} else {
if (rt == Type::tvoid || keepUnchanged(rt))
return false;
Classification cl = classify(rt);
if (cl.isMemory) {
assert(state().int_regs > 0
&& "No int registers available when determining sret-ness?");
// An sret parameter takes an integer register.
state().int_regs--;
return true;
}
return false;
}
}
void MaxMeaningfulClustering::operator()(t_float *data, unsigned int num, unsigned char method, vector< vector<int> > *meaningful_clusters)
{
t_float *Z = (t_float*)malloc(((num-1)*4) * sizeof(t_float)); // we need 4 floats foreach sample merge.
linkage(data, (int)num, Z, method); //TODO think if complete linkage is the correct
vector<HCluster> merge_info;
build_merge_info(Z, (int)num, &merge_info, meaningful_clusters);
free(Z);
merge_info.clear();
}
extern PyObject *linkage_wrap(PyObject *self, PyObject *args) {
int method, n;
PyArrayObject *dm, *Z;
distfunc *df;
if (!PyArg_ParseTuple(args, "O!O!ii",
&PyArray_Type, &dm,
&PyArray_Type, &Z,
&n,
&method)) {
return NULL;
}
else {
switch (method) {
case CPY_LINKAGE_SINGLE:
df = dist_single;
break;
case CPY_LINKAGE_COMPLETE:
df = dist_complete;
break;
case CPY_LINKAGE_AVERAGE:
df = dist_average;
break;
case CPY_LINKAGE_WEIGHTED:
df = dist_weighted;
break;
case CPY_LINKAGE_WARD:
df = dist_ward;
break;
default:
/** Report an error. */
df = 0;
break;
}
if (linkage((double*)dm->data, (double*)Z->data,
0, 0, n, 0, 0, df, method) == -1) {
PyErr_SetString(PyExc_MemoryError,
"out of memory while computing linkage");
return NULL;
}
}
return Py_BuildValue("d", 0.0);
}
extern PyObject *linkage_euclid_wrap(PyObject *self, PyObject *args) {
int method, m, n, ml;
PyArrayObject *dm, *Z, *X;
distfunc *df;
if (!PyArg_ParseTuple(args, "O!O!O!iii",
&PyArray_Type, &dm,
&PyArray_Type, &Z,
&PyArray_Type, &X,
&m,
&n,
&method)) {
return NULL;
}
else {
ml = 0;
/** fprintf(stderr, "m: %d, n: %d\n", m, n);**/
switch (method) {
case CPY_LINKAGE_CENTROID:
df = dist_centroid;
break;
case CPY_LINKAGE_MEDIAN:
df = dist_centroid;
break;
default:
/** Report an error. */
df = 0;
break;
}
if (linkage((double*)dm->data, (double*)Z->data, (double*)X->data,
m, n, 1, 1, df, method) == -1) {
PyErr_SetString(PyExc_MemoryError,
"out of memory while computing linkage");
return NULL;
}
}
return Py_BuildValue("d", 0.0);
}
extern PyObject *linkage_euclid_wrap(PyObject *self, PyObject *args) {
int method, m, n, ml;
PyArrayObject *dm, *Z, *X;
distfunc *df;
if (!PyArg_ParseTuple(args, "O!O!O!iii",
&PyArray_Type, &dm,
&PyArray_Type, &Z,
&PyArray_Type, &X,
&m,
&n,
&method)) {
return 0;
}
else {
ml = 0;
/** fprintf(stderr, "m: %d, n: %d\n", m, n);**/
switch (method) {
case CPY_LINKAGE_CENTROID:
df = dist_centroid;
break;
case CPY_LINKAGE_MEDIAN:
df = dist_centroid;
break;
case CPY_LINKAGE_WARD:
df = dist_ward;
// ml = 1;
break;
default:
/** Report an error. */
df = 0;
break;
}
linkage((double*)dm->data, (double*)Z->data, (double*)X->data,
m, n, 1, 1, df, method);
}
return Py_BuildValue("d", 0.0);
}
void CMT::estimate(const std::vector<std::pair<cv::KeyPoint, int> >& keypointsIN, cv::Point2f& center, float& scaleEstimate, float& medRot, std::vector<std::pair<cv::KeyPoint, int> >& keypoints)
{
center = cv::Point2f(NAN,NAN);
scaleEstimate = NAN;
medRot = NAN;
//At least 2 keypoints are needed for scale
if(keypointsIN.size() > 1)
{
//sort
std::vector<PairInt> list;
for(int i = 0; i < keypointsIN.size(); i++)
list.push_back(std::make_pair(keypointsIN[i].second, i));
std::sort(&list[0], &list[0]+list.size(), comparatorPair<int>);
for(int i = 0; i < list.size(); i++)
keypoints.push_back(keypointsIN[list[i].second]);
std::vector<int> ind1;
std::vector<int> ind2;
for(int i = 0; i < list.size(); i++)
for(int j = 0; j < list.size(); j++)
{
if(i != j && keypoints[i].second != keypoints[j].second)
{
ind1.push_back(i);
ind2.push_back(j);
}
}
if(ind1.size() > 0)
{
std::vector<int> class_ind1;
std::vector<int> class_ind2;
std::vector<cv::KeyPoint> pts_ind1;
std::vector<cv::KeyPoint> pts_ind2;
for(int i = 0; i < ind1.size(); i++)
{
class_ind1.push_back(keypoints[ind1[i]].second-1);
class_ind2.push_back(keypoints[ind2[i]].second-1);
pts_ind1.push_back(keypoints[ind1[i]].first);
pts_ind2.push_back(keypoints[ind2[i]].first);
}
std::vector<float> scaleChange;
std::vector<float> angleDiffs;
for(int i = 0; i < pts_ind1.size(); i++)
{
cv::Point2f p = pts_ind2[i].pt - pts_ind1[i].pt;
//This distance might be 0 for some combinations,
//as it can happen that there is more than one keypoint at a single location
float dist = sqrt(p.dot(p));
float origDist = squareForm[class_ind1[i]][class_ind2[i]];
scaleChange.push_back(dist/origDist);
//Compute angle
float angle = atan2(p.y, p.x);
float origAngle = angles[class_ind1[i]][class_ind2[i]];
float angleDiff = angle - origAngle;
//Fix long way angles
if(fabs(angleDiff) > CV_PI)
angleDiff -= sign(angleDiff) * 2 * CV_PI;
angleDiffs.push_back(angleDiff);
}
scaleEstimate = median(scaleChange);
if(!estimateScale)
scaleEstimate = 1;
medRot = median(angleDiffs);
if(!estimateRotation)
medRot = 0;
votes = std::vector<cv::Point2f>();
for(int i = 0; i < keypoints.size(); i++)
votes.push_back(keypoints[i].first.pt - scaleEstimate * rotate(springs[keypoints[i].second-1], medRot));
//Compute linkage between pairwise distances
std::vector<Cluster> linkageData = linkage(votes);
//Perform hierarchical distance-based clustering
std::vector<int> T = fcluster(linkageData, thrOutlier);
//Count votes for each cluster
std::vector<int> cnt = binCount(T);
//Get largest class
int Cmax = argmax(cnt);
//Remember outliers
outliers = std::vector<std::pair<cv::KeyPoint, int> >();
std::vector<std::pair<cv::KeyPoint, int> > newKeypoints;
std::vector<cv::Point2f> newVotes;
for(int i = 0; i < keypoints.size(); i++)
{
if(T[i] != Cmax)
outliers.push_back(keypoints[i]);
else
{
newKeypoints.push_back(keypoints[i]);
newVotes.push_back(votes[i]);
}
}
keypoints = newKeypoints;
center = cv::Point2f(0,0);
for(int i = 0; i < newVotes.size(); i++)
center += newVotes[i];
center *= (1.0/newVotes.size());
}
}
}
bool X86_64TargetABI::passByVal(Type* t) {
t = t->toBasetype();
if (linkage() == LINKd) {
return t->toBasetype()->ty == Tstruct;
} else {
// This implements the C calling convention for x86-64.
// It might not be correct for other calling conventions.
Classification cl = classify(t);
if (cl.isMemory)
return true;
// Figure out how many registers we want for this arg:
RegCount wanted = { 0, 0 };
for (int i = 0 ; i < 2; i++) {
if (cl.classes[i] == Integer)
wanted.int_regs++;
else if (cl.classes[i] == Sse)
wanted.sse_regs++;
}
// See if they're available:
RegCount& state = this->state();
if (wanted.int_regs <= state.int_regs && wanted.sse_regs <= state.sse_regs) {
state.int_regs -= wanted.int_regs;
state.sse_regs -= wanted.sse_regs;
} else {
if (keepUnchanged(t)) {
// Not enough registers available, but this is passed as if it's
// multiple arguments. Just use the registers there are,
// automatically spilling the rest to memory.
if (wanted.int_regs > state.int_regs)
state.int_regs = 0;
else
state.int_regs -= wanted.int_regs;
if (wanted.sse_regs > state.sse_regs)
state.sse_regs = 0;
else
state.sse_regs -= wanted.sse_regs;
} else if (t->iscomplex() || t->ty == Tstruct) {
// Spill entirely to memory, even if some of the registers are
// available.
// FIXME: Don't do this if *none* of the wanted registers are available,
// (i.e. only when absolutely necessary for abi-compliance)
// so it gets alloca'd by the callee and -scalarrepl can
// more easily break it up?
// Note: this won't be necessary if the following LLVM bug gets fixed:
// http://llvm.org/bugs/show_bug.cgi?id=3741
return true;
} else {
assert(t == Type::tfloat80 || t == Type::timaginary80 || t->ty == Tsarray || t->size() <= 8
&& "What other big types are there?");
// In any case, they shouldn't be represented as structs in LLVM:
assert(!isaStruct(DtoType(t)));
}
}
// Everything else that's passed in memory is handled by LLVM.
return false;
}
}
//==============================================================================
LinkagePtr Linkage::create(const Criteria &_criteria, const std::string& _name)
{
LinkagePtr linkage(new Linkage(_criteria, _name));
linkage->mPtr = linkage;
return linkage;
}
void SnpEstimation::estimate(GenotypeFileHandler &genotypeFileHandler, boost::ptr_vector<Snp> &snpList, boost::ptr_vector<Region> ®ionList){
// This contains the genotypes from the reference panel, the main container for this function
boost::ptr_deque<Genotype> genotype;
// This contains the index of the SNPs included in the genotype
std::deque<size_t> snpLoc;
// This should be invoked when we came to the end of chromosome / region
bool windowEnd = false;
// Initialize the linkage and decompose
Linkage linkage(m_thread);
Decomposition decompose(m_thread);
fprintf(stderr, "Estimate SNP heritability\n\n");
size_t doneItems = 0;
size_t totalSnp = snpList.size();
std::string chr = "";
// This is use for indicating whether if the whole genome is read
bool completed = false;
std::vector<size_t> boundary;
bool starting = true;
size_t checking = 0; //DEBUG
while(!completed){
// Keep doing this until the whole genome is read
progress(doneItems, totalSnp, chr);
// only used when the finalizeBuff is true, this indicate whether if the last block is coming from somewhere new
bool retainLastBlock=false;
while(boundary.size() < 5 && !completed && !windowEnd){
genotypeFileHandler.getBlock(snpList, genotype, snpLoc, windowEnd, completed,boundary, false);
bool boundChange = false;
linkage.construct(genotype, snpLoc, boundary, snpList, m_ldCorrection, boundChange);
if(boundChange && boundary.back()==snpLoc.size()){
windowEnd=true;
boundary.pop_back();
}
else if(boundChange&&snpList.at(snpLoc[boundary.back()]).getLoc()- snpList.at(snpLoc[boundary.back()-1]).getLoc() > m_blockSize){
retainLastBlock = true;
windowEnd=true;
}
genotypeFileHandler.getBlock(snpList, genotype, snpLoc, windowEnd, completed,boundary, true);
linkage.construct(genotype, snpLoc, boundary, snpList, m_ldCorrection, boundChange);
}
if(windowEnd && !retainLastBlock && boundary.size() > 2){ //Check whether if we need to merge the two blocks
size_t indexOfLastSnpOfSecondLastBlock = snpLoc.at(boundary.back()-1); // just in case
size_t lastSnp = snpLoc.back();
if(snpList.at(lastSnp).getLoc()-snpList.at(indexOfLastSnpOfSecondLastBlock).getLoc() <= m_blockSize){
boundary.pop_back();
bool boundChange=false;
linkage.construct(genotype, snpLoc, boundary, snpList, m_ldCorrection, boundChange);
}
}
if(retainLastBlock && !windowEnd) throw std::runtime_error("Impossible combination of windowEnd and retain last block!");
decompose.run(linkage, snpLoc, boundary, snpList, windowEnd, !retainLastBlock, starting, regionList);
doneItems= snpLoc.at(boundary.back());
chr = snpList[snpLoc[boundary.back()]].getChr();
if(retainLastBlock){
// Then we must remove everything except the last block
// because finalizeBuff must be true here
size_t update = boundary.back();
genotype.erase(genotype.begin(), genotype.begin()+update);
snpLoc.erase(snpLoc.begin(), snpLoc.begin()+update);
boundary.clear();
boundary.push_back(0);
linkage.clear(update);
starting = true;
windowEnd = false;
retainLastBlock = false;
}
else if(windowEnd){
snpLoc.clear();
boundary.clear();
genotype.clear();
linkage.clear();
starting = true;
windowEnd = false;
retainLastBlock = false;
}
else{
starting = false;
size_t update=boundary[1];
genotype.erase(genotype.begin(), genotype.begin()+update);
snpLoc.erase(snpLoc.begin(), snpLoc.begin()+update);
linkage.clear(update);
for(size_t i = 0; i < boundary.size()-1; ++i) boundary[i] = boundary[i+1]-update;
boundary.pop_back();
}
}
progress(totalSnp, totalSnp, "");
fprintf(stderr, "\n\nEstimated the SNP Heritability, now proceed to output\n");
}
