bool KinematicsMetrics::getManipulabilityIndex(const robot_state::RobotState &state,
const robot_model::JointModelGroup *joint_model_group,
double &manipulability_index,
bool translation) const
{
// state.getJacobian() only works for chain groups.
if(!joint_model_group->isChain())
{
return false;
}
Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
// Get joint limits penalty
double penalty = getJointLimitsPenalty(state, joint_model_group);
if (translation)
{
Eigen::MatrixXd jacobian_2 = jacobian.topLeftCorner(3,jacobian.cols());
Eigen::MatrixXd matrix = jacobian_2*jacobian_2.transpose();
// Get manipulability index
manipulability_index = penalty * sqrt(matrix.determinant());
}
else
{
Eigen::MatrixXd matrix = jacobian*jacobian.transpose();
// Get manipulability index
manipulability_index = penalty * sqrt(matrix.determinant());
}
return true;
}
double weight_gaussian_predictive_rev(Gaussian &g1, Gaussian &g2)
{
int dim = g1.dim;
double energy1 = 0.;
Eigen::MatrixXd cov = g1.predictive_covariance + g1.predictive_covariance;
Eigen::VectorXd mean = g1.predictive_mean - g1.predictive_mean;
Eigen::MatrixXd invij = cov.inverse();
double a = mean.transpose()*invij*mean;
double gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
energy1 += gauss;
double energy2 = 0.;
cov = g1.predictive_covariance + g2.predictive_covariance;
mean = g1.predictive_mean - g2.predictive_mean;
invij = cov.inverse();
a = mean.transpose()*invij*mean;
gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
energy2 += gauss;
double energy3 = 0.;
cov = g2.predictive_covariance + g2.predictive_covariance;
mean = g2.predictive_mean - g2.predictive_mean;
invij = cov.inverse();
a = mean.transpose()*invij*mean;
gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
energy3 += gauss;
// cout<<(maxDist - (energy1 - 2*energy2 + energy3))/maxDist<<endl;
return energy1 - 2*energy2 + energy3;
}
double weight_l2_rev(PCObject &o1, PCObject &o2)
{
double last = pcl::getTime ();
// reference :
// Robust Point Set Registration Using Gaussian Mixture Models
// Bing Jina, and Baba C. Vemuri
// IEEE Transactions on Pattern Analysis and Machine Intelligence 2010
int n = o1.gmm.size();
int m = o2.gmm.size();
double energy1 = 0.;
for(int i=0;i<n;i++){
for(int j=0;j<n;j++){
int dim = o1.gmm.at(i).dim;
Eigen::MatrixXd cov = o1.gmm.at(i).covariance + o1.gmm.at(j).covariance;
Eigen::VectorXd mean = o1.gmm.at(i).mean - o1.gmm.at(j).mean;
Eigen::MatrixXd invij = cov.inverse();
double a = mean.transpose()*invij*mean;
double gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
energy1 += o1.gmm.at(i).weight*o1.gmm.at(j).weight*gauss;
}
}
double energy2 = 0.;
for(int i=0;i<n;i++){
for(int j=0;j<m;j++){
int dim = o1.gmm.at(i).dim;
Eigen::MatrixXd cov = o1.gmm.at(i).covariance + o2.gmm.at(j).covariance;
Eigen::VectorXd mean = o1.gmm.at(i).mean - o2.gmm.at(j).mean;
Eigen::MatrixXd invij = cov.inverse();
double a = mean.transpose()*invij*mean;
double gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
energy2 += o1.gmm.at(i).weight*o2.gmm.at(j).weight*gauss;
}
}
double energy3 = 0.;
for(int i=0;i<m;i++){
for(int j=0;j<m;j++){
int dim = o2.gmm.at(i).dim;
Eigen::MatrixXd cov = o2.gmm.at(i).covariance + o2.gmm.at(j).covariance;
Eigen::VectorXd mean = o2.gmm.at(i).mean - o2.gmm.at(j).mean;
Eigen::MatrixXd invij = cov.inverse();
double a = mean.transpose()*invij*mean;
double gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
energy3 += o2.gmm.at(i).weight*o2.gmm.at(j).weight*gauss;
}
}
double now = pcl::getTime ();
// cout << "l2-distance time " << now-last << " second" << endl;
// cout<<"l2distance"<<energy1 - 2*energy2 + energy3<<endl;
return (energy1 - 2*energy2 + energy3);
}
void mrpt::vision::pnp::rpnp::calcampose(Eigen::MatrixXd& XXc, Eigen::MatrixXd& XXw, Eigen::Matrix3d& R2, Eigen::Vector3d& t2)
{
Eigen::MatrixXd X = XXc;
Eigen::MatrixXd Y = XXw;
Eigen::MatrixXd K = Eigen::MatrixXd::Identity(n, n) - Eigen::MatrixXd::Ones(n, n) * 1 / n;
Eigen::VectorXd ux, uy;
uy = X.rowwise().mean();
ux = Y.rowwise().mean();
// Need to verify sigmax2
double sigmax2 = (((X*K).array() * (X*K).array()).colwise().sum()).mean();
Eigen::MatrixXd SXY = Y*K*(X.transpose()) / n;
Eigen::JacobiSVD<Eigen::MatrixXd> svd(SXY, Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::Matrix3d S = Eigen::MatrixXd::Identity(3, 3);
if (SXY.determinant() <0)
S(2, 2) = -1;
R2 = svd.matrixV()*S*svd.matrixU().transpose();
double c2 = (svd.singularValues().asDiagonal()*S).trace() / sigmax2;
t2 = uy - c2*R2*ux;
Eigen::Vector3d x, y, z;
x = R2.col(0);
y = R2.col(1);
z = R2.col(2);
if ((x.cross(y) - z).norm()>0.02)
R2.col(2) = -R2.col(2);
}
int main() {
int N = 10;
Eigen::MatrixXd A = Eigen::MatrixXd::Random(N,N);
double detc = get_Log_Determinant(A);
double det = log(fabs(A.determinant()));
std::cout << fabs(detc-det) << std::endl;
}
// computes the optimal rigid body transformation given a set of points
void PoseTracker::computeOptimalRigidTransformation(Eigen::MatrixXd startP, Eigen::MatrixXd finalP)
{
Eigen::Matrix4d transf;
if (startP.rows()!=finalP.rows())
{
ROS_ERROR("We need that the rows be the same at the beggining");
exit(1);
}
Eigen::RowVector3d centroid_startP = Eigen::RowVector3d::Zero();
Eigen::RowVector3d centroid_finalP = Eigen::RowVector3d::Zero(); //= mean(B);
Eigen::Matrix3d H = Eigen::Matrix3d::Zero();
//calculate the mean
for (int i=0;i<startP.rows();i++)
{
centroid_startP = centroid_startP+startP.row(i);
centroid_finalP = centroid_finalP+finalP.row(i);
}
centroid_startP = centroid_startP/startP.rows();
centroid_finalP = centroid_finalP/startP.rows();
for (int i=0;i<startP.rows();i++)
H = H + (startP.row(i)-centroid_startP).transpose()*(finalP.row(i)-centroid_finalP);
Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::MatrixXd U = svd.matrixU();
Eigen::MatrixXd V = svd.matrixV();
if (V.determinant()<0)
V.col(2)=-V.col(2)*(-1);
Eigen::MatrixXd R = V*U.transpose();
Eigen::Matrix4d C_A = Eigen::Matrix4d::Identity();
Eigen::Matrix4d C_B = Eigen::Matrix4d::Identity();
Eigen::Matrix4d R_new = Eigen::Matrix4d::Identity();
C_A.block<3,1>(0,3) = -centroid_startP.transpose();
R_new.block<3,3>(0,0) = R;
C_B.block<3,1>(0,3) = centroid_finalP.transpose();
transf = C_B * R_new * C_A;
Eigen::Quaterniond mat_rot(transf.block<3,3>(0,0));
Eigen::Vector3d trasl = transf.block<3,1>(0,3).transpose();
transfParameters_ << trasl(0), trasl(1), trasl(2), mat_rot.x(), mat_rot.y(), mat_rot.z(), mat_rot.w();
}
double Jacobian::getManipulabilityMeasure(const Eigen::VectorXd& joint_values)
{
ROS_ASSERT(initialized_);
Eigen::MatrixXd jac;
getJacobian(joint_values, jac);
Eigen::MatrixXd JJT = jac * jac.transpose();
return sqrt(JJT.determinant());
}
double multivariateGaussianDensity(const Eigen::VectorXd& mean,
const Eigen::MatrixXd& cov,
const Eigen::VectorXd& z)
{
Eigen::VectorXd diff = mean - z;
Eigen::VectorXd exponent = -0.5 * (diff.transpose() * cov.inverse() * diff);
return pow(2 * M_PI, (double) z.size() / -2.0) * pow(cov.determinant(), -0.5) *
exp(exponent(0));
}
double weight_l2_gauss(PCObject &o1, PCObject &o2)
{
// l2 distance
Eigen::MatrixXd covsum = o1.gaussian.covariance+o2.gaussian.covariance;
Eigen::VectorXd meandiff = o1.gaussian.mean - o2.gaussian.mean;
Eigen::MatrixXd inv = covsum.inverse();
double det = covsum.determinant();
double a = meandiff.transpose()*inv*meandiff;
double l2 = 2.-2.*(1./sqrt(pow(2*pi,3)*det)) * exp(-0.5*a);
if(l2 < 0) l2 = 0.;
return l2;
}
Eigen::Matrix3d RMFE::getRotationMatrix() {
// Paper: Robust Manhattan Frame Estimation from a Single RGB-D Image
double lambda = 0.3;
Eigen::MatrixXd N = this->getNormalMatrix();
Eigen::Matrix3d R = Eigen::Matrix<double, 3, 3>::Identity();
float last_error = 10000.0f, error_diff = 10000.0f;
while (error_diff > 0.0001 || error_diff < 0) {
Eigen::MatrixXd X = R * N;
RMFE::applySoftThresholding(X, lambda);
Eigen::MatrixXd XNTranspose = X * N.transpose();
Eigen::JacobiSVD<Eigen::MatrixXd> svd(XNTranspose,
Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::MatrixXd U = svd.matrixU();
Eigen::MatrixXd V = svd.matrixV();
Eigen::VectorXd D = svd.singularValues();
/*std::cout << "U: " << U << std::endl;
std::cout << "V: " << V << std::endl;
std::cout << "D: " << D << std::endl;*/
//std::cout << "Remaining: " << (U * D.asDiagonal() * V.transpose() - XNTranspose) << std::endl;
Eigen::Matrix3d S = Eigen::Matrix<double, 3, 3>::Identity();
if (XNTranspose.determinant() < 0) {
S(2, 2) = -1;
}
R = U * S * V.transpose();
float L1NormSum = 0;
for (int j = 0; j < X.rows(); j++) {
L1NormSum += X.row(j).lpNorm<1>();
}
float error = 0.5 * std::pow((R * N - X).lpNorm<2>(), 2)
+ lambda * L1NormSum;
error_diff = last_error - error;
last_error = error;
/*std::cout << "Iteration: " << iter << std::endl;
std::cout << "R: " << R << std::endl;
//std::cout << "X: " << X << std::endl;
std::cout << "Error: " << error << std::endl;*/
}
return R;
}
int main(int argc, char** argv)
{
int sequence_number = 1;
if (argc >= 2)
sequence_number = atoi(argv[1]);
ros::init(argc, argv, "figures");
ROS_INFO("figures");
ros::NodeHandle n;
const double timestep = 0.05;
const double prediction_timestep = 0.05;
const double sensor_error = 0.005;
const double collision_probability = 0.95;
const int acceleration_inference_window_size = 5;
const int prediction_frames = 6;
ros::Rate rate(1.0 / timestep);
KinectPredictor predictor(sequence_number);
predictor.setTimestep(timestep);
predictor.setSensorDiagonalCovariance(sensor_error * sensor_error); // variance is proportional to square of sensing error
predictor.setCollisionProbability(collision_probability);
predictor.setAccelerationInferenceWindowSize(acceleration_inference_window_size);
predictor.setMaximumIterations(5);
predictor.setGradientDescentMaximumIterations(5);
predictor.setGradientDescentAlpha(0.005);
predictor.setHumanShapeLengthConstraintEpsilon(0.01);
predictor.translate(Eigen::Vector3d(0, 0, -0.1));
predictor.setVisualizerTopic("figures");
int frame = 0;
while (true)
{
predictor.moveToNextFrame();
frame++;
if (frame == 10)
{
FILE* fp = fopen("../data/figures/circles.txt", "w");
// to visualize
predictor.visualizePointcloud();
predictor.visualizeHuman();
for (int future_frame_index = 0; future_frame_index < prediction_frames; future_frame_index++)
predictor.visualizePrediction(future_frame_index * prediction_timestep);
// to retrieve gaussian distribution
std::vector<Eigen::Vector3d> mu;
std::vector<Eigen::Matrix3d> sigma;
std::vector<double> radius;
predictor.getPredictedGaussianDistribution(0, mu, sigma, radius);
for (int j=0; j<mu.size(); j++)
{
// to YZ plane
const double y = mu[j](1);
const double z = mu[j](2);
const double r = radius[j];
const Eigen::Matrix2d sigma2 = sigma[j].block(1, 1, 2, 2);
Eigen::JacobiSVD<Eigen::MatrixXd> svd(sigma2, Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::MatrixXd U = svd.matrixU();
if (U.determinant() < 0.)
U.col(1) *= -1.;
/*
fprintf(fp, "%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",
y, z, r,
U(0,0), U(1,0), svd.singularValues()(0),
U(0,1), U(1,1), svd.singularValues()(1));
*/
fprintf(fp, "%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",
y, z, r,
sigma2(0,0), sigma2(1,0), sigma2(0,1), sigma2(1,1));
}
fclose(fp);
const Pointcloud& pointcloud = predictor.pointcloud();
fp = fopen("../data/figures/pointcloud.txt", "w");
for (int i=0; i<pointcloud.size(); i++)
{
const Eigen::Vector3d& v = pointcloud.point(i);
fprintf(fp, "%lf,%lf\n", v(1), v(2));
}
fclose(fp);
break;
}
}
return 0;
}
