// draw all the lines between aPts and Pts that have a corr>threshold.
// if aPtInd is in the range of aPts, then draw only the lines that comes from aPts[aPtInd]
void drawCorrLines(PlotLines::Ptr lines, const vector<btVector3>& aPts, const vector<btVector3>& bPts, const Eigen::MatrixXf& corr, float threshold, int aPtInd) {
vector<btVector3> linePoints;
vector<btVector4> lineColors;
float max_corr = corr.maxCoeff(); // color lines by corr, where corr has been mapped [threshold,max_corr] -> [0,1]
for (int i=0; i<corr.rows(); ++i)
for (int j=0; j<corr.cols(); ++j)
if (corr(i,j) > threshold) {
if (aPtInd<0 || aPtInd>=corr.rows() || aPtInd==i) {
linePoints.push_back(aPts[i]);
linePoints.push_back(bPts[j]);
float color_factor = (corr(i,j)-threshold)/(max_corr-threshold); //basically, it ranges from 0 to 1
lineColors.push_back(btVector4(color_factor, color_factor,0,1));
}
}
lines->setPoints(linePoints, lineColors);
}
slimage::Image3ub PlotDeltaDensity(const Eigen::MatrixXf& dd)
{
return PlotDeltaDensity(dd,
std::max(std::abs(dd.minCoeff()), std::abs(dd.maxCoeff())));
}
slimage::Image3ub PlotDeltaDensity(const Eigen::MatrixXf& actual, const Eigen::MatrixXf& reference)
{
float a = std::abs(reference.maxCoeff());
return PlotDeltaDensity(actual - reference, 0.1f * a);
}
slimage::Image3ub PlotDensity(const Eigen::MatrixXf& d) {
return PlotDensity(d,
d.minCoeff(), d.maxCoeff());
}