Eigen::Vector3d CalibrateData::calibrate(Eigen::Vector3d data,
Eigen::Matrix3d alignment_matrix,
Eigen::Matrix3d sensitivity_matrix,
Eigen::Vector3d offset_vector)
{
Eigen::Vector3d calibrated_data;
calibrated_data = alignment_matrix.inverse() *
sensitivity_matrix.inverse() *
(data - offset_vector);
return calibrated_data;
}
/**
* @brief DiagonalMatrix creates a diagonal matrix.
* @param D a vector of size 3.
* @return It returns a diagonal matrix.
*/
Eigen::Matrix3d DiagonalMatrix(Eigen::Vector3d D)
{
Eigen::Matrix3d ret;
ret.setZero();
ret(0, 0) = D[0];
ret(1, 1) = D[1];
ret(2, 2) = D[2];
return ret;
}
/*
* Creates a rotation matrix which describes how a point in an auxiliary
* coordinate system, whose x axis is desbibed by vec_along_x_axis and has
* a point on its xz-plane vec_on_xz_plane, rotates into the real coordinate
* system.
*/
void Cornea::createRotationMatrix(const Eigen::Vector3d &vec_along_x_axis,
const Eigen::Vector3d &vec_on_xz_plane,
Eigen::Matrix3d &R) {
// normalise pw
Eigen::Vector3d vec_on_xz_plane_n = vec_on_xz_plane.normalized();
// define helper variables x, y and z
// x
Eigen::Vector3d xn = vec_along_x_axis.normalized();
// y
Eigen::Vector3d tmp = vec_on_xz_plane_n.cross(xn);
Eigen::Vector3d yn = tmp.normalized();
// z
tmp = xn.cross(yn);
Eigen::Vector3d zn = tmp.normalized();
// create the rotation matrix
R.col(0) << xn(0), xn(1), xn(2);
R.col(1) << yn(0), yn(1), yn(2);
R.col(2) << zn(0), zn(1), zn(2);
}
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);
}
DensityGrid get_rasterized_fast(const Gaussian3Ds &gmm, const Floats &weights,
double cell_width, const BoundingBox3D &bb, double factor) {
DensityGrid ret(cell_width, bb, 0);
for (unsigned int ng = 0; ng < gmm.size(); ng++) {
Eigen::Matrix3d covar = get_covariance(gmm[ng]);
Eigen::Matrix3d inverse = Eigen::Matrix3d::Zero(3, 3);
double determinant;
bool invertible;
covar.computeInverseAndDetWithCheck(inverse, determinant, invertible);
IMP_INTERNAL_CHECK((invertible && determinant > 0),
"Tried to invert Gaussian, but it's not proper matrix");
double pre(get_gaussian_eval_prefactor(determinant));
Eigen::Vector3d evals = covar.eigenvalues().real();
double maxeval = sqrt(evals.maxCoeff());
double cutoff = factor * maxeval;
double cutoff2 = cutoff * cutoff;
Vector3D c = gmm[ng].get_center();
Vector3D lower = c - Vector3D(cutoff, cutoff, cutoff);
Vector3D upper = c + Vector3D(cutoff, cutoff, cutoff);
GridIndex3D lowerindex = ret.get_nearest_index(lower);
GridIndex3D upperindex = ret.get_nearest_index(upper);
Eigen::Vector3d center(c.get_data());
IMP_INTERNAL_CHECK(invertible, "matrix wasn't invertible! uh oh!");
IMP_GRID3D_FOREACH_SMALLER_EXTENDED_INDEX_RANGE(ret, upperindex, lowerindex,
upperindex, {
GridIndex3D i(voxel_index[0], voxel_index[1], voxel_index[2]);
Eigen::Vector3d r(get_vec_from_center(i, ret, center));
if (r.squaredNorm() < cutoff2) {
update_value(&ret, i, r, inverse, pre, weights[ng]);
}
})
}
return ret;
}
CSUtils::ENUPoint CSUtils::ecef_to_enu(const CSUtils::ECEFPoint& _point, const CSUtils::GCSPoint& _origin)
{
double phi = _origin.latitude;
double lamda = _origin.longitude;
double h = _origin.altitude;
Eigen::Vector3d lccs_center_point;
lccs_center_point(0) = (a * cos(atan(b * tan(phi) / a)) + h * cos(phi)) * cos(lamda);
lccs_center_point(1) = (a * cos(atan(b * tan(phi) / a)) + h * cos(phi)) * sin(lamda);
lccs_center_point(2) = b * sin(atan(b * tan(phi) / a)) + h * sin(phi);
Eigen::Matrix3d C_1 = Eigen::Matrix3d::Zero();
C_1(0, 2) = -1.0; C_1(1, 1) = 1.0; C_1(2, 0) = 1.0;
Eigen::Matrix3d C_2 = Eigen::Matrix3d::Zero();
C_2(0, 0) = 1.0;
C_2(1, 1) = cos(phi); C_2(1, 2) = -sin(phi);
C_2(2, 1) = sin(phi); C_2(2, 2) = cos(phi);
Eigen::Matrix3d C_3 = Eigen::Matrix3d::Zero();
C_3(0, 0) = sin(lamda); C_3(0, 1) = cos(lamda);
C_3(1, 0) = -cos(lamda); C_3(1, 1) = sin(lamda);
C_3(2, 2) = 1.0;
Eigen::Matrix3d C = C_3 * C_2 * C_1;
C.transposeInPlace();
Eigen::Vector3d result = C * Eigen::Vector3d(_point.x, _point.y, _point.z) - C * lccs_center_point;
return CSUtils::ENUPoint(result(0), result(1), result(2));
}
Gaussian3D get_gaussian_from_covariance(const Eigen::Matrix3d &covar,
const Vector3D ¢er) {
Rotation3D rot;
Vector3D radii;
// get eigen decomposition and sort by eigen vector
Eigen::EigenSolver<Eigen::Matrix3d> es(covar);
Eigen::Matrix3d evecs = es.eigenvectors().real();
Eigen::Vector3d evals = es.eigenvalues().real();
// fill in sorted stuff
for (int i = 0; i < 3; i++) {
radii[i] = evals[i];
}
// reflect if necessary
double det = evecs.determinant();
// std::cout<<"Determinant is "<<det<<std::endl;
if (det < 0) {
Eigen::Matrix3d reflect = Eigen::Vector3d(1, 1, -1).asDiagonal();
evecs = evecs * reflect;
}
// create rotation matrix and return
Eigen::Quaterniond eq(evecs);
rot = Rotation3D(eq.w(), eq.x(), eq.y(), eq.z());
return Gaussian3D(ReferenceFrame3D(Transformation3D(rot, center)), radii);
}
void sdh_moveto_cb(boost::shared_ptr<std::string> data){
Eigen::Vector3d p,o;
p.setZero();
o.setZero();
p(0) = right_newP_x;
p(1) = right_newP_y;
p(2) = right_newP_z;
Eigen::Vector3d desired_euler;
Eigen::Matrix3d Ident;
desired_euler.setZero();
Ident.setIdentity();
desired_euler(0) = 0;
desired_euler(1) = -1*M_PI;
desired_euler(2) = 0.5*M_PI;
o = euler2axisangle(desired_euler,Ident);
mutex_act.lock();
right_ac_vec.clear();
right_task_vec.clear();
right_ac_vec.push_back(new ProActController(*right_pm));
right_task_vec.push_back(new KukaSelfCtrlTask(RP_NOCONTROL));
right_task_vec.back()->mt = JOINTS;
right_task_vec.back()->mft = GLOBAL;
right_task_vec.back()->set_desired_p_eigen(p);
right_task_vec.back()->set_desired_o_ax(o);
mutex_act.unlock();
std::cout<<"kuka sdh self movement and move to new pose"<<std::endl;
}
void GreenStrain_LIMSolver3D::prepareProblemData(std::vector<int>& hessRowIdx, std::vector<int>& hessColIdx)
{
const int numNodes = mesh->InitalVertices->rows();
// Compute deformation gradients
int numTets = mesh->Tetrahedra->rows();
Ms.resize(4,3*numTets);
MMTs.resize(4,4*numTets);
Eigen::Matrix<double,4,3> SelectorM;
SelectorM.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
SelectorM.row(3) = Eigen::Vector3d::Ones()*-1;
for(int t=0;t<numTets;t++)
{
Eigen::VectorXi indices = TetrahedronVertexIdx.col(t);
Eigen::Vector3d A = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,0)).cast<double>();
Eigen::Vector3d B = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,1)).cast<double>();
Eigen::Vector3d C = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,2)).cast<double>();
Eigen::Vector3d D = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,3)).cast<double>();
Eigen::Matrix3d V;
V << A-D,B-D,C-D;
Eigen::Matrix<double,4,3> Mtemp = SelectorM*V.inverse().cast<double>();
Ms.block<4,3>(0,3*t) = Mtemp;
MMTs.block<4,4>(0,4*t) = Mtemp*Mtemp.transpose();
}
}
bool MotionEstimator::solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &Rotation, Vector3d &Translation)
{
if (corres.size() >= 15)
{
vector<cv::Point2f> ll, rr;
for (int i = 0; i < int(corres.size()); i++)
{
ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));
rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
}
cv::Mat mask;
cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);
cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
cv::Mat rot, trans;
int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask);
//cout << "inlier_cnt " << inlier_cnt << endl;
Eigen::Matrix3d R;
Eigen::Vector3d T;
for (int i = 0; i < 3; i++)
{
T(i) = trans.at<double>(i, 0);
for (int j = 0; j < 3; j++)
R(i, j) = rot.at<double>(i, j);
}
Rotation = R.transpose();
Translation = -R.transpose() * T;
if(inlier_cnt > 12)
return true;
else
return false;
}
return false;
}
void tangent_and_bitangent(const Eigen::Vector3d & n_,
Eigen::Vector3d & t_, Eigen::Vector3d & b_)
{
double rmin = 0.99;
double n0 = n_(0), n1 = n_(1), n2 = n_(2);
if (std::abs(n0) <= rmin) {
rmin = std::abs(n0);
t_(0) = 0.0;
t_(1) = - n2 / std::sqrt(1.0 - std::pow(n0, 2));
t_(2) = n1 / std::sqrt(1.0 - std::pow(n0, 2));
}
if (std::abs(n1) <= rmin) {
rmin = std::abs(n1);
t_(0) = n2 / std::sqrt(1.0 - std::pow(n1, 2));
t_(1) = 0.0;
t_(2) = - n0 / std::sqrt(1.0 - std::pow(n1, 2));
}
if (std::abs(n2) <= rmin) {
rmin = std::abs(n2);
t_(0) = n1 / std::sqrt(1.0 - std::pow(n2, 2));
t_(1) = -n0 / std::sqrt(1.0 - std::pow(n2, 2));
t_(2) = 0.0;
}
b_ = n_.cross(t_);
// Check that the calculated Frenet-Serret frame is left-handed (levogiro)
// by checking that the determinant of the matrix whose columns are the normal,
// tangent and bitangent vectors has determinant 1 (the system is orthonormal!)
Eigen::Matrix3d M;
M.col(0) = n_;
M.col(1) = t_;
M.col(2) = b_;
if (boost::math::sign(M.determinant()) != 1) {
PCMSOLVER_ERROR("Frenet-Serret local frame is not left-handed!", BOOST_CURRENT_FUNCTION);
}
}
void PartFilter::computeDistance(int partition)
{
std::multimap<int, std::string>::iterator it;
for (int i=0 ; i<mModels.size() ; i++)
{
double distance=0;
it = mOffsetPartToName[i].find(partition);
for (it = mOffsetPartToName[i].equal_range(partition).first ; it != mOffsetPartToName[i].equal_range(partition).second ; ++it)
{
if (mJointNameToPos[(*it).second] != -1)
{
int pos = mJointNameToPos[(*it).second];
double distTemp=0;
// Mahalanobis distance
//cout << (*it).second << "=>" << mPosNames[pos] << endl;
Eigen::Vector3d jtPos = mModels[i]->getJoint((*it).second)->getXYZVect();
Eigen::Vector3d jtObs(mCurrentFrame[pos][1], mCurrentFrame[pos][2], mCurrentFrame[pos][3]);
Eigen::Vector3d diff = jtPos - jtObs;
Eigen::Matrix3d cov;
cov.setIdentity();
distTemp = diff.transpose()*(cov*diff);
distance += distTemp;
}
}
mCurrentDistances[i] = distance;
}
}
double GreenStrain_LIMSolver3D::computeFunction(const Eigen::Matrix<double,Eigen::Dynamic,1>& x)
{
// green strain energy
double shape = 0;
Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
for(int t=0;t<mesh->Tetrahedra->rows();t++)
{
Eigen::Vector3d A(x[TetrahedronVertexIdx.coeff(0,t)],x[TetrahedronVertexIdx.coeff(1,t)],x[TetrahedronVertexIdx.coeff(2,t)]);
Eigen::Vector3d B(x[TetrahedronVertexIdx.coeff(3,t)],x[TetrahedronVertexIdx.coeff(4,t)],x[TetrahedronVertexIdx.coeff(5,t)]);
Eigen::Vector3d C(x[TetrahedronVertexIdx.coeff(6,t)],x[TetrahedronVertexIdx.coeff(7,t)],x[TetrahedronVertexIdx.coeff(8,t)]);
Eigen::Vector3d D(x[TetrahedronVertexIdx.coeff(9,t)],x[TetrahedronVertexIdx.coeff(10,t)],x[TetrahedronVertexIdx.coeff(11,t)]);
Eigen::Matrix<double,3,4> V;
V.col(0) = A;
V.col(1) = B;
V.col(2) = C;
V.col(3) = D;
Eigen::Matrix3d F = V*Ms.block<4,3>(0,3*t);
Eigen::Matrix3d E = (F.transpose()*F - I);
shape += E.squaredNorm()*Divider;
}
return shape;
}
/*
* Given the line parameters [n_x,n_y,a_x,a_y] check if
* [n_x, n_y] dot [data.x-a_x, data.y-a_y] < m_delta
*/
bool RSTEstimator::agree(std::vector<double> ¶meters, std::pair<Eigen::Vector3d,Eigen::Vector3d> &data)
{
Eigen::Matrix3d R;
Eigen::Vector3d T;
Eigen::Vector3d dif;
double E1,E2;
R << parameters[0] , parameters[1] , parameters[2],
parameters[3] , parameters[4] , parameters[5],
parameters[6] , parameters[7] , parameters[8];
T << parameters[9] , parameters[10] , parameters[11];
dif = data.first - R*data.second + T; //X21
E1 = dif.transpose()*dif;
dif = data.second - R.inverse() * (data.first-T); //X12
E2 = dif.transpose()*dif;
//std::cout << "E1= " << E1 << "\nE2= " << E2 << "\nE1+E2= "<< E1+E2 << " " << this->deltaSquared <<"\n";
return ( (E1 + E2) < this->deltaSquared);
}
SRBASolver::SRBASolver()
{
rba_.setVerbosityLevel( 2 ); // 0: None; 1:Important only; 2:Verbose
// =========== Topology parameters ===========
rba_.parameters.srba.max_tree_depth = 3;
rba_.parameters.srba.max_optimize_depth = 3;
rba_.parameters.ecp.min_obs_to_loop_closure = 1; // This is a VERY IMPORTANT PARAM, if it is set to 1 everything goes to shit
rba_.parameters.srba.use_robust_kernel = false;
rba_.parameters.srba.optimize_new_edges_alone = true;
rba_.parameters.srba.dumpToConsole();
first_keyframe_ = true;
curr_kf_id_ = 0;
marker_count_ = 0;
relative_map_frame_ = "relative_map";
global_map_frame_ = "global_map";
loop_closed_ = false;
// Information matrix for relative pose observations:
{
Eigen::Matrix3d ObsL;
ObsL.setZero();
ObsL(0,0) = 1/(STD_NOISE_XY*STD_NOISE_XY); // x
ObsL(1,1) = 1/(STD_NOISE_XY*STD_NOISE_XY); // y
ObsL(2,2) = 1/(STD_NOISE_YAW*STD_NOISE_YAW); // phi
// Set:
rba_.parameters.obs_noise.lambda = ObsL;
}
}
// Assume t = double[3], q = double[4]
void EstimateTfSVD(double* t, double* q)
{
// Assemble the correlation matrix H = target * reference'
Eigen::Matrix3d H = (cloud_tgt_demean * cloud_ref_demean.transpose ()).topLeftCorner<3, 3>();
// Compute the Singular Value Decomposition
Eigen::JacobiSVD<Eigen::Matrix3d> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3d u = svd.matrixU ();
Eigen::Matrix3d v = svd.matrixV ();
// Compute R = V * U'
if (u.determinant () * v.determinant () < 0)
{
for (int i = 0; i < 3; ++i)
v (i, 2) *= -1;
}
// std::cout<< "centroid_src: "<<centroid_src(0) <<" "<< centroid_src(1) <<" "<< centroid_src(2) << " "<< centroid_src(3)<<std::endl;
// std::cout<< "centroid_tgt: "<<centroid_tgt(0) <<" "<< centroid_tgt(1) <<" "<< centroid_tgt(2) << " "<< centroid_tgt(3)<<std::endl;
Eigen::Matrix3d R = v * u.transpose ();
const Eigen::Vector3d Rc (R * centroid_tgt.head<3> ());
Eigen::Vector3d T = centroid_ref.head<3> () - Rc;
// Make sure these memory locations are valid
assert(t != NULL && q!=NULL);
Eigen::Quaterniond Q(R);
t[0] = T(0); t[1] = T(1); t[2] = T(2);
q[0] = Q.w(); q[1] = Q.x(); q[2] = Q.y(); q[3] = Q.z();
}
void mesh_core::Plane::leastSquaresGeneral(
const EigenSTL::vector_Vector3d& points,
Eigen::Vector3d* average)
{
if (points.empty())
{
normal_ = Eigen::Vector3d(0,0,1);
d_ = 0;
if (average)
*average = Eigen::Vector3d::Zero();
return;
}
// find c, the average of the points
Eigen::Vector3d c;
c.setZero();
EigenSTL::vector_Vector3d::const_iterator p = points.begin();
EigenSTL::vector_Vector3d::const_iterator end = points.end();
for ( ; p != end ; ++p)
c += *p;
c *= 1.0/double(points.size());
// Find the matrix
Eigen::Matrix3d m;
m.setZero();
p = points.begin();
for ( ; p != end ; ++p)
{
Eigen::Vector3d cp = *p - c;
m(0,0) += cp.x() * cp.x();
m(1,0) += cp.x() * cp.y();
m(2,0) += cp.x() * cp.z();
m(1,1) += cp.y() * cp.y();
m(2,1) += cp.y() * cp.z();
m(2,2) += cp.z() * cp.z();
}
m(0,1) = m(1,0);
m(0,2) = m(2,0);
m(1,2) = m(2,1);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(m);
if (eigensolver.info() == Eigen::Success)
{
normal_ = eigensolver.eigenvectors().col(0);
normal_.normalize();
}
else
{
normal_ = Eigen::Vector3d(0,0,1);
}
d_ = -c.dot(normal_);
if (average)
*average = c;
}
Eigen::Quaterniond mesh_core::PlaneProjection::getOrientation() const
{
Eigen::Matrix3d m;
m.col(0) = x_axis_;
m.col(1) = y_axis_;
m.col(2) = normal_;
return Eigen::Quaterniond(m);
}
//==============================================================================
Eigen::Vector3d BallJoint::getPositionDifferencesStatic(
const Eigen::Vector3d& _q2, const Eigen::Vector3d& _q1) const
{
const Eigen::Matrix3d R1 = convertToRotation(_q1);
const Eigen::Matrix3d R2 = convertToRotation(_q2);
return convertToPositions(R1.transpose() * R2);
}
Eigen::Matrix3d Reaching::reorderHandAxes(const Eigen::Matrix3d& Q)
{
Eigen::Matrix3d R = Eigen::MatrixXd::Zero(3, 3);
R.col(params_.axis_order_[0]) = Q.col(0); // grasp approach vector
R.col(params_.axis_order_[1]) = Q.col(1); // hand axis
R.col(params_.axis_order_[2]) = Q.col(2); // hand binormal
return R;
}
Eigen::Matrix3d make_inv_matr(
SuperCell& SCell
){
Eigen::Matrix3d invmat;
invmat.row(0) = ( SCell.T.row(1) ).cross( (SCell.T.row(2) )) / ( SCell.T.row( 0 ).dot( ( SCell.T.row(1) ).cross( (SCell.T.row(2) ))) );
invmat.row(1) = ( SCell.T.row(2) ).cross( (SCell.T.row(0) )) / ( SCell.T.row( 0 ).dot( ( SCell.T.row(1) ).cross( (SCell.T.row(2) ))) );
invmat.row(2) = ( SCell.T.row(0) ).cross( (SCell.T.row(1) )) / ( SCell.T.row( 0 ).dot( ( SCell.T.row(1) ).cross( (SCell.T.row(2) ))) );
return invmat;
}
//\fn Eigen::Matrix3d ExtrinsicParam::getRotationMatrix();
///\brief A getter function of the rotation matrix.
///\return The 3x3 matrix of rotation.
Eigen::Matrix3d ExtrinsicParam::getRotationMatrix()
{
if (rotation_computed)
return rotation;
Eigen::Matrix3d zero;
zero.fill(0.);
return zero;
}
const Eigen::Matrix3d CMiniVisionToolbox::getFundamental( const Eigen::Isometry3d& p_matTransformationFrom, const Eigen::Isometry3d& p_matTransformationTo, const Eigen::Matrix3d& p_matIntrinsic )
{
//ds get essential matrix
const Eigen::Matrix3d matEssential( CMiniVisionToolbox::getEssential( p_matTransformationFrom, p_matTransformationTo ) );
//ds compute fundamental matrix: http://en.wikipedia.org/wiki/Fundamental_matrix_%28computer_vision%29
const Eigen::Matrix3d matIntrinsicInverse( p_matIntrinsic.inverse( ) );
return matIntrinsicInverse.transpose( )*matEssential*matIntrinsicInverse;
}
Eigen::Matrix3d createRotationMatrix(double x, double y, double z, double w) {
KDL::Rotation kdl_rotaion = KDL::Rotation::Quaternion(x, y, z, w);
Eigen::Matrix3d result;
for(int i = 0; i < 9; i++) {
result.data()[i] = kdl_rotaion.data[i];
}
result.transposeInPlace();
return result;
}
void pose_estimation_3d3d (
const vector<Point3f>& pts1,
const vector<Point3f>& pts2,
Mat& R, Mat& t
)
{
Point3f p1, p2; // center of mass
int N = pts1.size();
for ( int i=0; i<N; i++ )
{
p1 += pts1[i];
p2 += pts2[i];
}
p1 = Point3f( Vec3f(p1) / N);
p2 = Point3f( Vec3f(p2) / N);
vector<Point3f> q1 ( N ), q2 ( N ); // remove the center
for ( int i=0; i<N; i++ )
{
q1[i] = pts1[i] - p1;
q2[i] = pts2[i] - p2;
}
// compute q1*q2^T
Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
for ( int i=0; i<N; i++ )
{
W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();
}
cout<<"W="<<W<<endl;
// SVD on W
Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();
if (U.determinant() * V.determinant() < 0)
{
for (int x = 0; x < 3; ++x)
{
U(x, 2) *= -1;
}
}
cout<<"U="<<U<<endl;
cout<<"V="<<V<<endl;
Eigen::Matrix3d R_ = U* ( V.transpose() );
Eigen::Vector3d t_ = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d ( p2.x, p2.y, p2.z );
// convert to cv::Mat
R = ( Mat_<double> ( 3,3 ) <<
R_ ( 0,0 ), R_ ( 0,1 ), R_ ( 0,2 ),
R_ ( 1,0 ), R_ ( 1,1 ), R_ ( 1,2 ),
R_ ( 2,0 ), R_ ( 2,1 ), R_ ( 2,2 )
);
t = ( Mat_<double> ( 3,1 ) << t_ ( 0,0 ), t_ ( 1,0 ), t_ ( 2,0 ) );
}
std::unique_ptr<Image> leastSquarePatch(const std::unique_ptr<Image>& image)
{
// process grayscale patches only
if (image->getChannelNum() > 1)
return std::unique_ptr<Image>();
auto data_ptr = image->getValues(0);
const auto w = image->getWidth();
const auto h = image->getHeight();
// least square fit to linear plane for light compensation
float xsum_orig = 0;
float ysum_orig = 0;
float csum_orig = 0;
for (size_t j = 0; j < h; j++)
{
for (size_t i = 0; i < w; i++)
{
xsum_orig += (i * data_ptr[j*w + i]);
ysum_orig += (j * data_ptr[j*w + i]);
csum_orig += (data_ptr[j*w + i]);
}
}
Eigen::Vector3d vsum(xsum_orig, ysum_orig, csum_orig);
float x2sum = 0, y2sum = 0, xysum = 0, xsum = 0, ysum = 0;
float csum = w*h;
for (size_t j = 0; j < h; j++)
{
for (size_t i = 0; i < w; i++)
{
x2sum += (i*i);
y2sum += (j*j);
xysum += (i*j);
xsum += i;
ysum += j;
}
}
Eigen::Matrix3d msum;
msum << x2sum, xysum, xsum,
xysum, y2sum, ysum,
xsum, ysum, csum;
auto vcoeff = msum.inverse() * vsum;
auto newImage = std::make_unique<Image>(w, h, 1);
for (size_t j = 0; j < h; j++)
{
for (size_t i = 0; i < w; i++)
{
(newImage->getValues(0))[j*w + i] = data_ptr[j*w + i]
- i*vcoeff[0] - j*vcoeff[1] - vcoeff[2];
}
}
return newImage;
}
ON_NurbsSurface
FittingSurface::initNurbsPCA (int order, NurbsDataSurface *m_data, ON_3dVector z)
{
ON_3dVector mean;
Eigen::Matrix3d eigenvectors;
Eigen::Vector3d eigenvalues;
unsigned s = m_data->interior.size ();
NurbsTools::pca (m_data->interior, mean, eigenvectors, eigenvalues);
m_data->mean = mean;
//m_data->eigenvectors = (*eigenvectors);
bool flip (false);
Eigen::Vector3d ez(z[0],z[1],z[2]);
if (eigenvectors.col (2).dot (ez) < 0.0)
flip = true;
eigenvalues = eigenvalues / s; // seems that the eigenvalues are dependent on the number of points (???)
ON_3dVector sigma(sqrt(eigenvalues[0]), sqrt(eigenvalues[1]), sqrt(eigenvalues[2]));
ON_NurbsSurface nurbs (3, false, order, order, order, order);
nurbs.MakeClampedUniformKnotVector (0, 1.0);
nurbs.MakeClampedUniformKnotVector (1, 1.0);
// +- 2 sigma -> 95,45 % aller Messwerte
double dcu = (4.0 * sigma[0]) / (nurbs.Order (0) - 1);
double dcv = (4.0 * sigma[1]) / (nurbs.Order (1) - 1);
ON_3dVector cv_t, cv;
Eigen::Vector3d ecv_t, ecv;
Eigen::Vector3d emean(mean[0], mean[1], mean[2]);
for (int i = 0; i < nurbs.Order (0); i++)
{
for (int j = 0; j < nurbs.Order (1); j++)
{
cv[0] = -2.0 * sigma[0] + dcu * i;
cv[1] = -2.0 * sigma[1] + dcv * j;
cv[2] = 0.0;
ecv (0) = -2.0 * sigma[0] + dcu * i;
ecv (1) = -2.0 * sigma[1] + dcv * j;
ecv (2) = 0.0;
ecv_t = eigenvectors * ecv + emean;
cv_t[0] = ecv_t (0);
cv_t[1] = ecv_t (1);
cv_t[2] = ecv_t (2);
if (flip)
nurbs.SetCV (nurbs.Order (0) - 1 - i, j, ON_3dPoint (cv_t[0], cv_t[1], cv_t[2]));
else
nurbs.SetCV (i, j, ON_3dPoint (cv_t[0], cv_t[1], cv_t[2]));
}
}
return nurbs;
}
void MarkerArrayVisualizer::drawGaussianDistributions(const char* ns, const std::vector<Eigen::Vector3d>& mu, const std::vector<Eigen::Matrix3d>& sigma, double probability, const std::vector<double>& offset)
{
visualization_msgs::MarkerArray marker_array;
const int size = mu.size();
for (int i=0; i<size; i++)
{
visualization_msgs::Marker marker;
marker.header.frame_id = "/world";
marker.header.stamp = ros::Time::now();
marker.ns = ns;
marker.id = i;
marker.type = visualization_msgs::Marker::SPHERE;
marker.action = visualization_msgs::Marker::ADD;
// axis: eigenvectors
// radius: eigenvalues
Eigen::JacobiSVD<Eigen::MatrixXd> svd(sigma[i], Eigen::ComputeThinU | Eigen::ComputeThinV);
const Eigen::VectorXd& r = svd.singularValues();
Eigen::Matrix3d Q = svd.matrixU();
// to make determinant 1
if (Q.determinant() < 0)
Q.col(2) *= -1.;
const Eigen::Quaterniond q(Q);
marker.pose.position.x = mu[i](0);
marker.pose.position.y = mu[i](1);
marker.pose.position.z = mu[i](2);
marker.pose.orientation.x = q.x();
marker.pose.orientation.y = q.y();
marker.pose.orientation.z = q.z();
marker.pose.orientation.w = q.w();
const double probability_radius = gaussianDistributionRadius3D(probability);
marker.scale.x = 2. * (probability_radius * std::sqrt(r[0]) + offset[i]);
marker.scale.y = 2. * (probability_radius * std::sqrt(r[1]) + offset[i]);
marker.scale.z = 2. * (probability_radius * std::sqrt(r[2]) + offset[i]);
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
marker.color.a = 0.25;
marker.lifetime = ros::Duration();
marker_array.markers.push_back(marker);
}
publish(marker_array);
}
void CETranslateWidget::checkSelection()
{
// User closed the dialog:
if (this->isHidden()) {
m_selectionTimer.stop();
return;
}
// Improper initialization
if (m_gl == NULL)
return setError(tr("No GLWidget?"));
QList<Primitive*> atoms = m_gl->selectedPrimitives().subList
(Primitive::AtomType);
// No selection
if (!atoms.size())
return setError(tr("Please select one or more atoms."));
clearError();
// Calculate centroid of selection
m_vector = Eigen::Vector3d::Zero();
for (QList<Primitive*>::const_iterator
it = atoms.constBegin(),
it_end = atoms.constEnd();
it != it_end; ++it) {
m_vector += (*qobject_cast<Atom* const>(*it)->pos());
}
m_vector /= static_cast<double>(atoms.size());
switch (static_cast<TranslateMode>
(ui.combo_translateMode->currentIndex())) {
default:
case Avogadro::CETranslateWidget::TM_VECTOR:
// Shouldn't happen, but just in case...
m_selectionTimer.stop();
this->enableVectorEditor();
break;
case Avogadro::CETranslateWidget::TM_ATOM_TO_ORIGIN:
m_vector = -m_vector;
break;
case Avogadro::CETranslateWidget::TM_ATOM_TO_CELL_CENTER:
// Calculate center of unit cell
const Eigen::Matrix3d cellRowMatrix = m_ext->currentCellMatrix();
const Eigen::Vector3d center = 0.5 *
(cellRowMatrix.row(0) + cellRowMatrix.row(1) + cellRowMatrix.row(2));
// Calculate necessary translation
m_vector = center - m_vector;
break;
}
updateGui();
}
Eigen::Vector4f IncrementalPlaneEstimator::
getPlane(const Eigen::Vector3d& iSum,
const Eigen::Matrix3d& iSumSquared,
const double iCount) {
Eigen::Vector3d mean = iSum/iCount;
Eigen::Matrix3d cov = iSumSquared/iCount - mean*mean.transpose();
Eigen::Vector4d plane;
plane.head<3>() = cov.jacobiSvd(Eigen::ComputeFullV).matrixV().col(2);
plane[3] = -plane.head<3>().dot(mean);
return plane.cast<float>();
}
