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;*/
}
void Element::getTerrainContactPoints(Eigen::Matrix3Xd &local_points)
{
if ( !hasGeometry() ) {
local_points = Eigen::Matrix3Xd();
return;
}
Eigen::Matrix3Xd points;
geometry->getTerrainContactPoints(points);
Eigen::Matrix4Xd transformed_points = T_element_to_local * points.colwise().homogeneous();
local_points = transformed_points.colwise().hnormalized();
}
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);
}
}
TEST(OdometryCalibration, Jacobian) {
Odometry odometry;
odometry.ux = 2.0;
odometry.uy = 3.0;
odometry.utheta = M_PI;
const Eigen::Matrix3Xd jacobian = OdometryCalibration::jacobian(odometry);
ASSERT_EQ(3, (int) jacobian.rows());
ASSERT_EQ(9, (int) jacobian.cols());
ASSERT_DOUBLE_EQ(-2.0, (double) jacobian(0, 0));
ASSERT_DOUBLE_EQ(-3.0, (double) jacobian(0, 1));
ASSERT_DOUBLE_EQ(-M_PI, (double) jacobian(0, 2));
ASSERT_DOUBLE_EQ(0.0, (double) jacobian(0, 3));
ASSERT_DOUBLE_EQ(-M_PI, (double) jacobian(2, 8));
ASSERT_DOUBLE_EQ(-15-3*M_PI, jacobian.sum());
}
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;
}
cv::Mat toMat(Eigen::Matrix3Xd emat){
double* p = new double[emat.rows()*emat.cols()];
int w = emat.cols();
for (int i=0;i<emat.rows();i++)
for (int j=0;j<w;j++)
p[i*w+j] = emat(i,j);
cv::Mat result(emat.rows(),emat.cols(),CV_64F,p);
return result;
}
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)!");
}
}
// 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;
}
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;
}