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);
}
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)
}
/**
* @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;
}
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();
}
);
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;
}
}
}
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);
}
);
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;
}
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)) );
}
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));
}
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)) );
}
}
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;
}
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))) );
}
}
// 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;
}
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()));
}
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;
}
void test_eigensolver_generalized_real()
{
for(int i = 0; i < g_repeat; i++) {
int s = 0;
CALL_SUBTEST_1( generalized_eigensolver_real(Matrix4f()) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_2( generalized_eigensolver_real(MatrixXd(s,s)) );
// some trivial but implementation-wise tricky cases
CALL_SUBTEST_2( generalized_eigensolver_real(MatrixXd(1,1)) );
CALL_SUBTEST_2( generalized_eigensolver_real(MatrixXd(2,2)) );
CALL_SUBTEST_3( generalized_eigensolver_real(Matrix<double,1,1>()) );
CALL_SUBTEST_4( generalized_eigensolver_real(Matrix2d()) );
TEST_SET_BUT_UNUSED_VARIABLE(s)
}
}
void test_swap()
{
CALL_SUBTEST_1( swap(Matrix3f()) ); // fixed size, no vectorization
CALL_SUBTEST_2( swap(Matrix4d()) ); // fixed size, possible vectorization
CALL_SUBTEST_3( swap(MatrixXd(3,3)) ); // dyn size, no vectorization
CALL_SUBTEST_4( swap(MatrixXf(30,30)) ); // dyn size, possible vectorization
}
void test_product_large()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST( product(MatrixXf(ei_random<int>(1,320), ei_random<int>(1,320))) );
CALL_SUBTEST( product(MatrixXd(ei_random<int>(1,320), ei_random<int>(1,320))) );
CALL_SUBTEST( product(MatrixXi(ei_random<int>(1,320), ei_random<int>(1,320))) );
CALL_SUBTEST( product(MatrixXcf(ei_random<int>(1,50), ei_random<int>(1,50))) );
CALL_SUBTEST( product(Matrix<float,Dynamic,Dynamic,RowMajor>(ei_random<int>(1,320), ei_random<int>(1,320))) );
}
{
// test a specific issue in DiagonalProduct
int N = 1000000;
VectorXf v = VectorXf::Ones(N);
MatrixXf m = MatrixXf::Ones(N,3);
m = (v+v).asDiagonal() * m;
VERIFY_IS_APPROX(m, MatrixXf::Constant(N,3,2));
}
{
// test deferred resizing in Matrix::operator=
MatrixXf a = MatrixXf::Random(10,4), b = MatrixXf::Random(4,10), c = a;
VERIFY_IS_APPROX((a = a * b), (c * b).eval());
}
}
bool WorldSQP::iterative_control_opt(vector<vector<World*> >& trajectory, vector<vector<Control*> >& controls, int num_opts, bool return_best_opt, double threshold)
{
current_states.resize(trajectory.size());
current_jacobians.resize(trajectory.size());
for (int i = 0; i < trajectory.size(); i++) {
current_states[i].resize(trajectory[i].size());
current_jacobians[i].resize(trajectory[i].size());
for (int j = 0; j < trajectory[i].size(); j++) {
current_states[i][j] = new World(*trajectory[i][j]);
current_jacobians[i][j] = MatrixXd();
}
}
assert(controls.size() == _num_worlds-1);
current_controls = controls;
for (int opt_iter = 0; opt_iter < num_opts; opt_iter++) {
solve();
}
trajectory = current_states;
controls = current_controls;
return true;
}
/**
* Constructs the MetropolisHastingsSimulation object.
*
* @param pModel Pointer to the Model.
* @param pData Pointer to the Data.
*/
MetropolisHastingsSimulation::MetropolisHastingsSimulation(Model* pModel,
Data* pData) :
Simulation(pModel, pData), //
lScores(1, VectorXd(nStatistics())), //
lScoreTargets(VectorXd::Zero(nStatistics())), //
lNeedsDerivative(false), //
lDerivative(1, MatrixXd(nParameters(), nParameters())), //
lMeanScoresMinusTargets(), //
lNRunMH(nPeriods()), //
lResult(&lTheta, &lScoreTargets, 0/*ptr*/, &lScores, 0/*ptr*/,
&lMeanScoresMinusTargets, 0/*ptr*/, 0/*ptr*/, &lDerivative, 0/*ptr*/)
{
// Conditional is not possible with maximum likelihood
// R: sienaModelCreate.r sienaModelCreate() (line ~56)
assert(!pModel->conditional());
// Calculate the number of simulation steps per period.
// R: initializeFran.r initializeFRAN() (line ~504)
const MatrixXd targets = calculateTargetStatistics();
const ArrayXb basicRates = rStatisticEffects().array()
== &Simulation::RATE_EFFECT;
const VectorXd zero = VectorXd::Zero(nStatistics());
for (int m = 0; m < nPeriods(); ++m) {
lNRunMH[m] = N_RUN_MH_MULTIPLICATOR_DEFAULT
* basicRates.select(targets.row(m).transpose(), zero).sum();
if (lNRunMH[m] > 100) {
lNRunMH[m] = round((double) lNRunMH[m] / 100.0) * 100;
}
}
}
void test_stdvector_overload()
{
// 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(Affine2f())); // does not need the specialization (2+1)^2 = 9
CALL_SUBTEST_4(check_stdvector_transform(Affine3f()));
CALL_SUBTEST_4(check_stdvector_transform(Affine3d()));
// some Quaternion
CALL_SUBTEST_5(check_stdvector_quaternion(Quaternionf()));
CALL_SUBTEST_5(check_stdvector_quaternion(Quaterniond()));
}
void test_qtvector()
{
// some non vectorizable fixed sizes
CALL_SUBTEST(check_qtvector_matrix(Vector2f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix3f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix3d()));
// some vectorizable fixed sizes
CALL_SUBTEST(check_qtvector_matrix(Matrix2f()));
CALL_SUBTEST(check_qtvector_matrix(Vector4f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix4f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix4d()));
// some dynamic sizes
CALL_SUBTEST(check_qtvector_matrix(MatrixXd(1,1)));
CALL_SUBTEST(check_qtvector_matrix(VectorXd(20)));
CALL_SUBTEST(check_qtvector_matrix(RowVectorXf(20)));
CALL_SUBTEST(check_qtvector_matrix(MatrixXcf(10,10)));
// some Transform
CALL_SUBTEST(check_qtvector_transform(Transform2f()));
CALL_SUBTEST(check_qtvector_transform(Transform3f()));
CALL_SUBTEST(check_qtvector_transform(Transform3d()));
//CALL_SUBTEST(check_qtvector_transform(Transform4d()));
// some Quaternion
CALL_SUBTEST(check_qtvector_quaternion(Quaternionf()));
CALL_SUBTEST(check_qtvector_quaternion(Quaternionf()));
}
void AddSingleTimeLinearPostureConstraint(
const double *t, const RigidBodyConstraint* constraint, int nq,
const drake::solvers::DecisionVariableView& vars,
OptimizationProblem* prog) {
DRAKE_ASSERT(
constraint->getCategory() ==
RigidBodyConstraint::SingleTimeLinearPostureConstraintCategory);
const SingleTimeLinearPostureConstraint* st_lpc =
static_cast<const SingleTimeLinearPostureConstraint*>(constraint);
if (!st_lpc->isTimeValid(t)) { return; }
VectorXd lb;
VectorXd ub;
st_lpc->bounds(t, lb, ub);
VectorXi iAfun;
VectorXi jAvar;
VectorXd A;
st_lpc->geval(t, iAfun, jAvar, A);
DRAKE_ASSERT(iAfun.size() == jAvar.size());
DRAKE_ASSERT(iAfun.size() == A.size());
typedef Eigen::Triplet<double> T;
std::vector<T> triplet_list;
for (int i = 0; i < iAfun.size(); i++) {
triplet_list.push_back(T(iAfun[i], jAvar[i], A[i]));
}
Eigen::SparseMatrix<double> A_sparse(
st_lpc->getNumConstraint(t), nq);
A_sparse.setFromTriplets(triplet_list.begin(), triplet_list.end());
prog->AddLinearConstraint(MatrixXd(A_sparse), lb, ub, {vars});
}
void insertRow(MatrixXd &mat, const VectorXd &row)
{
assert(mat.cols() == row.size());
int rows = mat.rows();
int cols = mat.cols();
mat.noalias() = (MatrixXd(rows+1,cols) << mat, row.transpose()).finished();
}
float64_t CKLDualInferenceMethod::get_derivative_related_cov(SGMatrix<float64_t> dK)
{
Map<MatrixXd> eigen_dK(dK.matrix, dK.num_rows, dK.num_cols);
Map<MatrixXd> eigen_K(m_ktrtr.matrix, m_ktrtr.num_rows, m_ktrtr.num_cols);
Map<VectorXd> eigen_W(m_W.vector, m_W.vlen);
Map<MatrixXd> eigen_L(m_L.matrix, m_L.num_rows, m_L.num_cols);
Map<VectorXd> eigen_sW(m_sW.vector, m_sW.vlen);
Map<MatrixXd> eigen_Sigma(m_Sigma.matrix, m_Sigma.num_rows, m_Sigma.num_cols);
Map<VectorXd> eigen_alpha(m_alpha.vector, m_alpha.vlen);
Map<VectorXd> eigen_dv(m_dv.vector, m_dv.vlen);
Map<VectorXd> eigen_df(m_df.vector, m_df.vlen);
index_t len=m_W.vlen;
//U=inv(L')*diag(sW)
MatrixXd eigen_U=eigen_L.triangularView<Upper>().adjoint().solve(MatrixXd(eigen_sW.asDiagonal()));
//A=I-K*diag(sW)*inv(L)*inv(L')*diag(sW)
Map<MatrixXd> eigen_V(m_V.matrix, m_V.num_rows, m_V.num_cols);
MatrixXd eigen_A=MatrixXd::Identity(len, len)-eigen_V.transpose()*eigen_U;
//AdK = A*dK;
MatrixXd AdK=eigen_A*eigen_dK;
//z = diag(AdK) + sum(A.*AdK,2) - sum(A'.*AdK,1)';
VectorXd z=AdK.diagonal()+(eigen_A.array()*AdK.array()).rowwise().sum().matrix()
-(eigen_A.transpose().array()*AdK.array()).colwise().sum().transpose().matrix();
float64_t result=eigen_alpha.dot(eigen_dK*(eigen_alpha/2.0-eigen_df))-z.dot(eigen_dv);
return result;
}
void test_eigen2_product_large()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( product(MatrixXf(ei_random<int>(1,320), ei_random<int>(1,320))) );
CALL_SUBTEST_2( product(MatrixXd(ei_random<int>(1,320), ei_random<int>(1,320))) );
CALL_SUBTEST_3( product(MatrixXi(ei_random<int>(1,320), ei_random<int>(1,320))) );
CALL_SUBTEST_4( product(MatrixXcf(ei_random<int>(1,50), ei_random<int>(1,50))) );
CALL_SUBTEST_5( product(Matrix<float,Dynamic,Dynamic,RowMajor>(ei_random<int>(1,320), ei_random<int>(1,320))) );
}
#ifdef EIGEN_TEST_PART_6
{
// test a specific issue in DiagonalProduct
int N = 1000000;
VectorXf v = VectorXf::Ones(N);
MatrixXf m = MatrixXf::Ones(N,3);
m = (v+v).asDiagonal() * m;
VERIFY_IS_APPROX(m, MatrixXf::Constant(N,3,2));
}
{
// test deferred resizing in Matrix::operator=
MatrixXf a = MatrixXf::Random(10,4), b = MatrixXf::Random(4,10), c = a;
VERIFY_IS_APPROX((a = a * b), (c * b).eval());
}
{
MatrixXf mat1(10,10); mat1.setRandom();
MatrixXf mat2(32,10); mat2.setRandom();
MatrixXf result = mat1.row(2)*mat2.transpose();
VERIFY_IS_APPROX(result, (mat1.row(2)*mat2.transpose()).eval());
}
#endif
}
void WorldSQP::initializeClosedLoopStepper(World* start, vector<vector<World*> >& target) {
VectorXd state;
world_to_state(start, state);
resize_controller(target[0].size() + 1, state.size(), target.size());
current_states.resize(target.size());
current_jacobians.resize(target.size());
current_controls.resize(target[0].size());
#pragma omp parallel for
for (int i = 0; i < target.size(); i++) {
current_states[i].resize(target[i].size());
current_jacobians[i].resize(target[i].size());
for (int j = 0; j < target[i].size(); j++) {
current_states[i][j] = new World(*target[i][j]);
current_jacobians[i][j] = MatrixXd();
}
}
for (int k = 0; k < target[0].size(); k++) {
current_controls[k].resize(2);
for (int j = 0; j < 2; j++) { //hack
current_controls[k][j] = new Control();
}
}
pushStart(start);
}
void test_nullary()
{
CALL_SUBTEST_1( testMatrixType(Matrix2d()) );
CALL_SUBTEST_2( testMatrixType(MatrixXcf(internal::random<int>(1,300),internal::random<int>(1,300))) );
CALL_SUBTEST_3( testMatrixType(MatrixXf(internal::random<int>(1,300),internal::random<int>(1,300))) );
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_4( testVectorType(VectorXd(internal::random<int>(1,300))) );
CALL_SUBTEST_5( testVectorType(Vector4d()) ); // regression test for bug 232
CALL_SUBTEST_6( testVectorType(Vector3d()) );
CALL_SUBTEST_7( testVectorType(VectorXf(internal::random<int>(1,300))) );
CALL_SUBTEST_8( testVectorType(Vector3f()) );
CALL_SUBTEST_8( testVectorType(Vector4f()) );
CALL_SUBTEST_8( testVectorType(Matrix<float,8,1>()) );
CALL_SUBTEST_8( testVectorType(Matrix<float,1,1>()) );
CALL_SUBTEST_9( testVectorType(VectorXi(internal::random<int>(1,300))) );
CALL_SUBTEST_9( testVectorType(Matrix<int,1,1>()) );
}
#ifdef EIGEN_TEST_PART_6
// Assignment of a RowVectorXd to a MatrixXd (regression test for bug #79).
VERIFY( (MatrixXd(RowVectorXd::LinSpaced(3, 0, 1)) - RowVector3d(0, 0.5, 1)).norm() < std::numeric_limits<double>::epsilon() );
#endif
}
void test_bdcsvd()
{
CALL_SUBTEST_3(( svd_verify_assert<BDCSVD<Matrix3f> >(Matrix3f()) ));
CALL_SUBTEST_4(( svd_verify_assert<BDCSVD<Matrix4d> >(Matrix4d()) ));
CALL_SUBTEST_7(( svd_verify_assert<BDCSVD<MatrixXf> >(MatrixXf(10,12)) ));
CALL_SUBTEST_8(( svd_verify_assert<BDCSVD<MatrixXcd> >(MatrixXcd(7,5)) ));
CALL_SUBTEST_101(( svd_all_trivial_2x2(bdcsvd<Matrix2cd>) ));
CALL_SUBTEST_102(( svd_all_trivial_2x2(bdcsvd<Matrix2d>) ));
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_3(( bdcsvd<Matrix3f>() ));
CALL_SUBTEST_4(( bdcsvd<Matrix4d>() ));
CALL_SUBTEST_5(( bdcsvd<Matrix<float,3,5> >() ));
int r = internal::random<int>(1, EIGEN_TEST_MAX_SIZE/2),
c = internal::random<int>(1, EIGEN_TEST_MAX_SIZE/2);
TEST_SET_BUT_UNUSED_VARIABLE(r)
TEST_SET_BUT_UNUSED_VARIABLE(c)
CALL_SUBTEST_6(( bdcsvd(Matrix<double,Dynamic,2>(r,2)) ));
CALL_SUBTEST_7(( bdcsvd(MatrixXf(r,c)) ));
CALL_SUBTEST_7(( compare_bdc_jacobi(MatrixXf(r,c)) ));
CALL_SUBTEST_10(( bdcsvd(MatrixXd(r,c)) ));
CALL_SUBTEST_10(( compare_bdc_jacobi(MatrixXd(r,c)) ));
CALL_SUBTEST_8(( bdcsvd(MatrixXcd(r,c)) ));
CALL_SUBTEST_8(( compare_bdc_jacobi(MatrixXcd(r,c)) ));
// Test on inf/nan matrix
CALL_SUBTEST_7( (svd_inf_nan<BDCSVD<MatrixXf>, MatrixXf>()) );
CALL_SUBTEST_10( (svd_inf_nan<BDCSVD<MatrixXd>, MatrixXd>()) );
}
// test matrixbase method
CALL_SUBTEST_1(( bdcsvd_method<Matrix2cd>() ));
CALL_SUBTEST_3(( bdcsvd_method<Matrix3f>() ));
// Test problem size constructors
CALL_SUBTEST_7( BDCSVD<MatrixXf>(10,10) );
// Check that preallocation avoids subsequent mallocs
// Disbaled because not supported by BDCSVD
// CALL_SUBTEST_9( svd_preallocate<void>() );
CALL_SUBTEST_2( svd_underoverflow<void>() );
}
void testVectorType(const VectorType& base)
{
typedef typename internal::traits<VectorType>::Index Index;
typedef typename internal::traits<VectorType>::Scalar Scalar;
const Index size = base.size();
Scalar high = internal::random<Scalar>(-500,500);
Scalar low = (size == 1 ? high : internal::random<Scalar>(-500,500));
if (low>high) std::swap(low,high);
const Scalar step = ((size == 1) ? 1 : (high-low)/(size-1));
// check whether the result yields what we expect it to do
VectorType m(base);
m.setLinSpaced(size,low,high);
VectorType n(size);
for (int i=0; i<size; ++i)
n(i) = low+i*step;
VERIFY_IS_APPROX(m,n);
// random access version
m = VectorType::LinSpaced(size,low,high);
VERIFY_IS_APPROX(m,n);
// Assignment of a RowVectorXd to a MatrixXd (regression test for bug #79).
VERIFY( (MatrixXd(RowVectorXd::LinSpaced(3, 0, 1)) - RowVector3d(0, 0.5, 1)).norm() < std::numeric_limits<Scalar>::epsilon() );
// These guys sometimes fail! This is not good. Any ideas how to fix them!?
//VERIFY( m(m.size()-1) == high );
//VERIFY( m(0) == low );
// sequential access version
m = VectorType::LinSpaced(Sequential,size,low,high);
VERIFY_IS_APPROX(m,n);
// These guys sometimes fail! This is not good. Any ideas how to fix them!?
//VERIFY( m(m.size()-1) == high );
//VERIFY( m(0) == low );
// check whether everything works with row and col major vectors
Matrix<Scalar,Dynamic,1> row_vector(size);
Matrix<Scalar,1,Dynamic> col_vector(size);
row_vector.setLinSpaced(size,low,high);
col_vector.setLinSpaced(size,low,high);
VERIFY( row_vector.isApprox(col_vector.transpose(), NumTraits<Scalar>::epsilon()));
Matrix<Scalar,Dynamic,1> size_changer(size+50);
size_changer.setLinSpaced(size,low,high);
VERIFY( size_changer.size() == size );
typedef Matrix<Scalar,1,1> ScalarMatrix;
ScalarMatrix scalar;
scalar.setLinSpaced(1,low,high);
VERIFY_IS_APPROX( scalar, ScalarMatrix::Constant(high) );
VERIFY_IS_APPROX( ScalarMatrix::LinSpaced(1,low,high), ScalarMatrix::Constant(high) );
}