Esempio n. 1
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;
}
Esempio n. 2
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;
  }
}
Esempio n. 3
0
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;*/
}
Esempio n. 4
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;
}
Esempio n. 5
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;
}