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;*/
}
Exemple #2
0
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;
}
Exemple #3
0
// 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;
}
Exemple #4
0
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;
  }
}
Exemple #5
0
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);
  }
}
Exemple #6
0
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";
}
Exemple #7
0
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;
}
Exemple #8
0
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;
}
Exemple #9
0
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;
}