// Gradient computations
double DenseCRF::gradient( int n_iterations, const ObjectiveFunction & objective, VectorXf * unary_grad, VectorXf * lbl_cmp_grad, VectorXf * kernel_grad) const {
// Run inference
std::vector< MatrixXf > Q(n_iterations+1);
MatrixXf tmp1, unary( M_, N_ ), tmp2;
unary.fill(0);
if( unary_ )
unary = unary_->get();
expAndNormalize( Q[0], -unary );
for( int it=0; it<n_iterations; it++ ) {
tmp1 = -unary;
for( unsigned int k=0; k<pairwise_.size(); k++ ) {
pairwise_[k]->apply( tmp2, Q[it] );
tmp1 -= tmp2;
}
expAndNormalize( Q[it+1], tmp1 );
}
// Compute the objective value
MatrixXf b( M_, N_ );
double r = objective.evaluate( b, Q[n_iterations] );
sumAndNormalize( b, b, Q[n_iterations] );
// Compute the gradient
if(unary_grad && unary_)
*unary_grad = unary_->gradient( b );
if( lbl_cmp_grad )
*lbl_cmp_grad = 0*labelCompatibilityParameters();
if( kernel_grad )
*kernel_grad = 0*kernelParameters();
for( int it=n_iterations-1; it>=0; it-- ) {
// Do the inverse message passing
tmp1.fill(0);
int ip = 0, ik = 0;
// Add up all pairwise potentials
for( unsigned int k=0; k<pairwise_.size(); k++ ) {
// Compute the pairwise gradient expression
if( lbl_cmp_grad ) {
VectorXf pg = pairwise_[k]->gradient( b, Q[it] );
lbl_cmp_grad->segment( ip, pg.rows() ) += pg;
ip += pg.rows();
}
// Compute the kernel gradient expression
if( kernel_grad ) {
VectorXf pg = pairwise_[k]->kernelGradient( b, Q[it] );
kernel_grad->segment( ik, pg.rows() ) += pg;
ik += pg.rows();
}
// Compute the new b
pairwise_[k]->applyTranspose( tmp2, b );
tmp1 += tmp2;
}
sumAndNormalize( b, tmp1.array()*Q[it].array(), Q[it] );
// Add the gradient
if(unary_grad && unary_)
*unary_grad += unary_->gradient( b );
}
return r;
}
VectorXf param_sensitivity_widget::computeSensitivity(
MatrixXf ¶meterMatrix, VectorXf &responseVector)
{
MatrixXf Ctemp = parameterMatrix.transpose()*parameterMatrix;
MatrixXf C;
C = Ctemp.inverse();
VectorXf b = C*parameterMatrix.transpose()*responseVector;
VectorXf Y_hat = parameterMatrix*b;
int p = b.rows();
VectorXf sigma2Vec = responseVector-Y_hat;
float sigma2 = sigma2Vec.squaredNorm();
sigma2= sigma2/(parameterMatrix.rows() - p);
Ctemp = C*sigma2;
MatrixXf denominator = Ctemp.diagonal();
// Do element-wise division
VectorXf t = b;
for (int i = 0; i < b.rows(); i++)
{
t(i) = abs(b(i)/sqrt(denominator(i)));
}
return t;
}
void D3DRandomProjector::doComputation(const sensor_msgs::PointCloud& data,
cloud_kdtree::KdTree& data_kdtree,
const vector<const std::vector<int>*>& interest_region_indices,
vector<vector<float> >& results)
{
assert(results.size() == interest_region_indices.size());
vvf orig_results;
descriptor_->compute(data, data_kdtree, interest_region_indices, orig_results); //TODO: Make descriptors cache their results.
results = vvf(orig_results.size());
for(size_t i=0; i<orig_results.size(); ++i) {
if(orig_results[i].empty()) {
results[i] = vector<float>(0);
continue;
}
// -- Project the feature.
VectorXf vec;
floatToEigen(orig_results[i], &vec);
VectorXf projected = projector_ * vec;
// -- Put into results.
results[i] = vector<float>(projected.rows());
for(size_t j=0; j<results[i].size(); ++j) {
results[i][j] = projected(j);
}
}
}
QStringList param_sensitivity_widget::sortSensitivity(VectorXf &senVal,
QStringList ¶mNames)
{
// We need to sort senVal and use the corresponding indices to sort
// paramNames
// Step 1: Convert senVal to vector of pairs
std::vector<std::pair<float,int> > sortedSenVals;
std::pair<float,int> senValuePair;
for (unsigned int i = 1; i < senVal.rows(); ++i)
{
senValuePair.first = senVal(i);
senValuePair.second = i-1; // We are ignoring first element
sortedSenVals.push_back(senValuePair);
}
// Step 2: Sort this vector
std::sort(sortedSenVals.begin(), sortedSenVals.end());
QStringList sortedParamNames;
// Step 3: Rewrite the senVal and paramNames
for (unsigned int i = 0; i < sortedSenVals.size(); ++i)
{
sortedParamNames.push_back(paramNames.at(sortedSenVals.at(i).second));
senVal[i] = sortedSenVals.at(i).first;
}
return sortedParamNames;
}
void RtSourceDataWorker::normalizeAndTransformToColor(const VectorXf& vecData,
MatrixX3f& matFinalVertColor,
double dThresholdX,
double dThresholdZ,
QRgb (*functionHandlerColorMap)(double v))
{
//Note: This function needs to be implemented extremly efficient.
if(vecData.rows() != matFinalVertColor.rows()) {
qDebug() << "RtSourceDataWorker::normalizeAndTransformToColor - Sizes of input data (" << vecData.rows() <<") do not match output data ("<< matFinalVertColor.rows() <<"). Returning ...";
return;
}
float fSample;
QRgb qRgb;
const double dTresholdDiff = dThresholdZ - dThresholdX;
for(int r = 0; r < vecData.rows(); ++r) {
//Take the absolute values because the histogram threshold is also calcualted using the absolute values
fSample = std::fabs(vecData(r));
if(fSample >= dThresholdX) {
//Check lower and upper thresholds and normalize to one
if(fSample >= dThresholdZ) {
fSample = 1.0f;
} else {
if(fSample != 0.0f && dTresholdDiff != 0.0 ) {
fSample = (fSample - dThresholdX) / (dTresholdDiff);
} else {
fSample = 0.0f;
}
}
qRgb = functionHandlerColorMap(fSample);
matFinalVertColor(r,0) = (float)qRed(qRgb)/255.0f;
matFinalVertColor(r,1) = (float)qGreen(qRgb)/255.0f;
matFinalVertColor(r,2) = (float)qBlue(qRgb)/255.0f;
}
}
}
QByteArray BrainSurfaceTreeItem::createCurvatureVertColor(const VectorXf& curvature, const QColor& colSulci, const QColor& colGyri)
{
QByteArray arrayCurvatureColor;
arrayCurvatureColor.resize(curvature.rows() * 3 * (int)sizeof(float));
float *rawColorArray = reinterpret_cast<float *>(arrayCurvatureColor.data());
int idxColor = 0;
for(int i = 0; i<curvature.rows(); i++) {
//Color (this is the default color and will be used until the updateVertColor function was called)
if(curvature[i] >= 0) {
rawColorArray[idxColor++] = colSulci.redF();
rawColorArray[idxColor++] = colSulci.greenF();
rawColorArray[idxColor++] = colSulci.blueF();
} else {
rawColorArray[idxColor++] = colGyri.redF();
rawColorArray[idxColor++] = colGyri.greenF();
rawColorArray[idxColor++] = colGyri.blueF();
}
}
return arrayCurvatureColor;
}
void plotNodesAsSpheres(const vector<btVector3>& nodes, const VectorXf& pVis, const Eigen::VectorXf& stdev, PlotSpheres::Ptr spheres) {
int nPts = pVis.rows();
using namespace osg;
ref_ptr<Vec3Array> centers = new Vec3Array();
ref_ptr<Vec4Array> colors = new Vec4Array();
//vector<float> sizes = toVec(stdev.array().sqrt());
vector<float> sizes = toVec(stdev/4.0);
for (int i=0; i<nPts; i++) {
float p = pVis[i];
centers->push_back(Vec3f(nodes[i].x(), nodes[i].y(), nodes[i].z()));
colors->push_back(Vec4f(p,p,p,.25));
}
spheres->plot(centers, colors, sizes);
}
//! Use a QP solver to determine the distance from a point to a polytope.
double computePointToPolytopeDistanceSquared(const vector<VectorXf>& As, const vector<float>& Bs, const VectorXf& point) {
assert(Bs.size() == As.size());
// -- Put all variables into a form that qpOASES can use.
int num_rows = As.size();
int num_cols = As[0].rows(); //I know, it's weird.
double A[num_rows * num_cols];
for(int i=0; i<num_rows; ++i) {
assert(As[i].rows() == num_cols);
for(int j=0; j<num_cols; ++j) {
A[i*num_cols + j] = As[i](j);
}
}
double ubA[Bs.size()];
for(size_t i=0; i<Bs.size(); ++i) {
ubA[i] = Bs[i];
}
double g[point.rows()];
for(int i=0; i<point.rows(); ++i) {
g[i] = -point(i);
}
// -- Run the solver.
int nWSR = 100;
QProblem solver(point.rows(), As.size(), HST_IDENTITY); //Our Hessian = I.
solver.setPrintLevel(PL_NONE);
returnValue rv = solver.init(NULL, g, A, NULL, NULL, NULL, ubA, nWSR, NULL);
if(rv != SUCCESSFUL_RETURN) {
cout << "Bad solve!" << endl;
assert(0);
}
double xopt[point.rows()];
solver.getPrimalSolution(xopt);
// -- Put xopt into eigen.
VectorXf closest_in_poly = VectorXf::Zero(point.rows());
for(int i=0; i<closest_in_poly.rows(); ++i) {
closest_in_poly(i) = xopt[i];
}
for(size_t i=0; i<As.size(); ++i) {
assert(As[i].dot(closest_in_poly) - Bs[i] <= 1e-4);
}
// -- Return the distance.
double dist=0;
for(int i=0; i<point.rows(); ++i) {
double tmp = (point(i) - xopt[i]);
dist += tmp * tmp;
}
return dist;
}
MatrixX3f FsSurfaceTreeItem::createCurvatureVertColor(const VectorXf& curvature, const QColor& colSulci, const QColor& colGyri) const
{
MatrixX3f colors(curvature.rows(), 3);
for(int i = 0; i < colors.rows(); ++i) {
//Color (this is the default color and will be used until the updateVertColor function was called)
if(curvature[i] >= 0) {
colors(i,0) = colSulci.redF();
colors(i,1) = colSulci.greenF();
colors(i,2) = colSulci.blueF();
} else {
colors(i,0) = colGyri.redF();
colors(i,1) = colGyri.greenF();
colors(i,2) = colGyri.blueF();
}
}
return colors;
}
void WCTree::growTree() {
// -- If we don't need any more splits, we're done.
if(pwcs_.size() < max_wcs_ || level_ == recursion_limit_) {
makeLeaf();
return;
}
// -- Fill X_ with the center points.
for(size_t i=0; i<pwcs_.size(); ++i) {
X_.col(i) = pwcs_[i]->center_;
}
// -- Subtract off the mean.
VectorXf mean = X_.rowwise().sum() / X_.cols();
for(size_t i=0; i<pwcs_.size(); ++i) {
X_.col(i) -= mean;
}
// -- If all of the weak classifiers had the same center, this is also a leaf.
if(X_.sum() == 0) {
makeLeaf();
return;
}
// -- Power method to find the eigenvector of XX', i.e. 1st principal component.
MatrixXf Xt = X_.transpose();
bool done = false;
while(!done) {
a_ = getRandomVector(X_.rows());
a_.normalize();
VectorXf prev = a_;
while(true) {
prev = a_;
a_ = X_ * (Xt * a_);
assert(a_.sum() != 0);
if(a_.sum() == 0) {
break;
}
a_.normalize();
if((a_ - prev).norm() < thresh_) {
done = true;
break;
}
}
}
// -- Compute b_ to be the mean value of a_'xt for xt in pwcs_.
VectorXf bs = VectorXf::Zero(pwcs_.size());
for(size_t i=0; i<pwcs_.size(); ++i) {
bs(i) = a_.dot(pwcs_[i]->center_);
}
b_ = bs.sum() / bs.rows();
// -- Add the newly computed a_ and b_ into the full list of constraints for the left and right children.
// The right child region is all x for a_'x >= b_, or -a_'x <= -b_
// The left child region is all x for a_'x <= b_
vector<VectorXf> region_a_left = region_a_;
vector<VectorXf> region_a_right = region_a_;
vector<float> region_b_left = region_b_;
vector<float> region_b_right = region_b_;
region_a_left.push_back(a_);
region_b_left.push_back(b_);
region_a_right.push_back(-a_);
region_b_right.push_back(-b_);
// -- Compute which weak classifiers in the region go on which side of the split.
vector<WeakClassifier*> left, right, left_consider, right_consider;
left.reserve(pwcs_.size() + consider_.size());
right.reserve(pwcs_.size() + consider_.size());
left_consider.reserve(pwcs_.size() + consider_.size());
right_consider.reserve(pwcs_.size() + consider_.size());
for(size_t i=0; i<pwcs_.size(); ++i) {
double dist = bs(i) - b_; //computeDistanceToSplit(pwcs_[i]->center_);
double dist2 = dist*dist;
if(dist == 0) {
right.push_back(pwcs_[i]);
left.push_back(pwcs_[i]);
}
else if(dist > 0) {
right.push_back(pwcs_[i]);
if(dist2 <= pwcs_[i]->theta_) {
left_consider.push_back(pwcs_[i]);
}
}
else {
left.push_back(pwcs_[i]);
if(dist2 <= pwcs_[i]->theta_) {
right_consider.push_back(pwcs_[i]);
}
}
}
// -- If all the weak classifiers are very close to each other, they can end up not being split by the
// boundary. If this happens, then call this a leaf and be done with it.
if(left.empty() || right.empty()) {
makeLeaf();
return;
}
// -- See which weak classifiers that leak into this region might also leak into the child regions.
for(size_t i=0; i<consider_.size(); ++i) {
if(USE_QP) {
int lc2=0, rc2=0;
// -- Use QP solver to find which wcs belong in the consider list.
clock_t start = clock();
double dist_left = computePointToPolytopeDistanceSquared(region_a_left, region_b_left, consider_[i]->center_);
double dist_right = computePointToPolytopeDistanceSquared(region_a_right, region_b_right, consider_[i]->center_);
// cout << "Took " << (double) (clock() - start) / (double) CLOCKS_PER_SEC * (double) 1000 << " ms to do 2 QP solves." << endl;
// -- Add to consider lists.
if(dist_left <= consider_[i]->theta_) {
left_consider.push_back(consider_[i]);
lc2++;
}
if(dist_right <= consider_[i]->theta_) {
right_consider.push_back(consider_[i]);
rc2++;
}
}
else {
// -- Old version.
int rc=0, lc=0;
double dist = computeDistanceToSplit(consider_[i]->center_);
double dist2 = dist*dist;
if(dist == 0) {
rc++;
lc++;
right_consider.push_back(pwcs_[i]);
left_consider.push_back(pwcs_[i]);
}
else if(dist > 0) {
rc++;
right_consider.push_back(consider_[i]);
if(dist2 <= consider_[i]->theta_) {
lc++;
left_consider.push_back(consider_[i]);
}
}
else {
lc++;
left_consider.push_back(consider_[i]);
if(dist2 <= consider_[i]->theta_) {
rc++;
right_consider.push_back(consider_[i]);
}
}
}
}
// -- Create the split.
double left_removed = pwcs_.size() + consider_.size() - left.size() - left_consider.size();
double right_removed = pwcs_.size() + consider_.size() - right.size() - right_consider.size();
// cout << "Split removed an average of " << (left_removed + right_removed)/2.0 << " wcs." << endl;
lchild_ = new WCTree(left, left_consider, recursion_limit_, level_+1, region_a_left, region_b_left);
rchild_ = new WCTree(right, right_consider, recursion_limit_, level_+1, region_a_right, region_b_right);
}
