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()); }