void Layouter::laplacianEigMap( const MatrixXd& laplacian, const VectorXf& radiusVec, const VectorXi& hashID, MatrixXf& finalPos2D, float& finalRadius, float sparseFactor /*= 1.f*/ )
{
if (laplacian.rows() <= 0 || laplacian.cols() <= 0 || radiusVec.size() <= 0)
{
finalPos2D = MatrixXf::Zero(1,2);
finalRadius = 0;
return;
}
if (laplacian.rows() == 1 && laplacian.cols() == 1 && radiusVec.size() == 1)
{
finalPos2D = MatrixXf::Zero(1,2);
finalRadius = radiusVec[0];
return;
}
MatrixXd pos2D, finalPosd;
LaplacianSolver::compute(laplacian, pos2D);
VectorXd minPos, maxPos;
MDSPostProcesser m_postProcessor(500, 1.0f, 1.0, 1.02, radiusVec.minCoeff());
m_postProcessor.setSparseFactor(sparseFactor);
m_postProcessor.set2DPos(pos2D, radiusVec.cast<double>(), &hashID);
m_postProcessor.compute();
m_postProcessor.getFinalPos(finalPosd);
m_postProcessor.getFinalBoundingRect(minPos, maxPos);
finalPos2D = finalPosd.cast<float>();
// finalPos2D = pos2D.cast<float>();
// minPos = pos2D.colwise().minCoeff();
// maxPos = pos2D.colwise().maxCoeff();
// VectorXd size = maxPos - minPos;
finalRadius = m_postProcessor.getFinalRadius();// size.norm() * 0.5;//(size[0] > size[1] ? size[0] : size[1]) * 0.5f;
}
void Layouter::mds(const MatrixXd& distMat,
const VectorXf& radiusVec,
const VectorXi& hashID,
MatrixXf& finalPos2D,
float& finalRadius,
float sparseFactor,
float paddingRatio)
{
if (distMat.rows() <= 0 || distMat.cols() <= 0 || radiusVec.size() <= 0)
{
finalPos2D = MatrixXf::Zero(1,2);
finalRadius = 0;
return;
}
if (distMat.rows() == 1 && distMat.cols() == 1 && radiusVec.size() == 1)
{
finalPos2D = MatrixXf::Zero(1,2);
finalRadius = radiusVec[0];
return;
}
MatrixXd pos2D, finalPosd;
ClassicalMDSSolver::compute(distMat, pos2D);
VectorXd minPos, maxPos;
float minPadding = radiusVec.minCoeff();
MDSPostProcesser m_postProcessor(5000, sparseFactor, 1.0, paddingRatio, minPadding);
m_postProcessor.set2DPos(pos2D, radiusVec.cast<double>(), &hashID);
m_postProcessor.compute();
m_postProcessor.getFinalPos(finalPosd);
m_postProcessor.getFinalBoundingRect(minPos, maxPos);
finalPos2D = finalPosd.cast<float>();
// finalPos2D = pos2D.cast<float>();
// minPos = pos2D.colwise().minCoeff();
// maxPos = pos2D.colwise().maxCoeff();
// VectorXd size = maxPos - minPos;
finalRadius = m_postProcessor.getFinalRadius();// size.norm() * 0.5;//(size[0] > size[1] ? size[0] : size[1]) * 0.5f;
}
void wavePlotter::autoScaleRange(VectorXf &data)
{
lowRange = data.minCoeff();//Utils::ofMin(&data(0), DATA_SIZE);
highRange = data.maxCoeff();//Utils::ofMax(&data(0), DATA_SIZE);
}
void D3DCloudProjector::projectCloud(int id, const sensor_msgs::PointCloud& data, const std::vector<int>& interest_region_indices) {
MatrixXf& oriented = orienter_->oriented_clouds_[id];
// -- Get a copy of the projected points.
MatrixXf projected(oriented.rows(), 2);
int c=0;
for(int i=0; i<3; ++i) {
if(i == axis_of_projection_)
continue;
projected.col(c) = oriented.col(i);
++c;
}
// -- Transform into pixel units. projected is currently in meters, centered at 0.
//projected *= pixels_per_meter_;
for(int i=0; i<projected.rows(); ++i) {
projected(i, 0) *= pixels_per_meter_;
projected(i, 1) *= pixels_per_meter_;
}
// -- Find min and max of u and v. TODO: noise sensitivity?
// u is the col number in the image plane, v is the row number.
float min_v = FLT_MAX;
float min_u = FLT_MAX;
float max_v = -FLT_MAX;
float max_u = -FLT_MAX;
for(int i=0; i<projected.rows(); ++i) {
float u = projected(i, 0);
float v = projected(i, 1);
if(u < min_u)
min_u = u;
if(u > max_u)
max_u = u;
if(v < min_v)
min_v = v;
if(v > max_v)
max_v = v;
}
// -- Translate to coordinate system where (0,0) is the upper right of the image.
for(int i=0; i<projected.rows(); ++i) {
projected(i, 0) -= min_u;
projected(i, 1) = max_v - projected(i, 1);
}
// -- Get the max depth.
float max_depth = -FLT_MAX;
float min_depth = FLT_MAX;
for(int i=0; i<oriented.rows(); ++i) {
if(oriented(i, axis_of_projection_) > max_depth)
max_depth = oriented(i, axis_of_projection_);
if(oriented(i, axis_of_projection_) < min_depth)
min_depth = oriented(i, axis_of_projection_);
}
// -- Compute the normalized depths. Depths are between 0 and 1, with 1 meaning closest and 0 meaning furthest.
VectorXf depths = oriented.col(axis_of_projection_);
if(axis_of_projection_ == 1)
depths = -depths;
depths = depths.cwise() - depths.minCoeff();
depths = depths / depths.maxCoeff();
// -- Fill the IplImages.
assert(sizeof(float) == 4);
CvSize size = cvSize(ceil(max_u - min_u), ceil(max_v - min_v));
IplImage* acc = cvCreateImage(size, IPL_DEPTH_32F, 1);
IplImage* intensity = cvCreateImage(size, IPL_DEPTH_32F, 1);
IplImage* depth = cvCreateImage(size, IPL_DEPTH_32F, 1);
cvSetZero(acc);
cvSetZero(depth);
cvSetZero(intensity);
assert(projected.rows() == (int)interest_region_indices.size());
for(int i=0; i<projected.rows(); ++i) {
int row = floor(projected(i, 1));
int col = floor(projected(i, 0));
// Update accumulator.
assert(interest_region_indices[i] < (int)data.channels[0].values.size() && (int)interest_region_indices[i] >= 0);
((float*)(acc->imageData + row * acc->widthStep))[col]++;
// Add to intensity values.
float val = (float)data.channels[0].values[interest_region_indices[i]] / 255.0 * (3.0 / 4.0) + 0.25;
assert(val <= 1.0 && val >= 0.0);
((float*)(intensity->imageData + row * intensity->widthStep))[col] += val;
// Add to depth values.
((float*)(depth->imageData + row * depth->widthStep))[col] += depths(i); //
}
// -- Normalize by the number of points falling in each pixel.
for(int v=0; v<acc->height; ++v) {
float* intensity_ptr = (float*)(intensity->imageData + v * intensity->widthStep);
float* depth_ptr = (float*)(depth->imageData + v * depth->widthStep);
float* acc_ptr = (float*)(acc->imageData + v * acc->widthStep);
for(int u=0; u<acc->width; ++u) {
if(*acc_ptr == 0) {
*intensity_ptr = 0;
*depth_ptr = 0;
}
else {
*intensity_ptr = *intensity_ptr / *acc_ptr;
*depth_ptr = *depth_ptr / *acc_ptr;
}
intensity_ptr++;
depth_ptr++;
acc_ptr++;
}
}
// -- Store images.
depth_projections_.push_back(depth);
intensity_projections_.push_back(intensity);
// -- Debugging.
if(debug_) {
float scale = 10;
IplImage* intensity_big = cvCreateImage(cvSize(((float)intensity->width)*scale, ((float)intensity->height)*scale), intensity->depth, intensity->nChannels);
cvResize(intensity, intensity_big, CV_INTER_AREA);
IplImage* depth_big = cvCreateImage(cvSize(((float)depth->width)*scale, ((float)depth->height)*scale), depth->depth, depth->nChannels);
cvResize(depth, depth_big, CV_INTER_AREA);
CVSHOW("Intensity Image", intensity_big);
CVSHOW("Depth Image", depth_big);
cvWaitKey(0);
cvDestroyWindow("Intensity Image");
cvDestroyWindow("Depth Image");
}
// -- Clean up.
cvReleaseImage(&acc);
}
void constrain_marginals_bp (
MatrixXf & joint,
const VectorXf & prior_dom,
const VectorXf & prior_cod,
VectorXf & temp_dom,
VectorXf & temp_cod,
float tol,
size_t max_steps,
bool logging)
{
// Enforce simultaineous constraints on a joint PMF
//
// /\x. sum y. J(y,x) = p(x)
// /\y. sum x. J(y,x) = q(y)
ASSERT_EQ(prior_dom.size(), joint.cols());
ASSERT_EQ(prior_cod.size(), joint.rows());
ASSERT_LT(0, prior_dom.minCoeff());
ASSERT_LT(0, prior_cod.minCoeff());
if (logging) LOG(" constraining marginals via full BP");
const size_t X = joint.cols();
const size_t Y = joint.rows();
const Vector<float> p = as_vector(prior_dom);
const Vector<float> q = as_vector(prior_cod);
Vector<float> J = as_vector(joint);
Vector<float> sum_y_J = as_vector(temp_dom);
Vector<float> sum_x_J = as_vector(temp_cod);
float stepsize = 0;
size_t steps = 0;
while (steps < max_steps) {
++steps;
if (logging) cout << " step " << steps << "/" << max_steps << flush;
stepsize = 0;
// constrain sum y. J(y,x) = 1 first,
// in case joint is initalized with conditional
for (size_t x = 0; x < X; ++x) {
Vector<float> J_x = J.block(Y, x);
sum_y_J[x] = sum(J_x);
}
ASSERT_LT(0, min(sum_y_J)); // XXX error here
imax(stepsize, sqrtf(max_dist_squared(sum_y_J, p)));
idiv_store_rhs(p, sum_y_J);
for (size_t x = 0; x < X; ++x) {
Vector<float> J_x = J.block(Y, x);
J_x *= sum_y_J[x];
}
sum_x_J.zero();
for (size_t x = 0; x < X; ++x) {
Vector<float> J_x = J.block(Y, x);
sum_x_J += J_x;
}
ASSERT_LT(0, min(sum_x_J));
imax(stepsize, sqrtf(max_dist_squared(sum_x_J, q)));
idiv_store_rhs(q, sum_x_J);
for (size_t x = 0; x < X; ++x) {
Vector<float> J_x = J.block(Y, x);
J_x *= sum_x_J;
}
if (logging) LOG(", stepsize = " << stepsize);
if (stepsize < tol) break;
}
}
void CloudProjector::_compute() {
assert(orienter_);
assert(orienter_->getOutputCloud());
assert(!depth_projection_);
assert(!intensity_projection_);
MatrixXf& oriented = *orienter_->getOutputCloud();
VectorXf& intensities = *orienter_->getOutputIntensity();
// -- Get a copy of the projected points.
MatrixXf projected(oriented.rows(), 2);
int c=0;
for(int i=0; i<3; ++i) {
if(i == axis_of_projection_)
continue;
projected.col(c) = oriented.col(i);
++c;
}
// -- Transform into pixel units. projected is currently in meters, centered at 0.
//projected *= pixels_per_meter_;
for(int i=0; i<projected.rows(); ++i) {
projected(i, 0) *= pixels_per_meter_;
projected(i, 1) *= pixels_per_meter_;
}
// -- Find min and max of u and v. TODO: noise sensitivity?
// u is the col number in the image plane, v is the row number.
float min_v = FLT_MAX;
float min_u = FLT_MAX;
float max_v = -FLT_MAX;
float max_u = -FLT_MAX;
for(int i=0; i<projected.rows(); ++i) {
float u = projected(i, 0);
float v = projected(i, 1);
if(u < min_u)
min_u = u;
if(u > max_u)
max_u = u;
if(v < min_v)
min_v = v;
if(v > max_v)
max_v = v;
}
// -- Translate to coordinate system where (0,0) is the upper right of the image.
for(int i=0; i<projected.rows(); ++i) {
projected(i, 0) -= min_u;
projected(i, 1) = max_v - projected(i, 1);
}
// -- Get the max depth.
float max_depth = -FLT_MAX;
float min_depth = FLT_MAX;
for(int i=0; i<oriented.rows(); ++i) {
if(oriented(i, axis_of_projection_) > max_depth)
max_depth = oriented(i, axis_of_projection_);
if(oriented(i, axis_of_projection_) < min_depth)
min_depth = oriented(i, axis_of_projection_);
}
// -- Compute the normalized depths. Depths are between 0 and 1, with 1 meaning closest and 0 meaning furthest.
VectorXf depths = oriented.col(axis_of_projection_);
if(axis_of_projection_ == 1)
depths = -depths;
depths = (depths.array() - depths.minCoeff()).matrix();
depths = depths / depths.maxCoeff();
// -- Fill the IplImages.
assert(sizeof(float) == 4);
CvSize size = cvSize(ceil(max_u - min_u) + 1, ceil(max_v - min_v) + 1);
float pad_width = 0;
if(min_width_ > 0 && size.width < min_width_) {
pad_width = (float)(min_width_ - size.width) / 2.;
size.width = min_width_;
}
float pad_height = 0;
if(min_height_ > 0 && size.height < min_height_) {
pad_height = (float)(min_height_ - size.height) / 2.;
size.height = min_height_;
}
IplImage* acc = cvCreateImage(size, IPL_DEPTH_32F, 1);
intensity_projection_ = cvCreateImage(size, IPL_DEPTH_32F, 1);
depth_projection_ = cvCreateImage(size, IPL_DEPTH_32F, 1);
cvSetZero(acc);
cvSetZero(depth_projection_);
cvSetZero(intensity_projection_);
assert(intensities.rows() == projected.rows());
for(int i=0; i<projected.rows(); ++i) {
int row = floor(projected(i, 1) + pad_height);
int col = floor(projected(i, 0) + pad_width);
assert(row < size.height && row >= 0 && col < size.width && col >= 0);
// Update accumulator.
((float*)(acc->imageData + row * acc->widthStep))[col]++;
// Update intensity values.
float val = intensities(i) / 255.0 * (3.0 / 4.0) + 0.25;
assert(val <= 1.0 && val >= 0.0);
((float*)(intensity_projection_->imageData + row * intensity_projection_->widthStep))[col] += val;
// Update depth values.
((float*)(depth_projection_->imageData + row * depth_projection_->widthStep))[col] += depths(i); //
}
// -- Normalize by the number of points falling in each pixel.
for(int v=0; v<acc->height; ++v) {
float* intensity_ptr = (float*)(intensity_projection_->imageData + v * intensity_projection_->widthStep);
float* depth_ptr = (float*)(depth_projection_->imageData + v * depth_projection_->widthStep);
float* acc_ptr = (float*)(acc->imageData + v * acc->widthStep);
for(int u=0; u<acc->width; ++u) {
if(*acc_ptr == 0) {
*intensity_ptr = 0;
*depth_ptr = 0;
}
else {
*intensity_ptr = *intensity_ptr / *acc_ptr;
*depth_ptr = *depth_ptr / *acc_ptr;
}
intensity_ptr++;
depth_ptr++;
acc_ptr++;
}
}
// -- Blur the images. TODO: depth too?
cvSmooth(intensity_projection_, intensity_projection_, CV_GAUSSIAN, smoothing_, smoothing_);
// -- Clean up.
cvReleaseImage(&acc);
}
