void ARAPTerm::updateRotation() { Solver::DeformPetal& deform_petal = Solver::deform_petals_[petal_id_]; Solver::PetalMatrix& origin_petal = deform_petal._origin_petal; Solver::PetalMatrix& petal_matrix = deform_petal._petal_matrix; Solver::RotList& rot_list = deform_petal._R_list; Solver::ScaleList& scale_list = deform_petal._S_list; Solver::AdjList& adj_list = deform_petal._adj_list; Solver::WeightMatrix& weight_matrix = deform_petal._weight_matrix; Eigen::Matrix3f Si; Eigen::MatrixXd Di; Eigen::Matrix3Xd Pi_Prime; Eigen::Matrix3Xd Pi; for (size_t i = 0, i_end = rot_list.size(); i < i_end; ++i) { Di = Eigen::MatrixXd::Zero(adj_list[i].size(), adj_list[i].size()); Pi_Prime.resize(3, adj_list[i].size()); Pi.resize(3, adj_list[i].size()); for (size_t j = 0, j_end = adj_list[i].size(); j < j_end; ++j) { Di(j, j) = weight_matrix.coeffRef(i, adj_list[i][j]); Pi.col(j) = origin_petal.col(i) - origin_petal.col(adj_list[i][j]); Pi_Prime.col(j) = petal_matrix.col(i) - petal_matrix.col(adj_list[i][j]); } Si = Pi.cast<float>() * Di.cast<float>() * Pi_Prime.transpose().cast<float>(); Eigen::Matrix3f Ui; Eigen::Vector3f Wi; Eigen::Matrix3f Vi; wunderSVD3x3(Si, Ui, Wi, Vi); rot_list[i] = Vi.cast<double>() * Ui.transpose().cast<double>(); if (rot_list[i].determinant() < 0) std::cout << "determinant is negative!" << std::endl; double s = 0; for (size_t j = 0, j_end = adj_list[i].size(); j < j_end; ++j) { s += Di(j, j) * Pi.col(j).squaredNorm(); } // scale_list[i] = Wi.trace() / s; /* if (scale_list[i] < 0.95 ) scale_list[i] = 0.95; else if (scale_list[i] > 1.05) scale_list[i] = 1.05;*/ } /*if (petal_id_ == 0) std::cout << "vertex: " << 0 << " " << scale_list[0] << std::endl;*/ }
bool SkeletonCAD120Stream::getSkeleton(Eigen::Matrix3Xd& skeleton) { if (!reader_.readNextFrame()) { finished_ = true; return false; } skeleton.resize(Eigen::NoChange, joint_names_.size()); for (int i=0; i<joint_names_.size(); i++) { std::map<std::string, std::string>::const_iterator it = joint_name_conversion_map_.find(joint_names_[i]); if (it == joint_name_conversion_map_.end()) { // joint name could not be found in conversion map fprintf(stderr, "ERROR: %s is not found in CAD120 joint name conversion map\n", joint_names_[i].c_str()); return false; } const std::string cad120_joint_name = it->second; Eigen::Vector3d position; if (!reader_.getJointPosition(cad120_joint_name, position)) { // joint name is not defined in CAD120 fprintf(stderr, "ERROR: %s is not defined in CAD120\n", cad120_joint_name.c_str()); return false; } skeleton.col(i) = Eigen::Vector3d(position(0), position(2), position(1)) / 1000.; } return true; }
// The input 3D points are stored as columns. Eigen::Affine3d Find3DAffineTransform(Eigen::Matrix3Xd in, Eigen::Matrix3Xd out) { // Default output Eigen::Affine3d A; A.linear() = Eigen::Matrix3d::Identity(3, 3); A.translation() = Eigen::Vector3d::Zero(); if (in.cols() != out.cols()) throw "Find3DAffineTransform(): input data mis-match"; // First find the scale, by finding the ratio of sums of some distances, // then bring the datasets to the same scale. double dist_in = 0, dist_out = 0; for (int col = 0; col < in.cols()-1; col++) { dist_in += (in.col(col+1) - in.col(col)).norm(); dist_out += (out.col(col+1) - out.col(col)).norm(); } if (dist_in <= 0 || dist_out <= 0) return A; double scale = dist_out/dist_in; out /= scale; // Find the centroids then shift to the origin Eigen::Vector3d in_ctr = Eigen::Vector3d::Zero(); Eigen::Vector3d out_ctr = Eigen::Vector3d::Zero(); for (int col = 0; col < in.cols(); col++) { in_ctr += in.col(col); out_ctr += out.col(col); } in_ctr /= in.cols(); out_ctr /= out.cols(); for (int col = 0; col < in.cols(); col++) { in.col(col) -= in_ctr; out.col(col) -= out_ctr; } // SVD Eigen::MatrixXd Cov = in * out.transpose(); Eigen::JacobiSVD<Eigen::MatrixXd> svd(Cov, Eigen::ComputeThinU | Eigen::ComputeThinV); // Find the rotation double d = (svd.matrixV() * svd.matrixU().transpose()).determinant(); if (d > 0) d = 1.0; else d = -1.0; Eigen::Matrix3d I = Eigen::Matrix3d::Identity(3, 3); I(2, 2) = d; Eigen::Matrix3d R = svd.matrixV() * I * svd.matrixU().transpose(); // The final transform A.linear() = scale * R; A.translation() = scale*(out_ctr - R*in_ctr); return A; }
void transfer_spheres(const std::vector<Sphere> & spheres, Eigen::Matrix3Xd & sphereCenter, Eigen::VectorXd & sphereRadius) { size_t nSpheres = spheres.size(); sphereCenter.resize(Eigen::NoChange, nSpheres); sphereRadius.resize(nSpheres); for (size_t i = 0; i < nSpheres; ++i) { sphereCenter.col(i) = spheres[i].center; sphereRadius(i) = spheres[i].radius; } }
void PinholeCamera<DISTORTION_T>::projectBatch( const Eigen::Matrix3Xd & points, Eigen::Matrix2Xd * imagePoints, std::vector<CameraBase::ProjectionStatus> * stati) const { const int numPoints = points.cols(); for (int i = 0; i < numPoints; ++i) { Eigen::Vector3d point = points.col(i); Eigen::Vector2d imagePoint; CameraBase::ProjectionStatus status = project(point, &imagePoint); imagePoints->col(i) = imagePoint; if(stati) stati->push_back(status); } }
void GraspSet::calculateVoxelizedShadowVectorized(const Eigen::Matrix3Xd& points, const Eigen::Vector3d& shadow_vec, int num_shadow_points, double voxel_grid_size, Vector3iSet& shadow_set) const { double t0_set = omp_get_wtime(); const int n = points.cols() * num_shadow_points; const double voxel_grid_size_mult = 1.0 / voxel_grid_size; const double max = 1.0 / 32767.0; // Eigen::Vector3d w; for(int i = 0; i < n; i++) { const int pt_idx = i / num_shadow_points; // const Eigen::Vector3d w = (points.col(pt_idx) + ((double) fastrand() * max) * shadow_vec) * voxel_grid_size_mult; shadow_set.insert(((points.col(pt_idx) + ((double) fastrand() * max) * shadow_vec) * voxel_grid_size_mult).cast<int>()); } if (MEASURE_TIME) printf("Shadow (1 camera) calculation. Runtime: %.3f, #points: %d, num_shadow_points: %d, #shadow: %d, max #shadow: %d\n", omp_get_wtime() - t0_set, (int) points.cols(), num_shadow_points, (int) shadow_set.size(), n); // std::cout << "Calculated shadow for 1 camera. Runtime: " << omp_get_wtime() - t0_set << ", #points: " << n << "\n"; }
bool SkeletonFileStream::getSkeleton(Eigen::Matrix3Xd& skeleton) { if (fp_ == NULL) return false; skeleton.resize(Eigen::NoChange, joint_names_.size()); for (int i=0; i<file_num_joints_; i++) { Eigen::Vector3d v; if (fscanf(fp_, "%lf%lf%lf", &v(0), &v(1), &v(2)) != 3) { fclose(fp_); fp_ = NULL; return false; } if (file_joint_index_map_[i] != -1) skeleton.col( file_joint_index_map_[i] ) = v; } return true; }
bool SkeletonRealtimeStream::getSkeleton(Eigen::Matrix3Xd& skeleton) { const int id = getUserId(); if (id == -1) return false; const std::string id_string = std::to_string(id); skeleton.resize(Eigen::NoChange, joint_names_.size()); for (int i=0; i<joint_names_.size(); i++) { ros::Time time; std::string error_string; if (listener_.getLatestCommonTime("camera_depth_frame", joint_names_[i] + "_" + id_string, time, &error_string) != tf::NO_ERROR) { ROS_ERROR("Skeleton realtime stream error TF error: %s", error_string.c_str()); return false; } else { try { tf::StampedTransform transform; listener_.lookupTransform("camera_depth_frame", joint_names_[i] + "_" + id_string, ros::Time(0), transform); tf::Vector3 p = transform.getOrigin(); skeleton.col(i) = Eigen::Vector3d(p.x(), p.y(), p.z()); } catch (tf::TransformException ex) { ROS_ERROR("Skeleton realtime stream error: %s", ex.what()); return false; } } } return true; }
void Input::initMolecule() { // Gather information necessary to build molecule_ // 1. number of atomic centers int nuclei = int(geometry_.size() / 4); // 2. position and charges of atomic centers Eigen::Matrix3Xd centers = Eigen::Matrix3Xd::Zero(3, nuclei); Eigen::VectorXd charges = Eigen::VectorXd::Zero(nuclei); int j = 0; for (int i = 0; i < nuclei; ++i) { centers.col(i) = (Eigen::Vector3d() << geometry_[j], geometry_[j + 1], geometry_[j + 2]) .finished(); charges(i) = geometry_[j + 3]; j += 4; } // 3. list of atoms and list of spheres std::vector<Atom> radiiSet; std::vector<Atom> atoms; atoms.reserve(nuclei); // FIXME Code duplication in function initMolecule in interface/Meddle.cpp tie(radiiSetName_, radiiSet) = utils::bootstrapRadiiSet().create(radiiSet_); for (int i = 0; i < charges.size(); ++i) { int index = int(charges(i)) - 1; atoms.push_back(radiiSet[index]); if (scaling_) atoms[i].radiusScaling = 1.2; } // Based on the creation mode (Implicit or Atoms) // the spheres list might need postprocessing if (mode_ == "IMPLICIT" || mode_ == "ATOMS") { for (int i = 0; i < charges.size(); ++i) { // Convert to Bohr and multiply by scaling factor (alpha) double radius = atoms[i].radius * angstromToBohr() * atoms[i].radiusScaling; spheres_.push_back(Sphere(centers.col(i), radius)); } if (mode_ == "ATOMS") { // Loop over the atomsInput array to get which atoms will have a user-given // radius for (size_t i = 0; i < atoms_.size(); ++i) { int index = atoms_[i] - 1; // -1 to go from human readable to machine readable // Put the new Sphere in place of the implicit-generated one spheres_[index] = Sphere(centers.col(index), radii_[i]); } } } // 4. masses Eigen::VectorXd masses = Eigen::VectorXd::Zero(nuclei); for (int i = 0; i < masses.size(); ++i) { masses(i) = atoms[i].mass; } // 5. molecular point group // FIXME currently hardcoded to C1 // OK, now get molecule_ molecule_ = Molecule(nuclei, charges, masses, centers, atoms, spheres_); // Check that all atoms have a radius attached std::vector<Atom>::const_iterator res = std::find_if(atoms.begin(), atoms.end(), invalid); if (res != atoms.end()) { std::cout << molecule_ << std::endl; PCMSOLVER_ERROR("Some atoms do not have a radius attached. Please specify a " "radius for all atoms (see " "http://pcmsolver.readthedocs.org/en/latest/users/input.html)!"); } }
double integrateD(const KernelD & F, const Element & e) { double result = 0.0; // Get the quadrature rules for azimuthal and polar integrations namespace mpl = boost::mpl; typedef typename mpl::at<rules_map, mpl::int_<PhiPoints> >::type PhiPolicy; typedef typename mpl::at<rules_map, mpl::int_<ThetaPoints> >::type ThetaPolicy; QuadratureRule<PhiPolicy> phiRule; QuadratureRule<ThetaPolicy> thetaRule; int upper_phi = PhiPoints / 2; // Upper limit for loop on phi points int upper_theta = ThetaPoints / 2; // Upper limit for loop on theta points // Extract relevant data from Element int nVertices = e.nVertices(); Eigen::Vector3d normal = e.normal(); Sphere sph = e.sphere(); Eigen::Matrix3Xd vertices = e.vertices(); Eigen::Matrix3Xd arcs = e.arcs(); // Calculation of the tangent and the bitangent (binormal) vectors // Tangent, Bitangent and Normal form a local reference frame: // T <-> x; B <-> y; N <-> z Eigen::Vector3d tangent, bitangent; tangent_and_bitangent(normal, tangent, bitangent); std::vector<double> theta(nVertices), phi(nVertices), phinumb(nVertices+1); std::vector<int> numb(nVertices+1); // Clean-up heap crap std::fill_n(theta.begin(), nVertices, 0.0); std::fill_n(phi.begin(), nVertices, 0.0); std::fill_n(numb.begin(), nVertices+1, 0); std::fill_n(phinumb.begin(), nVertices+1, 0.0); // Populate arrays and redefine tangent and bitangent e.spherical_polygon(tangent, bitangent, theta, phi, phinumb, numb); // Actual integration occurs here for (int i = 0; i < nVertices; ++i) { // Loop on edges double phiLower = phinumb[i]; // Lower vertex of edge double phiUpper = phinumb[i+1]; // Upper vertex of edge double phiA = (phiUpper - phiLower) / 2.0; double phiB = (phiUpper + phiLower) / 2.0; double thetaLower = theta[numb[i]]; double thetaUpper = theta[numb[i+1]]; double thetaMax = 0.0; Eigen::Vector3d oc = (arcs.col(i) - sph.center) / sph.radius; double oc_norm = oc.norm(); double oc_norm2 = std::pow(oc_norm, 2); for (int j = 0; j < upper_phi; ++j) { // Loop on Gaussian points: phi integration for (int k = 0; k <= 1; ++k) { double ph = (2*k - 1) * phiA * phiRule.abscissa(j) + phiB; double cos_phi = std::cos(ph); double sin_phi = std::sin(ph); if (oc_norm2 < 1.0e-07) { // This should check if oc_norm2 is zero double cotg_thmax = (std::sin(ph-phiLower) / std::tan(thetaUpper) + std::sin(phiUpper-ph) / std::tan( thetaLower)) / std::sin(phiUpper - phiLower); thetaMax = std::atan(1.0 / cotg_thmax); } else { Eigen::Vector3d scratch; scratch << tangent.dot(oc), bitangent.dot(oc), normal.dot(oc); double aa = std::pow(tangent.dot(oc)*cos_phi + bitangent.dot(oc)*sin_phi, 2) + std::pow(normal.dot(oc), 2); double bb = -normal.dot(oc) * oc_norm2; double cc = std::pow(oc_norm2, 2) - std::pow(tangent.dot(oc)*cos_phi + bitangent.dot(oc)*sin_phi, 2); double ds = std::pow(bb, 2) - aa*cc; if (ds < 0.0) ds = 0.0; double cs = (-bb + std::sqrt(ds)) / aa; if (cs > 1.0) cs = 1.0; if (cs < -1.0) cs = 1.0; thetaMax = std::acos(cs); } double thetaA = thetaMax / 2.0; double scratch = 0.0; if (!(thetaMax < 1.0e-08)) { for (int l = 0; l < upper_theta; ++l) { // Loop on Gaussian points: theta integration for (int m = 0; m <= 1; ++m) { double th = (2*m - 1) * thetaA * thetaRule.abscissa(l) + thetaA; double cos_theta = std::cos(th); double sin_theta = std::sin(th); Eigen::Vector3d point; point(0) = tangent(0) * sin_theta * cos_phi + bitangent(0) * sin_theta * sin_phi + normal(0) * (cos_theta - 1.0); point(1) = tangent(1) * sin_theta * cos_phi + bitangent(1) * sin_theta * sin_phi + normal(1) * (cos_theta - 1.0); point(2) = tangent(2) * sin_theta * cos_phi + bitangent(2) * sin_theta * sin_phi + normal(2) * (cos_theta - 1.0); double value = F(e.normal(), Eigen::Vector3d::Zero(), point); // Evaluate integrand at Gaussian point scratch += std::pow(sph.radius, 2) * value * sin_theta * thetaA * thetaRule.weight(l); } } result += scratch * phiA * phiRule.weight(j); } } } } return result; }