quadric.cpp
Go to the documentation of this file.
00001 #include <agile_grasp/quadric.h>
00002 
00003 Quadric::Quadric(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> >& T_cams,
00004   const PointCloud::Ptr& input, const Eigen::Vector3d& sample, bool is_deterministic) :
00005                 input_(input), sample_(sample), is_deterministic_(is_deterministic)
00006 {
00007         cam_origins_.resize(3, T_cams.size());
00008         for (int i = 0; i < cam_origins_.cols(); i++)
00009         {
00010                 cam_origins_.col(i) = T_cams.at(i).block < 3, 1 > (0, 3);
00011         }
00012 }
00013 
00014 void Quadric::fitQuadric(const std::vector<int> &indices)
00015 {
00016         int n = indices.size();
00017 
00018         // calculate matrices M and N
00019         Eigen::Matrix<double, TAUBIN_MATRICES_SIZE, TAUBIN_MATRICES_SIZE> M;
00020         Eigen::Matrix<double, TAUBIN_MATRICES_SIZE, TAUBIN_MATRICES_SIZE> N;
00021         M.setZero(10, 10);
00022         N.setZero(10, 10);
00023 
00024         for (int i = 0; i < n; i++)
00025         {
00026                 if (isnan(input_->points[indices[i]].x))
00027                         continue;
00028 
00029                 double x = input_->points[indices[i]].x;
00030                 double y = input_->points[indices[i]].y;
00031                 double z = input_->points[indices[i]].z;
00032                 double x2 = x * x;
00033                 double y2 = y * y;
00034                 double z2 = z * z;
00035                 double xy = x * y;
00036                 double yz = y * z;
00037                 double xz = x * z;
00038 
00039                 // required calculations for M
00040                 M(0, 0) += x2 * x2;
00041                 M(0, 1) += x2 * y2;
00042                 M(0, 2) += x2 * z2;
00043                 M(0, 3) += x2 * xy;
00044                 M(0, 4) += x2 * yz;
00045                 M(0, 5) += x2 * xz;
00046                 M(0, 6) += x2 * x;
00047                 M(0, 7) += x2 * y;
00048                 M(0, 8) += x2 * z;
00049                 M(0, 9) += x2;
00050                 M(1, 1) += y2 * y2;
00051                 M(1, 2) += y2 * z2;
00052                 M(1, 3) += y2 * xy;
00053                 M(1, 4) += y2 * yz;
00054                 M(1, 5) += y2 * xz;
00055                 M(1, 6) += y2 * x;
00056                 M(1, 7) += y2 * y;
00057                 M(1, 8) += y2 * z;
00058                 M(1, 9) += y2;
00059                 M(2, 2) += z2 * z2;
00060                 M(2, 3) += z2 * xy;
00061                 M(2, 4) += z2 * yz;
00062                 M(2, 5) += z2 * xz;
00063                 M(2, 6) += z2 * x;
00064                 M(2, 7) += z2 * y;
00065                 M(2, 8) += z2 * z;
00066                 M(2, 9) += z2;
00067                 M(3, 8) += x * yz;
00068                 M(3, 9) += xy;
00069                 M(4, 9) += yz;
00070                 M(5, 9) += xz;
00071                 M(6, 9) += x;
00072                 M(7, 9) += y;
00073                 M(8, 9) += z;
00074 
00075                 // repeating elements in M
00076                 M(3, 3) = M(0, 1);
00077                 M(5, 5) = M(0, 2);
00078                 M(3, 5) = M(0, 4);
00079                 M(3, 6) = M(0, 7);
00080                 M(5, 6) = M(0, 8);
00081                 M(6, 6) = M(0, 9);
00082 
00083                 M(4, 4) = M(1, 2);
00084                 M(3, 4) = M(1, 5);
00085                 M(3, 7) = M(1, 6);
00086                 M(4, 7) = M(1, 8);
00087                 M(7, 7) = M(1, 9);
00088 
00089                 M(4, 5) = M(2, 3);
00090                 M(5, 8) = M(2, 6);
00091                 M(4, 8) = M(2, 7);
00092                 M(8, 8) = M(2, 9);
00093 
00094                 M(4, 6) = M(3, 8);
00095                 M(5, 7) = M(3, 8);
00096                 M(6, 7) = M(3, 9);
00097 
00098                 M(7, 8) = M(4, 9);
00099 
00100                 M(6, 8) = M(5, 9);
00101 
00102                 // required calculations for N
00103                 N(0, 0) += 4.0 * x2;
00104                 N(0, 3) += 2.0 * xy;
00105                 N(0, 5) += 2.0 * xz;
00106                 N(0, 6) += 2.0 * x;
00107 
00108                 N(1, 1) += 4.0 * y2;
00109                 N(1, 3) += 2.0 * xy;
00110                 N(1, 4) += 2.0 * yz;
00111                 N(1, 7) += 2.0 * y;
00112 
00113                 N(2, 2) += 4.0 * z2;
00114                 N(2, 4) += 2.0 * yz;
00115                 N(2, 5) += 2.0 * xz;
00116                 N(2, 8) += 2.0 * z;
00117 
00118                 N(3, 3) += x2 + y2;
00119                 N(3, 4) += xz;
00120                 N(3, 5) += yz;
00121                 N(3, 6) += y;
00122                 N(3, 7) += x;
00123 
00124                 N(4, 4) += y2 + z2;
00125                 N(4, 5) += xy;
00126                 N(4, 7) += z;
00127                 N(4, 8) += y;
00128 
00129                 N(5, 5) += x2 + z2;
00130                 N(5, 6) += z;
00131                 N(5, 8) += x;
00132         }
00133 
00134         M(9, 9) = n;
00135         // reflect upper triangular part in lower triangular part
00136         M.triangularView<Eigen::StrictlyLower>() = M.triangularView<Eigen::StrictlyUpper>().transpose();
00137         N(6, 6) = n;
00138         N(7, 7) = n;
00139         N(8, 8) = n;
00140         // reflect upper triangular part in lower triangular part
00141         N.triangularView<Eigen::StrictlyLower>() = N.triangularView<Eigen::StrictlyUpper>().transpose();
00142         //~ std::cout<<"M:\n"<<M<<std::endl;
00143         //~ std::cout<<"N:\n"<<N<<std::endl;
00144 
00145         // solve generalized Eigen problem to find quadric parameters
00146         Eigen::MatrixXd eigen_vectors;
00147         Eigen::MatrixXd lambda;
00148         solveGeneralizedEigenProblem(M, N, eigen_vectors, lambda);
00149         Eigen::VectorXd eigen_values = lambda.col(0).cwiseQuotient(lambda.col(2));
00150         int min_index;
00151         eigen_values.segment(0, 9).minCoeff(&min_index);
00152         parameters_ = eigen_vectors.col(min_index);
00153         parameters_.segment(3, 3) *= 0.5;
00154 
00155         // compute centroid and covariance matrix of quadric
00156         unpackQuadric();
00157 }
00158 
00159 void Quadric::findTaubinNormalAxis(const std::vector<int> &indices, const Eigen::VectorXi& cam_source)
00160 {
00161         // quadric parameters in implicit form
00162         double a = parameters_(0);
00163         double b = parameters_(1);
00164         double c = parameters_(2);
00165         double d = 2.0 * parameters_(3);
00166         double e = 2.0 * parameters_(4);
00167         double f = 2.0 * parameters_(5);
00168         double g = parameters_(6);
00169         double h = parameters_(7);
00170         double i = parameters_(8);
00171 
00172         // sample <num_samples> points near surface
00173         Eigen::Matrix3Xd samples;
00174         Eigen::VectorXi samples_cam_source;
00175         if (!is_deterministic_)
00176         {
00177                 int num_samples = 50;
00178                 samples.resize(3, num_samples);
00179                 samples_cam_source.resize(num_samples);
00180                 if (indices.size() > num_samples)
00181                 {
00182                         for (int t = 0; t < num_samples; t++)
00183                         {
00184                                 int r = rand() % indices.size();
00185                                 while (isnan(input_->points[indices[r]].x))
00186                                 {
00187                                         r = rand() % indices.size();
00188                                 }
00189 
00190                                 samples.col(t) << input_->points[indices[r]].x, input_->points[indices[r]].y, input_->points[indices[r]].z;
00191                                 samples_cam_source(t) = cam_source(r);
00192                         }
00193                 }
00194                 else
00195                 {
00196                         samples_cam_source = cam_source;
00197                         samples.resize(3, indices.size());
00198                         for (int t = 0; t < indices.size(); t++)
00199                         {
00200                                 samples.col(t) << input_->points[indices[t]].x, input_->points[indices[t]].y, input_->points[indices[t]].z;
00201                         }
00202                 }
00203         }
00204         else
00205         {
00206                 samples_cam_source = cam_source;
00207                 samples.resize(3, indices.size());
00208                 for (int t = 0; t < indices.size(); t++)
00209                 {
00210                         samples.col(t) << input_->points[indices[t]].x, input_->points[indices[t]].y, input_->points[indices[t]].z;
00211                 }
00212         }
00213 
00214 //      std::cout << "Took subset of points in neighborhood" << std::endl;
00215 
00216         // calculate camera source for majority of points
00217         Eigen::VectorXd num_source(this->cam_origins_.cols());
00218         num_source << 0, 0;
00219         for (int cami = 0; cami < samples_cam_source.size(); cami++)
00220         {
00221 //    std::cout << "cami: " << cami << ", samples_cam_source(cami): " << samples_cam_source(cami) << std::endl;
00222                 if (samples_cam_source(cami) == 0)
00223                         num_source(0)++;else
00224                 if (samples_cam_source(cami) == 1)
00225                         num_source(1)++;}
00226 num_source      .maxCoeff(&majority_cam_source_);
00227 //  std::cout << "samples_cam_source.size(): " << samples_cam_source.size() << ", num_source: " << num_source.transpose() << ", majority_cam_source_: " << majority_cam_source_ << "\n";
00228 
00229         //~ std::cout<<"\n";
00230         //~ std::cout<<"samples_near_surf:\n"<<samples_near_surf<<std::endl;
00231         //~ std::cout<<"2.0*a*samples_near_surf.row(0):\n"<<2.0*a*samples_near_surf.row(0)<<"\n";
00232         //~ std::cout<<"2.0*a*samples_near_surf.row(0) + d*samples_near_surf.row(1) + f*samples_near_surf.row(2):\n"<<2.0*a*samples_near_surf.row(0) + d*samples_near_surf.row(1) + f*samples_near_surf.row(2)<<"\n";
00233         //~ std::cout<<"2.0*a*samples_near_surf.row(0) + d*samples_near_surf.row(1) + f*samples_near_surf.row(2).array():\n"<<(2.0*a*samples_near_surf.row(0) + d*samples_near_surf.row(1) + f*samples_near_surf.row(2)).array()<<"\n";
00234         //~ std::cout<<"2.0*a*samples_near_surf.row(0) + d*samples_near_surf.row(1) + f*samples_near_surf.row(2) + g:\n"<<(2.0*a*samples_near_surf.row(0) + d*samples_near_surf.row(1) + f*samples_near_surf.row(2)).array() + g<<"\n";
00235         //~ std::cout<<"g: "<<g<<std::endl;
00236 
00237         // calculate normals at each of these points
00238         Eigen::MatrixXd fx = (2.0 * a * samples.row(0) + d * samples.row(1) + f * samples.row(2)).array() + g;
00239         //~ std::cout<<"fx:\n"<<fx<<std::endl;
00240         Eigen::MatrixXd fy = (2.0 * b * samples.row(1) + d * samples.row(0) + e * samples.row(2)).array() + h;
00241         //~ std::cout<<"fy:\n"<<fy<<std::endl;
00242         Eigen::MatrixXd fz = (2.0 * c * samples.row(2) + e * samples.row(1) + f * samples.row(0)).array() + i;
00243         //~ std::cout<<"fz:\n"<<fz<<std::endl;
00244         Eigen::MatrixXd normals(3, samples.cols());
00245         normals << fx, fy, fz;
00246         Eigen::MatrixXd gradient_magnitude = ((normals.cwiseProduct(normals)).colwise().sum()).cwiseSqrt();
00247         normals = normals.cwiseQuotient(gradient_magnitude.replicate(3, 1));
00248 //  std::cout << "normals:\n" << normals.cols() << std::endl;
00249 
00250         findAverageNormalAxis(normals);
00251 }
00252 
00253 void Quadric::print()
00254 {
00255         std::cout << "sample: " << sample_.transpose() << std::endl;
00256         std::cout << "parameters: " << parameters_.transpose() << std::endl;
00257         std::cout << "normals_ratio: " << normals_ratio_ << std::endl;
00258         std::cout << "normals_axis: " << curvature_axis_.transpose() << std::endl;
00259         std::cout << "normals_average: " << normal_.transpose() << std::endl;
00260         std::cout << "binormal: " << binormal_.transpose() << std::endl;
00261 }
00262 
00263 void Quadric::findAverageNormalAxis(const Eigen::MatrixXd &normals)
00264 {
00265         // calculate curvature axis
00266         Eigen::Matrix3d M = normals * normals.transpose();
00267         //~ std::cout<<"M:\n"<<M<<std::endl;
00268         Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver(M);
00269         Eigen::Vector3d eigen_values = eigen_solver.eigenvalues().real();
00270         Eigen::Matrix3d eigen_vectors = eigen_solver.eigenvectors().real();
00271         //~ std::cout<<"eigen_values:\n"<<eigen_values<<std::endl;
00272         //~ std::cout<<"eigen_vectors:\n"<<eigen_vectors<<std::endl;
00273         Eigen::Vector3d sorted_eigen_values = eigen_values;
00274         std::sort(sorted_eigen_values.data(), sorted_eigen_values.data() + sorted_eigen_values.size());
00275         //~ std::cout<<"eigen_values:\n"<<eigen_values<<std::endl;
00276         //~ std::cout<<"sorted eigen_values:\n"<<sorted_eigen_values<<std::endl;
00277         normals_ratio_ = sorted_eigen_values(1) / sorted_eigen_values(2);
00278         int min_index;
00279         eigen_values.minCoeff(&min_index);
00280         curvature_axis_ = eigen_vectors.col(min_index);
00281 
00282         // calculate surface normal
00283         int max_index;
00284         (normals.transpose() * normals).array().pow(6).colwise().sum().maxCoeff(&max_index);
00285         Eigen::Vector3d normpartial = (Eigen::MatrixXd::Identity(3, 3)
00286                         - curvature_axis_ * curvature_axis_.transpose()) * normals.col(max_index);
00287         //~ std::cout<<"normpartial:\n"<<normpartial<<std::endl;
00288         normal_ = normpartial / normpartial.norm();
00289 
00290         // create binormal
00291         binormal_ = curvature_axis_.cross(normal_);
00292 
00293         // require normal and binormal to be oriented towards source
00294         Eigen::Vector3d source_to_sample = sample_ - cam_origins_.col(majority_cam_source_);
00295 //  std::cout << source_to_sample.transpose() << std::endl;
00296 //  std::cout << "majority_cam_source: " << majority_cam_source_ << std::endl;
00297 //  std::cout << cam_origins.col(majority_cam_source).transpose() << std::endl;
00298         if (normal_.dot(source_to_sample) > 0) // normal points away from source
00299                 normal_ *= -1.0;
00300         if (binormal_.dot(source_to_sample) > 0) // binormal points away from source
00301                 binormal_ *= -1.0;
00302 
00303         // adjust curvature axis to new frame
00304         curvature_axis_ = normal_.cross(binormal_);
00305 }
00306 
00307 void Quadric::unpackQuadric()
00308 {
00309         double a = parameters_(0);
00310         double b = parameters_(1);
00311         double c = parameters_(2);
00312         double d = parameters_(3);
00313         double e = parameters_(4);
00314         double f = parameters_(5);
00315         double g = parameters_(6);
00316         double h = parameters_(7);
00317         double i = parameters_(8);
00318         double j = parameters_(9);
00319 
00320         Eigen::Matrix3d parameter_matrix;
00321         parameter_matrix << a, d, f, d, b, e, f, e, c;
00322         Eigen::Vector3d ghi;
00323         ghi << g, h, i;
00324         Eigen::Matrix3d inverse_parameter_matrix = parameter_matrix.inverse();
00325         centroid_ = -0.5 * inverse_parameter_matrix * ghi;
00326         double k = j - 0.25 * ghi.transpose() * inverse_parameter_matrix * ghi;
00327         covariance_matrix_ = -1 * parameter_matrix / k;
00328 }
00329 
00330 bool Quadric::solveGeneralizedEigenProblem(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B,
00331                 Eigen::MatrixXd& v, Eigen::MatrixXd& lambda)
00332 {
00333         int N = A.cols(); // Number of columns of A and B. Number of rows of v.
00334         if (B.cols() != N || A.rows() != N || B.rows() != N)
00335                 return false;
00336 
00337         v.resize(N, N);
00338         lambda.resize(N, 3);
00339 
00340         int LDA = A.outerStride();
00341         int LDB = B.outerStride();
00342         int LDV = v.outerStride();
00343 
00344         double WORKDUMMY;
00345         int LWORK = -1; // Request optimum work size.
00346         int INFO = 0;
00347 
00348         double* alphar = const_cast<double*>(lambda.col(0).data());
00349         double* alphai = const_cast<double*>(lambda.col(1).data());
00350         double* beta = const_cast<double*>(lambda.col(2).data());
00351 
00352         // Get the optimum work size.
00353         dggev_("N", "V", &N, A.data(), &LDA, B.data(), &LDB, alphar, alphai, beta, 0, &LDV, v.data(), &LDV,
00354                         &WORKDUMMY, &LWORK, &INFO);
00355 
00356         LWORK = int(WORKDUMMY) + 32;
00357         Eigen::VectorXd WORK(LWORK);
00358 
00359         dggev_("N", "V", &N, A.data(), &LDA, B.data(), &LDB, alphar, alphai, beta, 0, &LDV, v.data(), &LDV,
00360                         WORK.data(), &LWORK, &INFO);
00361 
00362         return INFO == 0;
00363 }
00364 
00365 void Quadric::plotAxes(void* viewer_void, int id) const
00366 {
00367         boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = *static_cast<boost::shared_ptr<
00368                         pcl::visualization::PCLVisualizer> *>(viewer_void);
00369 
00370         pcl::PointXYZ p, q, r;
00371         p.x = sample_(0);
00372         p.y = sample_(1);
00373         p.z = sample_(2);
00374         q.x = p.x + 0.02 * curvature_axis_(0);
00375         q.y = p.y + 0.02 * curvature_axis_(1);
00376         q.z = p.z + 0.02 * curvature_axis_(2);
00377         r.x = p.x + 0.02 * normal_(0);
00378         r.y = p.y + 0.02 * normal_(1);
00379         r.z = p.z + 0.02 * normal_(2);
00380 //  std::cout << "p: " << p << std::endl;
00381 //  std::cout << "q: " << q << std::endl;
00382 //  std::cout << "r: " << r << std::endl;
00383         viewer->addLine<pcl::PointXYZ>(p, q, 0, 0, 255, "curvature_axis_" + boost::lexical_cast<std::string>(id));
00384         viewer->addLine<pcl::PointXYZ>(p, r, 255, 0, 0, "normal_axis_" + boost::lexical_cast<std::string>(id));
00385 }


agile_grasp
Author(s): Andreas ten Pas
autogenerated on Sat Jun 8 2019 20:08:27