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