本文整理汇总了C++中eigen::Matrix3d类的典型用法代码示例。如果您正苦于以下问题:C++ Matrix3d类的具体用法?C++ Matrix3d怎么用?C++ Matrix3d使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了Matrix3d类的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: createRotationMatrix
/*
* 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);
}
开发者ID:bwrc,项目名称:gaze_tracker_glasses,代码行数:31,代码来源:Cornea_computer.cpp
示例2: svd
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);
}
开发者ID:mangi020,项目名称:mrpt,代码行数:33,代码来源:rpnp.cpp
示例3: get_rasterized_fast
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;
}
开发者ID:salilab,项目名称:imp,代码行数:35,代码来源:Gaussian3D.cpp
示例4:
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();
}
}
开发者ID:AurelGruber,项目名称:Scalable-Locally-Injective-Mappings,代码行数:30,代码来源:GreenStrain_LIMSolver3D.cpp
示例5: get_gaussian_from_covariance
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);
}
开发者ID:salilab,项目名称:imp,代码行数:28,代码来源:Gaussian3D.cpp
示例6: sdh_moveto_cb
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;
}
开发者ID:liqiang1980,项目名称:VTFS,代码行数:31,代码来源:HingedToolManip.cpp
示例7: ecef_to_enu
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));
}
开发者ID:gorgool,项目名称:Radar-software,代码行数:31,代码来源:CSTransform.cpp
示例8: solveRelativeRT
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;
}
开发者ID:JianCui1992,项目名称:VINS-Mono,代码行数:35,代码来源:solve_5pts.cpp
示例9: A
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;
}
开发者ID:AurelGruber,项目名称:Scalable-Locally-Injective-Mappings,代码行数:25,代码来源:GreenStrain_LIMSolver3D.cpp
示例10: EstimateTfSVD
// 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();
}
开发者ID:mruan,项目名称:range_calib,代码行数:32,代码来源:linearTF_solver.hpp
示例11: computeDistance
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;
}
}
开发者ID:cmollare,项目名称:IK_PF,代码行数:27,代码来源:PartFilter.cpp
示例12: agree
/*
* 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);
}
开发者ID:Modasshir,项目名称:socrob-ros-pkg,代码行数:30,代码来源:RSTEstimator.cpp
示例13: tangent_and_bitangent
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);
}
}
开发者ID:runesorland,项目名称:pcmsolver,代码行数:35,代码来源:Element.cpp
示例14: ObsL
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;
}
}
开发者ID:stwirth,项目名称:relative_slam,代码行数:33,代码来源:srba_solver.cpp
示例15: m
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;
}
开发者ID:team-vigir,项目名称:vigir_moveit_advanced,代码行数:59,代码来源:geom.cpp
示例16: Quaterniond
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);
}
开发者ID:team-vigir,项目名称:vigir_moveit_advanced,代码行数:8,代码来源:geom.cpp
示例17:
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;
}
开发者ID:atenpas,项目名称:grasp_selection,代码行数:8,代码来源:reaching.cpp
示例18: convertToPositions
//==============================================================================
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);
}
开发者ID:ayonga,项目名称:dart,代码行数:9,代码来源:BallJoint.cpp
示例19: pose_estimation_3d3d
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 ) );
}
开发者ID:gaoxiang12,项目名称:slambook,代码行数:57,代码来源:pose_estimation_3d3d.cpp
示例20:
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;
}
开发者ID:jdmcclain47,项目名称:ExtendedHubbard2,代码行数:9,代码来源:ao_ints.cpp
注:本文中的eigen::Matrix3d类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。 |
请发表评论