VectorXf project1D( const RMatrixXf & Y, int * rep_label=NULL ) {
// const int MAX_SAMPLE = 20000;
const bool fast = true, very_fast = true;
// Remove the DC (Y : N x M)
RMatrixXf dY = Y.rowwise() - Y.colwise().mean();
// RMatrixXf sY = dY;
// if( 0 < MAX_SAMPLE && MAX_SAMPLE < dY.rows() ) {
// VectorXi samples = randomChoose( dY.rows(), MAX_SAMPLE );
// std::sort( samples.data(), samples.data()+samples.size() );
// sY = RMatrixXf( samples.size(), dY.cols() );
// for( int i=0; i<samples.size(); i++ )
// sY.row(i) = dY.row( samples[i] );
// }
// ... and use (pc > 0)
VectorXf lbl = VectorXf::Zero( Y.rows() );
// Find the largest PC of (dY.T * dY) and project onto it
if( very_fast ) {
// Find the largest PC using poweriterations
VectorXf U = VectorXf::Random( dY.cols() );
U = U.array() / U.norm()+std::numeric_limits<float>::min();
for( int it=0; it<20; it++ ){
// Normalize
VectorXf s = dY.transpose()*(dY*U);
s.array() /= s.norm()+std::numeric_limits<float>::min();
if ( (U-s).norm() < 1e-6 )
break;
U = s;
}
// Project onto the PC
lbl = dY*U;
}
else if(fast) {
// Compute the eigen values of the covariance (and project onto the largest eigenvector)
MatrixXf cov = dY.transpose()*dY;
SelfAdjointEigenSolver<MatrixXf> eigensolver(0.5*(cov+cov.transpose()));
MatrixXf ev = eigensolver.eigenvectors();
lbl = dY * ev.col( ev.cols()-1 );
}
else {
// Use the SVD
JacobiSVD<RMatrixXf> svd = dY.jacobiSvd(ComputeThinU | ComputeThinV );
// Project onto the largest PC
lbl = svd.matrixU().col(0) * svd.singularValues()[0];
}
// Find the representative label
if( rep_label )
dY.array().square().rowwise().sum().minCoeff( rep_label );
return (lbl.array() < 0).cast<float>();
}
void PlaneFittingCloudOrienter::_compute() {
assert(input_cloud_);
assert(input_intensity_);
assert(!output_cloud_);
// -- Fit a plane.
VectorXf a = fitPlane(*input_cloud_);
// -- Rotate the points so that the direction of the best plane is the x axis.
assert(fabs(a.norm() - 1) < 1e-4);
double theta = M_PI/2. - atan2(a(1), a(0));
MatrixXf rotation = MatrixXf::Identity(3, 3);
rotation(0,0) = cos(theta);
rotation(1,1) = cos(theta);
rotation(0,1) = sin(theta);
rotation(1,0) = -sin(theta);
output_cloud_ = shared_ptr<MatrixXf>(new MatrixXf());
*output_cloud_ = *input_cloud_ * rotation;
VectorXf foo = fitPlane(*output_cloud_);
//cout << "New plane: " << foo.transpose() << endl;
// -- Subtract off the mean of the points.
MatrixXf& points = *output_cloud_;
VectorXf pt_mean = points.colwise().sum() / (float)points.rows();
for(int i=0; i<points.rows(); ++i)
points.row(i) -= pt_mean.transpose();
}
IplImage* CloudProjection::computeProjection(const sensor_msgs::PointCloud& data,
const std::vector<int>& interest_region_indices)
{
// -- Put cluster points into matrix form.
MatrixXf points(interest_region_indices.size(), 3);
for(size_t i=0; i<interest_region_indices.size(); ++i) {
points(i, 0) = data.points[interest_region_indices[i]].x;
points(i, 1) = data.points[interest_region_indices[i]].y;
points(i, 2) = data.points[interest_region_indices[i]].z;
}
// -- Subtract off the mean and flatten to z=0 to prepare for PCA.
MatrixXf X = points;
X.col(2) = VectorXf::Zero(X.rows());
VectorXf pt_mean = X.colwise().sum() / (float)X.rows();
for(int i=0; i<X.rows(); ++i) {
X.row(i) -= pt_mean.transpose();
}
MatrixXf Xt = X.transpose();
// -- Find the long axis.
// Start with a random vector.
VectorXf pc = VectorXf::Zero(3);
pc(0) = 1; //Chosen by fair dice roll.
pc(1) = 1;
pc.normalize();
// Power method.
VectorXf prev = pc;
double thresh = 1e-4;
int ctr = 0;
while(true) {
prev = pc;
pc = Xt * (X * pc);
pc.normalize();
ctr++;
if((pc - prev).norm() < thresh)
break;
}
assert(abs(pc(2)) < 1e-4);
// -- Find the short axis.
VectorXf shrt = VectorXf::Zero(3);
shrt(1) = -pc(0);
shrt(0) = pc(1);
assert(abs(shrt.norm() - 1) < 1e-4);
assert(abs(shrt.dot(pc)) < 1e-4);
// -- Build the basis of normalized coordinates.
MatrixXf basis = MatrixXf::Zero(3,3);
basis.col(0) = pc;
basis.col(1) = shrt;
basis(2,2) = -1.0;
assert(abs(basis.col(0).dot(basis.col(1))) < 1e-4);
assert(abs(basis.col(0).norm() - 1) < 1e-4);
assert(abs(basis.col(1).norm() - 1) < 1e-4);
assert(abs(basis.col(2).norm() - 1) < 1e-4);
// -- Put the cluster into normalized coordinates, and choose which axis to project on.
MatrixXf projected_basis(3, 2);
if(axis_ == 0) {
projected_basis.col(0) = basis.col(1);
projected_basis.col(1) = basis.col(2);
}
else if(axis_ == 1) {
projected_basis.col(0) = basis.col(0);
projected_basis.col(1) = basis.col(2);
}
else if(axis_ == 2) {
projected_basis.col(0) = basis.col(0);
projected_basis.col(1) = basis.col(1);
}
MatrixXf projected = points * projected_basis;
// -- Transform into pixel units.
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?
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;
}
// -- Shift the origin based on {u,v}_offset_pct.
// u_offset_pct_ is the percent of the way from min_u to max_u that the
// u_offset should be set to. If this makes the window fall outside min_u or max_u,
// then shift the window so that it is inside.
float u_offset = u_offset_pct_ * (max_u - min_u) + min_u;
float v_offset = v_offset_pct_ * (max_v - min_v) + min_v;
if(u_offset_pct_ > 0.5 && u_offset + cols_ / 2 > max_u)
u_offset = max_u - cols_ / 2 + 1;
if(u_offset_pct_ < 0.5 && u_offset - cols_ / 2 < min_u)
u_offset = min_u + cols_ / 2 - 1;
if(v_offset_pct_ > 0.5 && v_offset + rows_ / 2 > max_v)
v_offset = max_v - rows_ / 2 + 1;
if(v_offset_pct_ < 0.5 && v_offset - rows_ / 2 < min_v)
v_offset = min_v + rows_ / 2 - 1;
for(int i=0; i<projected.rows(); ++i) {
projected(i, 0) -= u_offset - (float)cols_ / 2.0;
projected(i, 1) -= v_offset - (float)rows_ / 2.0;
}
// -- Fill the IplImages.
assert(sizeof(float) == 4);
IplImage* acc = cvCreateImage(cvSize(cols_, rows_), IPL_DEPTH_32F, 1);
IplImage* img = cvCreateImage(cvSize(cols_, rows_), IPL_DEPTH_32F, 1);
cvSetZero(acc);
cvSetZero(img);
for(int i=0; i<projected.rows(); ++i) {
int row = floor(projected(i, 1));
int col = floor(projected(i, 0));
if(row >= rows_ || col >= cols_ || row < 0 || col < 0)
continue;
float intensity = (float)data.channels[0].values[interest_region_indices[i]] / 255.0 * (3.0 / 4.0) + 0.25;
//cout << i << ": " << interest_region_indices[i] << "/" << data.channels[0].values.size() << " " << (float)data.channels[0].values[interest_region_indices[i]] << " " << intensity << endl;
assert(interest_region_indices[i] < (int)data.channels[0].values.size() && (int)interest_region_indices[i] >= 0);
assert(intensity <= 1.0 && intensity >= 0.0);
((float*)(img->imageData + row * img->widthStep))[col] += intensity;
((float*)(acc->imageData + row * acc->widthStep))[col]++;
}
// -- Normalize by the number of points falling in each pixel.
for(int v=0; v<rows_; ++v) {
float* img_ptr = (float*)(img->imageData + v * img->widthStep);
float* acc_ptr = (float*)(acc->imageData + v * acc->widthStep);
for(int u=0; u<cols_; ++u) {
if(*acc_ptr == 0)
*img_ptr = 0;
else
*img_ptr = *img_ptr / *acc_ptr;
img_ptr++;
acc_ptr++;
}
}
// -- Clean up and return.
cvReleaseImage(&acc);
return img;
}
MatrixXf D3DCloudOrienter::orientCloud(const sensor_msgs::PointCloud& data,
const std::vector<int>& interest_region_indices)
{
// -- Put cluster points into matrix form.
MatrixXf points(interest_region_indices.size(), 3);
for(size_t i=0; i<interest_region_indices.size(); ++i) {
points(i, 0) = data.points[interest_region_indices[i]].x;
points(i, 1) = data.points[interest_region_indices[i]].y;
points(i, 2) = data.points[interest_region_indices[i]].z;
}
// -- Subtract off the mean of the points.
VectorXf pt_mean = points.colwise().sum() / (float)points.rows();
for(int i=0; i<points.rows(); ++i)
points.row(i) -= pt_mean.transpose();
// -- Flatten to z == 0.
MatrixXf X = points;
X.col(2) = VectorXf::Zero(X.rows());
MatrixXf Xt = X.transpose();
// -- Find the long axis.
// Start with a random vector.
VectorXf pc = VectorXf::Zero(3);
pc(0) = 1; //Chosen by fair dice roll.
pc(1) = 1;
pc.normalize();
// Power method.
VectorXf prev = pc;
double thresh = 1e-4;
int ctr = 0;
while(true) {
prev = pc;
pc = Xt * (X * pc);
pc.normalize();
ctr++;
if((pc - prev).norm() < thresh)
break;
}
assert(abs(pc(2)) < 1e-4);
// -- Find the short axis.
VectorXf shrt = VectorXf::Zero(3);
shrt(1) = -pc(0);
shrt(0) = pc(1);
assert(abs(shrt.norm() - 1) < 1e-4);
assert(abs(shrt.dot(pc)) < 1e-4);
// -- Build the basis of normalized coordinates.
MatrixXf basis = MatrixXf::Zero(3,3);
basis.col(0) = pc;
basis.col(1) = shrt;
basis(2,2) = 1.0;
assert(abs(basis.col(0).dot(basis.col(1))) < 1e-4);
assert(abs(basis.col(0).norm() - 1) < 1e-4);
assert(abs(basis.col(1).norm() - 1) < 1e-4);
assert(abs(basis.col(2).norm() - 1) < 1e-4);
// -- Rotate and return.
MatrixXf oriented = points * basis;
return oriented;
}
void HoughCloudOrienter::_compute() {
assert(input_cloud_);
assert(input_intensities_);
assert(!output_cloud_);
//cout << input_intensities_->rows() << " " << input_cloud_->rows() << endl;
assert(input_cloud_->rows() == input_intensities_->rows());
assert(input_cloud_->rows() > 2);
// -- Subtract off the mean of the points.
MatrixXf& points = *input_cloud_;
VectorXf pt_mean = points.colwise().sum() / (float)points.rows();
for(int i=0; i<points.rows(); ++i)
points.row(i) -= pt_mean.transpose();
// -- Find the principal axis.
static const int num_bins = 12;
static const int max_samples = 100;
static unsigned int count[num_bins];
static double angle_total[num_bins];
for (int i=0; i < num_bins; i++) {
count[i] = 0;
angle_total[i] = 0;
}
int num_points = points.rows();
int num_samples = 0;
unsigned int max_count = 0;
int max_index = 0;
while (num_samples < max_samples) {
int ix = rand() % num_points;
int iy = rand() % num_points;
while (ix == iy)
iy = rand() % num_points;
VectorXf p1 = points.row(ix);
VectorXf p2 = points.row(iy);
double dy = (p1(1) - p2(1));
double dx = (p1(0) - p2(0));
if ((fabs(dy) < 0.001) && ( fabs(dx) < 0.001))
continue;
double y = atan2(dy, dx);
// wrap into range
if (y < 0) y += M_PI;
if (y >= M_PI_2) y -= M_PI_2;
int idx = (num_bins * y / M_PI_2);
if (idx >= num_bins) {
idx = 0;
y = 0.0;
}
angle_total[idx] += y;
count[idx]++;
if (count[idx] > max_count) {
max_count = count[idx];
max_index = idx;
}
num_samples++;
}
double angle = angle_total[max_index] / max_count;
VectorXf pc = VectorXf::Zero(3);
pc(0) = sin(angle);
pc(1) = cos(angle);
pc(2) = 0.0;
assert(abs(pc(2)) < 1e-4);
// -- Find the short axis.
VectorXf shrt = VectorXf::Zero(3);
shrt(1) = -pc(0);
shrt(0) = pc(1);
assert(abs(shrt.norm() - 1) < 1e-4);
assert(abs(shrt.dot(pc)) < 1e-4);
// -- Build the basis of normalized coordinates.
MatrixXf basis = MatrixXf::Zero(3,3);
basis.col(0) = pc;
basis.col(1) = shrt;
basis(2,2) = 1.0;
assert(abs(basis.col(0).dot(basis.col(1))) < 1e-4);
assert(abs(basis.col(0).norm() - 1) < 1e-4);
assert(abs(basis.col(1).norm() - 1) < 1e-4);
assert(abs(basis.col(2).norm() - 1) < 1e-4);
// -- Rotate and set the output_cloud_.
output_cloud_ = shared_ptr<MatrixXf>(new MatrixXf);
*output_cloud_ = points * basis;
assert(output_cloud_->rows() == input_cloud_->rows());
}
void CloudOrienter::_compute() {
assert(input_cloud_);
assert(input_intensities_);
assert(!output_cloud_);
//cout << input_intensities_->rows() << " " << input_cloud_->rows() << endl;
assert(input_cloud_->rows() == input_intensities_->rows());
assert(input_cloud_->rows() > 2);
// -- Subtract off the mean of the points.
MatrixXf& points = *input_cloud_;
VectorXf pt_mean = points.colwise().sum() / (float)points.rows();
for(int i=0; i<points.rows(); ++i)
points.row(i) -= pt_mean.transpose();
// -- Flatten to z == 0.
MatrixXf X = points;
X.col(2) = VectorXf::Zero(X.rows());
MatrixXf Xt = X.transpose();
// -- Find the long axis.
// Start with a random vector.
VectorXf pc = VectorXf::Zero(3);
pc(0) = 1; //Chosen by fair dice roll.
pc(1) = 1;
pc.normalize();
// Power method.
VectorXf prev = pc;
double thresh = 1e-4;
int ctr = 0;
while(true) {
prev = pc;
pc = Xt * (X * pc);
pc.normalize();
ctr++;
if((pc - prev).norm() < thresh)
break;
// -- In some degenerate cases, it is possible for the vector
// to never settle down to the first PC.
if(ctr > 100)
break;
}
assert(abs(pc(2)) < 1e-4);
// -- Find the short axis.
VectorXf shrt = VectorXf::Zero(3);
shrt(1) = -pc(0);
shrt(0) = pc(1);
assert(abs(shrt.norm() - 1) < 1e-4);
assert(abs(shrt.dot(pc)) < 1e-4);
// -- Build the basis of normalized coordinates.
MatrixXf basis = MatrixXf::Zero(3,3);
basis.col(0) = pc;
basis.col(1) = shrt;
basis(2,2) = 1.0;
assert(abs(basis.col(0).dot(basis.col(1))) < 1e-4);
assert(abs(basis.col(0).norm() - 1) < 1e-4);
assert(abs(basis.col(1).norm() - 1) < 1e-4);
assert(abs(basis.col(2).norm() - 1) < 1e-4);
// -- Rotate and set the output_cloud_.
output_cloud_ = shared_ptr<MatrixXf>(new MatrixXf);
*output_cloud_ = points * basis;
assert(output_cloud_->rows() == input_cloud_->rows());
}
