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; }
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 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 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; }