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)
}
Пример #3
0
/**
 * @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
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)) );

    // 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
0
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
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);
  }
  );
Пример #7
0
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
0
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));
}
Пример #10
0
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
0
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
0
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
0
// 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
0
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
0
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)
    }
}
Пример #17
0
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
}
Пример #18
0
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());
  }
}
Пример #19
0
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;
}
Пример #20
0
/**
 * 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;
		}
	}
}
Пример #21
0
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()));
}
Пример #22
0
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()));
}
Пример #23
0
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});
}
Пример #24
0
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();
}
Пример #25
0
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;
}
Пример #26
0
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
}
Пример #27
0
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);
}
Пример #28
0
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>() );
}
Пример #30
0
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) );
}