本文整理汇总了C++中MatrixXd函数的典型用法代码示例。如果您正苦于以下问题:C++ MatrixXd函数的具体用法?C++ MatrixXd怎么用?C++ MatrixXd使用的例子?那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。
在下文中一共展示了MatrixXd函数的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: DQ_controller
DampedNumericalFilteredController::DampedNumericalFilteredController(DQ_kinematics robot, MatrixXd kp, double beta, double lambda_max, double epsilon) : DQ_controller()
{
//Constants
dq_one_ = DQ(1);
//Initialization of argument parameters
robot_dofs_ = (robot.links() - robot.n_dummy());
robot_ = robot;
kp_ = kp;
ki_ = MatrixXd::Zero(kp.rows(),kp.cols());
kd_ = MatrixXd::Zero(kp.rows(),kp.cols());
beta_ = beta;
lambda_max_ = lambda_max;
//Initilization of remaining parameters
thetas_ = MatrixXd(robot_dofs_,1);
delta_thetas_ = MatrixXd::Zero(robot_dofs_,1);
task_jacobian_ = MatrixXd(8,robot_dofs_);
svd_ = JacobiSVD<MatrixXd>(robot_dofs_,8);
svd_sigma_inverted_ = MatrixXd::Zero(robot_dofs_,8);
identity_ = Matrix<double,8,8>::Identity();
error_ = MatrixXd::Zero(8,1);
integral_error_ = MatrixXd::Zero(8,1);
last_error_ = MatrixXd::Zero(8,1);
at_least_one_error_ = false;
end_effector_pose_ = DQ(0,0,0,0,0,0,0,0);
}
示例2: test_eigensolver_selfadjoint
void test_eigensolver_selfadjoint()
{
int s = 0;
for(int i = 0; i < g_repeat; i++) {
// very important to test 3x3 and 2x2 matrices since we provide special paths for them
CALL_SUBTEST_1( selfadjointeigensolver(Matrix2f()) );
CALL_SUBTEST_1( selfadjointeigensolver(Matrix2d()) );
CALL_SUBTEST_1( selfadjointeigensolver(Matrix3f()) );
CALL_SUBTEST_1( selfadjointeigensolver(Matrix3d()) );
CALL_SUBTEST_2( selfadjointeigensolver(Matrix4d()) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_3( selfadjointeigensolver(MatrixXf(s,s)) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_4( selfadjointeigensolver(MatrixXd(s,s)) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_5( selfadjointeigensolver(MatrixXcd(s,s)) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_9( selfadjointeigensolver(Matrix<std::complex<double>,Dynamic,Dynamic,RowMajor>(s,s)) );
// some trivial but implementation-wise tricky cases
CALL_SUBTEST_4( selfadjointeigensolver(MatrixXd(1,1)) );
CALL_SUBTEST_4( selfadjointeigensolver(MatrixXd(2,2)) );
CALL_SUBTEST_6( selfadjointeigensolver(Matrix<double,1,1>()) );
CALL_SUBTEST_7( selfadjointeigensolver(Matrix<double,2,2>()) );
}
// Test problem size constructors
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_8(SelfAdjointEigenSolver<MatrixXf> tmp1(s));
CALL_SUBTEST_8(Tridiagonalization<MatrixXf> tmp2(s));
TEST_SET_BUT_UNUSED_VARIABLE(s)
}
示例3: forwardMatrixIterative
/**
* @brief likelyhood : method to compute the log
* likeyhood of observed sequence
* @param sequence :input observation sequence
* @return
*/
float ocv::CHMM::predictIterative(Mat sequence,bool init)
{
//computing the probability of observed sequence
//using forward algorithm
if(init==true)
{
count=0;
prob=0;
_alpha=MatrixXd(_nstates,_maxseqlen);
_scale=MatrixXd(1,_maxseqlen);
}
int i=count;
forwardMatrixIterative(sequence,init,i);
prob=(_scale(0,i));
//prob=-prob;
//cerr << prob <<":" << count <<"--";
count=count+1;
return prob;
}
示例4: test_eigensolver_generic
void test_eigensolver_generic()
{
int s = 0;
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( eigensolver(Matrix4f()) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_2( eigensolver(MatrixXd(s,s)) );
// some trivial but implementation-wise tricky cases
CALL_SUBTEST_2( eigensolver(MatrixXd(1,1)) );
CALL_SUBTEST_2( eigensolver(MatrixXd(2,2)) );
CALL_SUBTEST_3( eigensolver(Matrix<double,1,1>()) );
CALL_SUBTEST_4( eigensolver(Matrix2d()) );
}
CALL_SUBTEST_1( eigensolver_verify_assert(Matrix4f()) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_2( eigensolver_verify_assert(MatrixXd(s,s)) );
CALL_SUBTEST_3( eigensolver_verify_assert(Matrix<double,1,1>()) );
CALL_SUBTEST_4( eigensolver_verify_assert(Matrix2d()) );
// Test problem size constructors
CALL_SUBTEST_5(EigenSolver<MatrixXf> tmp(s));
// regression test for bug 410
CALL_SUBTEST_2(
{
MatrixXd A(1,1);
A(0,0) = std::sqrt(-1.);
Eigen::EigenSolver<MatrixXd> solver(A);
MatrixXd V(1, 1);
V(0,0) = solver.eigenvectors()(0,0).real();
}
);
示例5: MatrixXd
void LDA::gibbs(int K,double alpha,double beta) {
this->K=K;
this->alpha=alpha;
this->beta=beta;
if(SAMPLE_LAG >0) {
thetasum = MatrixXd(documentsSize(),K);
phisum= MatrixXd(K,V);
numstats=0;
}
initialState(K);
for(int i=0; i<ITERATIONS; i++) {
for(int m=0; m<z.rows(); m++) {
for(int n=0; n<z.cols(); n++) {
z(m,n)=sampleFullConditional(m,n);
}
}
if(i > BURN_IN && (i%THIN_INTERVAL==0)) {
dispcol++;
}
if ((i > BURN_IN) && (SAMPLE_LAG > 0) && (i % SAMPLE_LAG == 0)) {
updateParams();
if (i % THIN_INTERVAL != 0)
dispcol++;
}
if (dispcol >= 100) {
dispcol = 0;
}
}
}
示例6: test_eigensolver_generic
void test_eigensolver_generic()
{
int s = 0;
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( eigensolver(Matrix4f()) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_2( eigensolver(MatrixXd(s,s)) );
TEST_SET_BUT_UNUSED_VARIABLE(s)
// some trivial but implementation-wise tricky cases
CALL_SUBTEST_2( eigensolver(MatrixXd(1,1)) );
CALL_SUBTEST_2( eigensolver(MatrixXd(2,2)) );
CALL_SUBTEST_3( eigensolver(Matrix<double,1,1>()) );
CALL_SUBTEST_4( eigensolver(Matrix2d()) );
}
CALL_SUBTEST_1( eigensolver_verify_assert(Matrix4f()) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_2( eigensolver_verify_assert(MatrixXd(s,s)) );
CALL_SUBTEST_3( eigensolver_verify_assert(Matrix<double,1,1>()) );
CALL_SUBTEST_4( eigensolver_verify_assert(Matrix2d()) );
// Test problem size constructors
CALL_SUBTEST_5(EigenSolver<MatrixXf> tmp(s));
// regression test for bug 410
CALL_SUBTEST_2(
{
MatrixXd A(1,1);
A(0,0) = std::sqrt(-1.); // is Not-a-Number
Eigen::EigenSolver<MatrixXd> solver(A);
VERIFY_IS_EQUAL(solver.info(), NumericalIssue);
}
);
示例7: x0
void CVMath::setupMatrixM1()
{
Line r1 = Points::getInstance().getR1();
Line r2 = Points::getInstance().getR2();
Line r3 = Points::getInstance().getR3();
Line r4 = Points::getInstance().getR4();
l0 << r1.x, r1.y, r1.z;
l1 << r2.x, r2.y, r2.z;
l2 << r3.x, r3.y, r3.z;
l3 << r4.x, r4.y, r4.z;
x0 = l0.cross(l1);
x0 << x0(0)/x0(2), x0(1)/x0(2), 1;
x1 = l2.cross(l3);
x1 << x1(0)/x1(2), x1(1)/x1(2), 1;
l = x0.cross(x1);
Hp = MatrixXd(3, 3);
Hp << 1, 0, 0,
0, 1, 0,
l(0), l(1), l(2);
std::cout << Hp << std::endl;
Vector3d l0;
Vector3d l1;
Hp_INV = MatrixXd(3, 3);
Hp_INV = Hp.inverse().eval();
std::cout << Hp_INV << std::endl;
}
示例8: test_determinant
void test_determinant()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( determinant(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( determinant(Matrix<double, 2, 2>()) );
CALL_SUBTEST_3( determinant(Matrix<double, 3, 3>()) );
CALL_SUBTEST_4( determinant(Matrix<double, 4, 4>()) );
CALL_SUBTEST_5( determinant(Matrix<std::complex<double>, 10, 10>()) );
CALL_SUBTEST_6( determinant(MatrixXd(20, 20)) );
}
CALL_SUBTEST_6( determinant(MatrixXd(200, 200)) );
}
示例9: test_matrix_power
void test_matrix_power()
{
CALL_SUBTEST_2(test2dRotation<double>(1e-13));
CALL_SUBTEST_1(test2dRotation<float>(2e-5)); // was 1e-5, relaxed for clang 2.8 / linux / x86-64
CALL_SUBTEST_9(test2dRotation<long double>(1e-13));
CALL_SUBTEST_2(test2dHyperbolicRotation<double>(1e-14));
CALL_SUBTEST_1(test2dHyperbolicRotation<float>(1e-5));
CALL_SUBTEST_9(test2dHyperbolicRotation<long double>(1e-14));
CALL_SUBTEST_10(test3dRotation<double>(1e-13));
CALL_SUBTEST_11(test3dRotation<float>(1e-5));
CALL_SUBTEST_12(test3dRotation<long double>(1e-13));
CALL_SUBTEST_2(testGeneral(Matrix2d(), 1e-13));
CALL_SUBTEST_7(testGeneral(Matrix3dRowMajor(), 1e-13));
CALL_SUBTEST_3(testGeneral(Matrix4cd(), 1e-13));
CALL_SUBTEST_4(testGeneral(MatrixXd(8,8), 2e-12));
CALL_SUBTEST_1(testGeneral(Matrix2f(), 1e-4));
CALL_SUBTEST_5(testGeneral(Matrix3cf(), 1e-4));
CALL_SUBTEST_8(testGeneral(Matrix4f(), 1e-4));
CALL_SUBTEST_6(testGeneral(MatrixXf(2,2), 1e-3)); // see bug 614
CALL_SUBTEST_9(testGeneral(MatrixXe(7,7), 1e-13));
CALL_SUBTEST_10(testGeneral(Matrix3d(), 1e-13));
CALL_SUBTEST_11(testGeneral(Matrix3f(), 1e-4));
CALL_SUBTEST_12(testGeneral(Matrix3e(), 1e-13));
CALL_SUBTEST_2(testSingular(Matrix2d(), 1e-13));
CALL_SUBTEST_7(testSingular(Matrix3dRowMajor(), 1e-13));
CALL_SUBTEST_3(testSingular(Matrix4cd(), 1e-13));
CALL_SUBTEST_4(testSingular(MatrixXd(8,8), 2e-12));
CALL_SUBTEST_1(testSingular(Matrix2f(), 1e-4));
CALL_SUBTEST_5(testSingular(Matrix3cf(), 1e-4));
CALL_SUBTEST_8(testSingular(Matrix4f(), 1e-4));
CALL_SUBTEST_6(testSingular(MatrixXf(2,2), 1e-3));
CALL_SUBTEST_9(testSingular(MatrixXe(7,7), 1e-13));
CALL_SUBTEST_10(testSingular(Matrix3d(), 1e-13));
CALL_SUBTEST_11(testSingular(Matrix3f(), 1e-4));
CALL_SUBTEST_12(testSingular(Matrix3e(), 1e-13));
CALL_SUBTEST_2(testLogThenExp(Matrix2d(), 1e-13));
CALL_SUBTEST_7(testLogThenExp(Matrix3dRowMajor(), 1e-13));
CALL_SUBTEST_3(testLogThenExp(Matrix4cd(), 1e-13));
CALL_SUBTEST_4(testLogThenExp(MatrixXd(8,8), 2e-12));
CALL_SUBTEST_1(testLogThenExp(Matrix2f(), 1e-4));
CALL_SUBTEST_5(testLogThenExp(Matrix3cf(), 1e-4));
CALL_SUBTEST_8(testLogThenExp(Matrix4f(), 1e-4));
CALL_SUBTEST_6(testLogThenExp(MatrixXf(2,2), 1e-3));
CALL_SUBTEST_9(testLogThenExp(MatrixXe(7,7), 1e-13));
CALL_SUBTEST_10(testLogThenExp(Matrix3d(), 1e-13));
CALL_SUBTEST_11(testLogThenExp(Matrix3f(), 1e-4));
CALL_SUBTEST_12(testLogThenExp(Matrix3e(), 1e-13));
}
示例10: test_eigen2_eigensolver
void test_eigen2_eigensolver()
{
for(int i = 0; i < g_repeat; i++) {
// very important to test a 3x3 matrix since we provide a special path for it
CALL_SUBTEST_1( selfadjointeigensolver(Matrix3f()) );
CALL_SUBTEST_2( selfadjointeigensolver(Matrix4d()) );
CALL_SUBTEST_3( selfadjointeigensolver(MatrixXf(7,7)) );
CALL_SUBTEST_4( selfadjointeigensolver(MatrixXcd(5,5)) );
CALL_SUBTEST_5( selfadjointeigensolver(MatrixXd(19,19)) );
CALL_SUBTEST_6( eigensolver(Matrix4f()) );
CALL_SUBTEST_5( eigensolver(MatrixXd(17,17)) );
}
}
示例11: moved_I
Derived lidarBoostEngine::apply_optical_flow(const MatrixBase<Derived>& I, std::vector< Derived > uv)
{
Derived moved_I(beta*n, beta*m);
// W = SparseMatrix<double>( beta*n, beta*m );
// W.reserve(VectorXi::Constant(n, m));
// T = SparseMatrix<double>( beta*n, beta*m );
// MatrixXi W( beta*n, beta*m ), T( beta*n, beta*m );
W = MatrixXd( beta*n, beta*m );
int new_i, new_j;
for( int i = 0; i < I.rows(); i++ )
{
for( int j = 0; j < I.cols(); j++ )
{
new_i = round( beta * (i + uv[0](i, j)) );
new_j = round( beta * (j + uv[1](i, j)) );
if(new_i > 0 && new_i < beta*n && new_j > 0 && new_j < beta*m)
{
moved_I(new_i, new_j) = I(i ,j);
W(new_i, new_j) = 1;
}
}
}
return moved_I;
}
示例12: test_redux
void test_redux()
{
// the max size cannot be too large, otherwise reduxion operations obviously generate large errors.
int maxsize = (std::min)(100,EIGEN_TEST_MAX_SIZE);
EIGEN_UNUSED_VARIABLE(maxsize);
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( matrixRedux(Matrix<float, 1, 1>()) );
CALL_SUBTEST_1( matrixRedux(Array<float, 1, 1>()) );
CALL_SUBTEST_2( matrixRedux(Matrix2f()) );
CALL_SUBTEST_2( matrixRedux(Array2f()) );
CALL_SUBTEST_3( matrixRedux(Matrix4d()) );
CALL_SUBTEST_3( matrixRedux(Array4d()) );
CALL_SUBTEST_4( matrixRedux(MatrixXcf(internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
CALL_SUBTEST_4( matrixRedux(ArrayXXcf(internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
CALL_SUBTEST_5( matrixRedux(MatrixXd (internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
CALL_SUBTEST_5( matrixRedux(ArrayXXd (internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
CALL_SUBTEST_6( matrixRedux(MatrixXi (internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
CALL_SUBTEST_6( matrixRedux(ArrayXXi (internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
}
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_7( vectorRedux(Vector4f()) );
CALL_SUBTEST_7( vectorRedux(Array4f()) );
CALL_SUBTEST_5( vectorRedux(VectorXd(internal::random<int>(1,maxsize))) );
CALL_SUBTEST_5( vectorRedux(ArrayXd(internal::random<int>(1,maxsize))) );
CALL_SUBTEST_8( vectorRedux(VectorXf(internal::random<int>(1,maxsize))) );
CALL_SUBTEST_8( vectorRedux(ArrayXf(internal::random<int>(1,maxsize))) );
}
}
示例13: build_pose
// add_odom_xy_global_yprz_t: sets up the pose nodes and then calls the factor adding functions
bool FactorGraph::add_odom_xy_global_yprz(odom_xy_global_yprz_t odom){
if (_poses[odom.id].size()==0){
cout << "Rejecting odometry since no prior" << endl;
return false;
}
Pose3dTS_Node* old_pose = _poses[odom.id].back();
if (_poses[odom.id].back()->ts() != odom.t[0])
cout << "Warning: Time mismatch for last pose time " << _poses[odom.id].back()->ts() << "and received" << odom.t[0] << endl;
// for now just build a new pose and connect it to the last one for that id
Pose3dTS_Node* pose = build_pose(odom.t[1],odom.id);
// manually initialize the new pose since all the factors are partial
pos2_t pos2 = odom.odom_xy.pos2;
rot3_t rot3 = odom.attitude;
depth_t depth = odom.depth;
Pose3d old = (dynamic_cast<Pose3d_Node*>(old_pose))->value();
Pose3d measure(pos2.mu[0], pos2.mu[1],0,0,0,0);
Pose3d p2 = (MatrixXd(old.vector() + measure.vector()));
Pose3d predict(p2.x(),p2.y(),depth.mu,rot3.mu[0],rot3.mu[1],rot3.mu[2]);
pose->init(predict);
// call private factor adding functions
add_odom_xy(odom.odom_xy,old_pose,pose);
add_global_ypr(odom.attitude,pose);
add_global_z(odom.depth,pose);
return true;
}
示例14: test_stdvector
void test_stdvector()
{
// some non vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Vector2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix3f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix3d()));
// some vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Matrix2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Vector4f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix4f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix4d()));
// some dynamic sizes
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXd(1,1)));
CALL_SUBTEST_3(check_stdvector_matrix(VectorXd(20)));
CALL_SUBTEST_3(check_stdvector_matrix(RowVectorXf(20)));
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXcf(10,10)));
// some Transform
CALL_SUBTEST_4(check_stdvector_transform(Projective2f()));
CALL_SUBTEST_4(check_stdvector_transform(Projective3f()));
CALL_SUBTEST_4(check_stdvector_transform(Projective3d()));
//CALL_SUBTEST(heck_stdvector_transform(Projective4d()));
// some Quaternion
CALL_SUBTEST_5(check_stdvector_quaternion(Quaternionf()));
CALL_SUBTEST_5(check_stdvector_quaternion(Quaterniond()));
}
示例15: MatrixXd
void lidarBoostEngine::set_selected_cloud(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, std::vector<int> numOfClouds)
{
sz = cloud->size();
n_cloud = sz/one_pcl_sz;
std::cout << n_cloud << std::endl;
int list_sz = numOfClouds.size();
Y = std::vector< MatrixXd >( list_sz );
std::vector< MatrixXd >eigMat( list_sz );
int num = 0;
// typedef std::vector< MatrixXd >::iterator it_type;
for( int i = 0; i < list_sz; i++ )
{
eigMat[i] = MatrixXd( n, m );
num = numOfClouds[i];
for (int j = 0; j < one_pcl_sz; j++ )
{
eigMat[i]( j/m, j%m ) = cloud->points[ j+num*one_pcl_sz ].z;
}
}
Y = eigMat;
}