void CCIPCA::update(const float *pp) { const int D = m_mean.cols(); const int K = m_eigenvecs.cols(); m_num_data_points++; //< TODO amnesic weights const float w1 = (m_num_data_points-1)/(float)m_num_data_points; const float w2 = (1)/(float)m_num_data_points; VectorXf vec_pp = Map<const VectorXf>(pp, D) - m_mean.transpose(); for (int k = 0; k < K; k++) { VectorXf v = m_eigenvecs.col(k); m_eigenvecs.col(k) = w1 * v + (v.dot(vec_pp))*vec_pp*(w2/m_eigenvecs_norms(k)); m_eigenvecs_norms(k) = m_eigenvecs.col(k).norm(); VectorXf nrm_v = m_eigenvecs.col(k).normalized(); vec_pp -= (vec_pp.dot(nrm_v))*nrm_v; } }
float Sphere::fit_eval ( const VectorXf &fitpar, const void *user_data) { /* * Calculate the cost function value * Optimize for the radius inside here */ const fitUserNew& user = (fitUserNew)user_data; const VectorXf& r0 = fitpar; float F; MatrixXf diff = user->rr.rowwise() - r0.transpose(); VectorXf one = diff.rowwise().norm(); float sum = one.sum(); float sum2 = one.dot(one); F = sum2 - sum*sum/user->rr.rows(); if(user->report) std::cout << "r0: " << 1000*r0[0] << ", r1: " << 1000*r0[1] << ", r2: " << 1000*r0[2] << "; R: " << 1000*sum/user->rr.rows() << "; fval: "<<F<<std::endl; return F; }
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()); }