Example #1
0
void cv2eigen( const Matx<_Tp, 1, _cols>& src,
               Eigen::Matrix<_Tp, 1, Eigen::Dynamic>& dst )
{
    dst.resize(_cols);
    if( !(dst.Flags & Eigen::RowMajorBit) )
    {
        Mat _dst(_cols, 1, DataType<_Tp>::type,
                 dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
        transpose(src, _dst);
        CV_DbgAssert(_dst.data == (uchar*)dst.data());
    }
    else
    {
        Mat _dst(1, _cols, DataType<_Tp>::type,
                 dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
        Mat(src).copyTo(_dst);
        CV_DbgAssert(_dst.data == (uchar*)dst.data());
    }
}
Example #2
0
template <typename PointT, typename Scalar> inline void
pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 
                        const std::vector<int> &indices,
                        Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
{
  typedef typename pcl::traits::fieldList<PointT>::type FieldList;

  // Get the size of the fields
  centroid.setZero (boost::mpl::size<FieldList>::value);

  if (indices.empty ())
    return;
  // Iterate over each point
  int nr_points = static_cast<int> (indices.size ());
  for (int i = 0; i < nr_points; ++i)
  {
    // Iterate over each dimension
    pcl::for_each_type <FieldList> (NdCentroidFunctor<PointT, Scalar> (cloud[indices[i]], centroid));
  }
  centroid /= static_cast<Scalar> (nr_points);
}
Example #3
0
    Eigen::Matrix<T, Eigen::Dynamic, 1>
    positive_ordered_free(const Eigen::Matrix<T, Eigen::Dynamic, 1>& y) {
      using Eigen::Matrix;
      using Eigen::Dynamic;
      using stan::math::index_type;

      typedef typename index_type<Matrix<T, Dynamic, 1> >::type size_type;

      stan::math::check_positive_ordered("stan::math::positive_ordered_free",
                                                   "Positive ordered variable",
                                                   y);

      size_type k = y.size();
      Matrix<T, Dynamic, 1> x(k);
      if (k == 0)
        return x;
      x[0] = log(y[0]);
      for (size_type i = 1; i < k; ++i)
        x[i] = log(y[i] - y[i-1]);
      return x;
    }
TEST(ErrorHandlingMatrix, checkMatchingDimsMatrix) {
  Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> y;
  Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> x;
  
  y.resize(3,3);
  x.resize(3,3);
  EXPECT_TRUE(stan::math::check_matching_dims("checkMatchingDims", "x", x,
                                                        "y", y));
  x.resize(0,0);
  y.resize(0,0);
  EXPECT_TRUE(stan::math::check_matching_dims("checkMatchingDims", "x", x,
                                                        "y", y));

  y.resize(1,2);
  EXPECT_THROW(stan::math::check_matching_dims("checkMatchingDims", "x", x,
                                                         "y", y), 
               std::invalid_argument);

  x.resize(2,1);
  EXPECT_THROW(stan::math::check_matching_dims("checkMatchingDims", "x", x,
                                                         "y", y), 
               std::invalid_argument);
}
Example #5
0
 inline 
 Eigen::Matrix<fvar<T>,R1,C2> 
 multiply(const Eigen::Matrix<fvar<T>,R1,C1>& m1,
          const Eigen::Matrix<double,R2,C2>& m2) {
   stan::math::validate_multiplicable(m1,m2,"multiply");
   Eigen::Matrix<fvar<T>,R1,C2>
     result(m1.rows(),m2.cols());
   for (size_type i = 0; i < m1.rows(); i++) {
     Eigen::Matrix<fvar<T>,1,C1> crow = m1.row(i);
     for (size_type j = 0; j < m2.cols(); j++) {
       Eigen::Matrix<double,R2,1> ccol = m2.col(j);
       result(i,j) = stan::agrad::dot_product(crow,ccol);
     }
   }
   return result;
 }
TEST(ErrorHandlingMatrix, checkMultiplicableMatrix) {
  Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> y;
  Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> x;
  
  y.resize(3,3);
  x.resize(3,3);
  EXPECT_TRUE(stan::math::check_multiplicable("checkMultiplicable", "x", x,
                                                        "y", y));
  x.resize(3,2);
  y.resize(2,4);
  EXPECT_TRUE(stan::math::check_multiplicable("checkMultiplicable", "x", x,
                                                        "y", y));

  y.resize(1,2);
  EXPECT_THROW(stan::math::check_multiplicable("checkMultiplicable", "x", x,
                                                         "y", y), 
               std::invalid_argument);

  x.resize(2,2);
  EXPECT_THROW(stan::math::check_multiplicable("checkMultiplicable", "x", x,
                                                         "y", y), 
               std::invalid_argument);
}
Example #7
0
template <typename PointT, typename Scalar> void
pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
                          const std::vector<int> &indices, 
                          pcl::PointCloud<PointT> &cloud_out,
                          const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
  size_t npts = indices.size ();
  // In order to transform the data, we need to remove NaNs
  cloud_out.is_dense = cloud_in.is_dense;
  cloud_out.header   = cloud_in.header;
  cloud_out.width    = static_cast<int> (npts);
  cloud_out.height   = 1;
  cloud_out.points.resize (npts);

  if (cloud_in.is_dense)
  {
    // If the dataset is dense, simply transform it!
    for (size_t i = 0; i < npts; ++i)
    {
      // Copy fields first, then transform xyz data
      //cloud_out.points[i] = cloud_in.points[indices[i]]; 
      //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
      Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
      cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
      cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
      cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
    }
  }
  else
  {
    // Dataset might contain NaNs and Infs, so check for them first,
    // otherwise we get errors during the multiplication (?)
    for (size_t i = 0; i < npts; ++i)
    {
      if (!pcl_isfinite (cloud_in.points[indices[i]].x) || 
          !pcl_isfinite (cloud_in.points[indices[i]].y) || 
          !pcl_isfinite (cloud_in.points[indices[i]].z))
        continue;
      //cloud_out.points[i] = cloud_in.points[indices[i]]; 
      //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
      Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
      cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
      cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
      cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
    }
  }
}
Example #8
0
Eigen::Matrix<T, -1, 1> getPolynomialVariables(const Eigen::Matrix<T, -1, 1> &vars,
                                               const size_t & degree)
{


    typedef Eigen::Matrix<T, -1, 1> Vector;
    typedef Eigen::Matrix<T, -1, -1, Eigen::RowMajor> Matrix;

    auto expand_to_degree = [](const float &x, const int &degree)
    {
        Vector out(degree+1);

        for (int i = 0; i <= degree ; ++i)
            out(i)  = pow(x, i);

        return out;
    };


    Vector current (1);
    current << 1;

    for (int i = 0; i < vars.rows() ; ++i)
    {
        // we expand to the given degree the variable
        Vector expanded = expand_to_degree(vars(i), degree);

        Matrix mul = current * expanded.transpose();


        current.resize(mul.size());

        current = Eigen::Map<Vector> (mul.data(), mul.size());

    }

    return current;

}
Example #9
0
 inline typename boost::disable_if<boost::is_same<I, index_uni>, void>::type
 assign(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& x,
        const cons_index_list<I, nil_index_list>& idxs,
        const Eigen::Matrix<U, Eigen::Dynamic, Eigen::Dynamic>& y,
        const char* name = "ANON", int depth = 0) {
   int x_idx_rows = rvalue_index_size(idxs.head_, x.rows());
   math::check_size_match("matrix[multi] assign row sizes",
                          "lhs", x_idx_rows,
                          name, y.rows());
   math::check_size_match("matrix[multi] assign col sizes",
                          "lhs", x.cols(),
                          name, y.cols());
   for (int i = 0; i < y.rows(); ++i) {
     int m = rvalue_at(i, idxs.head_);
     math::check_range("matrix[multi] assign range", name, x.rows(), m);
     // recurse to allow double to var assign
     for (int j = 0; j < x.cols(); ++j)
       x(m - 1, j) = y(i, j);
   }
 }
Example #10
0
		void
		MitsubishiH7::setMotorPulse(const ::Eigen::Matrix< ::std::int32_t, ::Eigen::Dynamic, 1>& p)
		{
			assert(p.size() >= this->getDof());
			
			this->out.dat2.pls.p1 = 0;
			this->out.dat2.pls.p2 = 0;
			this->out.dat2.pls.p3 = 0;
			this->out.dat2.pls.p4 = 0;
			this->out.dat2.pls.p5 = 0;
			this->out.dat2.pls.p6 = 0;
			this->out.dat2.pls.p7 = 0;
			this->out.dat2.pls.p8 = 0;
			
			switch (this->getDof())
			{
			case 8:
				this->out.dat2.pls.p8 = p(7);
			case 7:
				this->out.dat2.pls.p7 = p(6);
			case 6:
				this->out.dat2.pls.p6 = p(5);
			case 5:
				this->out.dat2.pls.p5 = p(4);
			case 4:
				this->out.dat2.pls.p4 = p(3);
			case 3:
				this->out.dat2.pls.p3 = p(2);
			case 2:
				this->out.dat2.pls.p2 = p(1);
			case 1:
				this->out.dat2.pls.p1 = p(0);
			default:
				break;
			}
			
			this->out.command = MXT_COMMAND_MOVE;
			this->out.sendType = MXT_TYPE_PULSE;
		}
Example #11
0
void check_velocities(const double vmin_[N_MOTORS], const double vmax_[N_MOTORS], const Eigen::Matrix <double,
                      N_SEGMENTS, N_MOTORS> & m3w_, const Eigen::Matrix <double, N_SEGMENTS, N_MOTORS> & m2w_, const Eigen::Matrix <
                      double, N_SEGMENTS, N_MOTORS> & m1w_)
{
#if 0
    cout << "vmin:\n";
    for (int mtr = 0; mtr < N_MOTORS; ++mtr) {
        cout << vmin_[mtr] << " ";
    }
    cout << endl;
    cout << "vmax:\n";
    for (int mtr = 0; mtr < N_MOTORS; ++mtr) {
        cout << vmax_[mtr] << " ";
    }
    cout << endl;
#endif

    // Compute extreme velocities for all segments and motors (at once! - mi low eigen;)).
    Eigen::Matrix <double, N_SEGMENTS, N_MOTORS> v_extremum = (m2w_.cwise() * m2w_).cwise() / (3.0 * m3w_) + m1w_;
    // Correct all NANs.
    for (int sgt = 0; sgt < N_SEGMENTS; ++sgt)
        for (int mtr = 0; mtr < N_MOTORS; ++mtr) {
            if (m3w_(sgt, mtr) == 0.0)
                v_extremum(sgt, mtr) = m1w_(sgt, mtr);
        }

#if 0
    cout << "v_extremum:\n" << v_extremum << endl;
#endif

    // Check conditions for all segments and motors.
    for (int sgt = 0; sgt < N_SEGMENTS; ++sgt)
        for (int mtr = 0; mtr < N_MOTORS; ++mtr) {
            if (v_extremum(sgt, mtr) > vmax_[mtr])
                BOOST_THROW_EXCEPTION(nfe_motor_velocity_constraint_exceeded() << constraint_type(MAXIMUM_CONSTRAINT) << motor_number(mtr) << segment_number(sgt) << constraint_value(vmax_[mtr]) << desired_value(v_extremum(sgt, mtr)));
            else if (v_extremum(sgt, mtr) < vmin_[mtr])
                BOOST_THROW_EXCEPTION(nfe_motor_velocity_constraint_exceeded() << constraint_type(MINIMUM_CONSTRAINT) << motor_number(mtr) << segment_number(sgt) << constraint_value(vmin_[mtr]) << desired_value(v_extremum(sgt, mtr)));
        }
}
void PositionCommand::getGoal(const geometry_msgs::PoseStamped::ConstPtr & goal)
{

    Eigen::Matrix<double,3,1> euler = Eigen::Quaterniond(goal->pose.orientation.w,
                                                         goal->pose.orientation.x,
                                                         goal->pose.orientation.y,
                                                         goal->pose.orientation.z).matrix().eulerAngles(2, 1, 0);
    double yaw   = euler(0,0);
    double pitch = euler(1,0);
    double roll  = euler(2,0);

    goal_pose_ <<   goal->pose.position.x,
            goal->pose.position.y,
            goal->pose.position.z,
            roll,
            pitch,
            yaw;

    ROS_INFO("GOT NEW GOAL");
    std::cout << "goal_pose: " << goal_pose_.transpose()<< std::endl;

}
Example #13
0
template <typename PointT, typename Scalar> void
pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
                          pcl::PointCloud<PointT> &cloud_out,
                          const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
  if (&cloud_in != &cloud_out)
  {
    // Note: could be replaced by cloud_out = cloud_in
    cloud_out.header   = cloud_in.header;
    cloud_out.is_dense = cloud_in.is_dense;
    cloud_out.width    = cloud_in.width;
    cloud_out.height   = cloud_in.height;
    cloud_out.points.reserve (cloud_out.points.size ());
    cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
  }

  if (cloud_in.is_dense)
  {
    // If the dataset is dense, simply transform it!
    for (size_t i = 0; i < cloud_out.points.size (); ++i)
    {
      //cloud_out.points[i].getVector3fMap () = transform * cloud_in.points[i].getVector3fMap ();
      Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
      cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
      cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
      cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
    }
  }
  else
  {
    // Dataset might contain NaNs and Infs, so check for them first,
    // otherwise we get errors during the multiplication (?)
    for (size_t i = 0; i < cloud_out.points.size (); ++i)
    {
      if (!pcl_isfinite (cloud_in.points[i].x) || 
          !pcl_isfinite (cloud_in.points[i].y) || 
          !pcl_isfinite (cloud_in.points[i].z))
        continue;
      //cloud_out.points[i].getVector3fMap () = transform * cloud_in.points[i].getVector3fMap ();
      Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
      cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
      cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
      cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
    }
  }
}
Example #14
0
    inline 
    Eigen::Matrix<fvar<T>,R1,C2>
    mdivide_right(const Eigen::Matrix<fvar<T>,R1,C1> &A,
                  const Eigen::Matrix<double,R2,C2> &b) {
      
      using stan::math::multiply;      
      using stan::math::mdivide_right;
      stan::math::validate_square(b,"mdivide_right");
      stan::math::validate_multiplicable(A,b,"mdivide_right");

      Eigen::Matrix<T,R2,C2> deriv_b_mult_inv_b(b.rows(),b.cols());
      Eigen::Matrix<T,R1,C1> val_A(A.rows(),A.cols()); 
      Eigen::Matrix<T,R1,C1> deriv_A(A.rows(),A.cols());

      for (int j = 0; j < A.cols(); j++) {
        for(int i = 0; i < A.rows(); i++) {
          val_A(i,j) = A(i,j).val_;
          deriv_A(i,j) = A(i,j).d_;
        }
      }

      return stan::agrad::to_fvar(mdivide_right(val_A, b), 
                                  mdivide_right(deriv_A, b));
    }
Example #15
0
 /* ctor for cholesky function
  *
  * Stores varis for A
  * Instantiates and stores varis for L
  * Instantiates and stores dummy vari for
  * upper triangular part of var result returned
  * in cholesky_decompose function call
  *
  * variRefL aren't on the chainable
  * autodiff stack, only used for storage
  * and computation. Note that varis for
  * L are constructed externally in
  * cholesky_decompose.
  *
  * @param matrix A
  * @param matrix L, cholesky factor of A
  * */
 cholesky_decompose_v_vari(const Eigen::Matrix<var, -1, -1>& A,
                           const Eigen::Matrix<double, -1, -1>& L_A)
   : vari(0.0),
     M_(A.rows()),
     variRefA_(ChainableStack::memalloc_.alloc_array<vari*>
               (A.rows() * (A.rows() + 1) / 2)),
     variRefL_(ChainableStack::memalloc_.alloc_array<vari*>
               (A.rows() * (A.rows() + 1) / 2)) {
   size_t accum = 0;
   size_t accum_i = accum;
   for (size_type j = 0; j < M_; ++j) {
     for (size_type i = j; i < M_; ++i) {
       accum_i += i;
       size_t pos = j + accum_i;
       variRefA_[pos] = A.coeffRef(i, j).vi_;
       variRefL_[pos] = new vari(L_A.coeffRef(i, j), false);
     }
     accum += j;
     accum_i = accum;
   }
 }
Example #16
0
void Element::CalculateStiffnessMatrix(const Eigen::Matrix3f& D, std::vector<Eigen::Triplet<float> >& triplets)
{
	Eigen::Vector3f x, y;
	x << nodesX[nodesIds[0]], nodesX[nodesIds[1]], nodesX[nodesIds[2]];
	y << nodesY[nodesIds[0]], nodesY[nodesIds[1]], nodesY[nodesIds[2]];
	
	Eigen::Matrix3f C;
	C << Eigen::Vector3f(1.0f, 1.0f, 1.0f), x, y;
	
	Eigen::Matrix3f IC = C.inverse();

	for (int i = 0; i < 3; i++)
	{
		B(0, 2 * i + 0) = IC(1, i);
		B(0, 2 * i + 1) = 0.0f;
		B(1, 2 * i + 0) = 0.0f;
		B(1, 2 * i + 1) = IC(2, i);
		B(2, 2 * i + 0) = IC(2, i);
		B(2, 2 * i + 1) = IC(1, i);
	}
	Eigen::Matrix<float, 6, 6> K = B.transpose() * D * B * C.determinant() / 2.0f;

	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			Eigen::Triplet<float> trplt11(2 * nodesIds[i] + 0, 2 * nodesIds[j] + 0, K(2 * i + 0, 2 * j + 0));
			Eigen::Triplet<float> trplt12(2 * nodesIds[i] + 0, 2 * nodesIds[j] + 1, K(2 * i + 0, 2 * j + 1));
			Eigen::Triplet<float> trplt21(2 * nodesIds[i] + 1, 2 * nodesIds[j] + 0, K(2 * i + 1, 2 * j + 0));
			Eigen::Triplet<float> trplt22(2 * nodesIds[i] + 1, 2 * nodesIds[j] + 1, K(2 * i + 1, 2 * j + 1));

			triplets.push_back(trplt11);
			triplets.push_back(trplt12);
			triplets.push_back(trplt21);
			triplets.push_back(trplt22);
		}
	}
}
Example #17
0
    inline Eigen::VectorXd
    multi_student_t_rng(const double nu,
                        const Eigen::Matrix<double, Dynamic, 1>& mu,
                        const Eigen::Matrix<double, Dynamic, Dynamic>& s,
                        RNG& rng) {
      static const char* function("stan::prob::multi_student_t_rng");

      using stan::math::check_finite;
      using stan::math::check_not_nan;
      using stan::math::check_symmetric;
      using stan::math::check_positive;

      check_finite(function, "Location parameter", mu);
      check_symmetric(function, "Scale parameter", s);
      check_not_nan(function, "Degrees of freedom parameter", nu);
      check_positive(function, "Degrees of freedom parameter", nu);

      Eigen::VectorXd z(s.cols());
      z.setZero();

      double w = stan::prob::inv_gamma_rng(nu / 2, nu / 2, rng);
      return mu + std::sqrt(w) * stan::prob::multi_normal_rng(z, s, rng);
    }
Example #18
0
TEST_F(MaterialLibSolidsKelvinVector6, DeviatoricSphericalProjections)
{
    auto const& P_dev = Invariants<size>::deviatoric_projection;
    auto const& P_sph = Invariants<size>::spherical_projection;

    // Test product P_dev * P_sph is zero.
    Eigen::Matrix<double, 6, 6> const P_dev_P_sph = P_dev * P_sph;
    EXPECT_LT(P_dev_P_sph.norm(), std::numeric_limits<double>::epsilon());
    EXPECT_LT(P_dev_P_sph.maxCoeff(), std::numeric_limits<double>::epsilon());

    // Test product P_sph * P_dev is zero.
    Eigen::Matrix<double, 6, 6> const P_sph_P_dev = P_sph * P_dev;
    EXPECT_LT(P_sph_P_dev.norm(), std::numeric_limits<double>::epsilon());
    EXPECT_LT(P_sph_P_dev.maxCoeff(), std::numeric_limits<double>::epsilon());

    // Test sum is identity.
    EXPECT_EQ(P_dev + P_sph, (Eigen::Matrix<double, size, size>::Identity()));
}
Example #19
0
void cv2eigen( const Matx<_Tp, _rows, _cols>& src,
               Eigen::Matrix<_Tp, _rows, _cols, _options, _maxRows, _maxCols>& dst )
{
    if( !(dst.Flags & Eigen::RowMajorBit) )
    {
        Mat _dst(_cols, _rows, DataType<_Tp>::type,
                 dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
        transpose(src, _dst);
        CV_DbgAssert(_dst.data == (uchar*)dst.data());
    }
    else
    {
        Mat _dst(_rows, _cols, DataType<_Tp>::type,
                 dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
        Mat(src).copyTo(_dst);
        CV_DbgAssert(_dst.data == (uchar*)dst.data());
    }
}
Example #20
0
void CameraDirectLinearTransformation::decomposePMatrix(const Eigen::Matrix<double,3,4> &P)
{
    Matrix3d M = P.topLeftCorner<3,3>();
    Vector3d m3 = M.row(2).transpose();
    // Follow the HartleyZisserman - "Multiple view geometry in computer vision" implementation chapter 3

    Matrix3d P123,P023,P013,P012;
    P123 << P.col(1),P.col(2),P.col(3);
    P023 << P.col(0),P.col(2),P.col(3);
    P013 << P.col(0),P.col(1),P.col(3);
    P012 << P.col(0),P.col(1),P.col(2);

    double X = P123.determinant();
    double Y = -P023.determinant();
    double Z = P013.determinant();
    double T = -P012.determinant();
    C << X/T,Y/T,Z/T;

    // Image Principal points computed with homogeneous normalization
    this->principalPoint = (M*m3).eval().hnormalized().head<2>();

    // Principal vector  from the camera centre C through pp pointing out from the camera.  This may not be the same as  R(:,3)
    // if the principal point is not at the centre of the image, but it should be similar.
    this->principalVector  =  (M.determinant()*m3).normalized();
    this->R = this->K = Matrix3d::Identity();
    this->rq3(M,this->K,this->R);
    // To understand how K is formed http://ksimek.github.io/2013/08/13/intrinsic/
    // and also read http://ksimek.github.io/2012/08/14/decompose/
    K/=K(2,2); // EXTREMELY IMPORTANT TO MAKE THE K(2,2)==1 !!!

    // K = [ fx, s, x0;
    //       0, fy, y0;
    //       0,  0,  1];
    // Where fx is the focal length on x measured in pixels, fy is the focal length ony  measured in pixels

    // Negate the second column of K and R because the y window coordinates and camera y direction are opposite is positive
    // This is the solution I've found personally to correct the behaviour using OpenGL gluPerspective convention
    this->R.row(2)=-R.row(2);
    // Our 3x3 intrinsic camera matrix K needs two modifications before it's ready to use in OpenGL. First, for proper clipping, the (3,3) element of K must be -1. OpenGL's camera looks down the negative z-axis, so if K33 is positive, vertices in front of the camera will have a negative w coordinate after projection. In principle, this is okay, but because of how OpenGL performs clipping, all of these points will be clipped.
    //this->K.col(2) = -K.col(2);

    // t is the location of the world origin in camera coordinates.
    t = -R*C;
}
Example #21
0
    typename boost::math::tools::promote_args<T_prob>::type
    categorical_logit_log(int n, 
                          const Eigen::Matrix<T_prob,Eigen::Dynamic,1>& beta) {
      static const char* function = "stan::prob::categorical_logit_log(%1%)";

      using stan::math::check_bounded;
      using stan::math::check_finite;
      using stan::math::log_sum_exp;

      double lp = 0.0;
      if (!check_bounded(function, n, 1, beta.size(),
                         "categorical outcome out of support",
                         &lp))
        return lp;

      if (!check_finite(function, beta, "log odds parameter", &lp))
        return lp;

      if (!include_summand<propto,T_prob>::value)
        return 0.0;
        
      // FIXME:  wasteful vs. creating term (n-1) if not vectorized
      return beta(n-1) - log_sum_exp(beta); // == log_softmax(beta)(n-1);
    }
Example #22
0
 void
 hessian(const F& f,
         const Eigen::Matrix<T, Eigen::Dynamic, 1>& x,
         T& fx,
         Eigen::Matrix<T, Eigen::Dynamic, 1>& grad,
         Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& H) {
   H.resize(x.size(), x.size());
   grad.resize(x.size());
   Eigen::Matrix<fvar<fvar<T> >, Eigen::Dynamic, 1> x_fvar(x.size());
   for (int i = 0; i < x.size(); ++i) {
     for (int j = i; j < x.size(); ++j) {
       for (int k = 0; k < x.size(); ++k)
         x_fvar(k) = fvar<fvar<T> >(fvar<T>(x(k), j == k),
                                    fvar<T>(i == k, 0));
       fvar<fvar<T> > fx_fvar = f(x_fvar);
       if (j == 0)
         fx = fx_fvar.val_.val_;
       if (i == j)
         grad(i) = fx_fvar.d_.val_;
       H(i, j) = fx_fvar.d_.d_;
       H(j, i) = H(i, j);
     }
   }
 }
Example #23
0
 void
 jacobian(const F& f,
          const Eigen::Matrix<T, Dynamic, 1>& x,
          Eigen::Matrix<T, Dynamic, 1>& fx,
          Eigen::Matrix<T, Dynamic, Dynamic>& J) {
   using Eigen::Matrix;
   using stan::agrad::fvar;
   Matrix<fvar<T>, Dynamic, 1> x_fvar(x.size());
   for (int i = 0; i < x.size(); ++i) {
     for (int k = 0; k < x.size(); ++k)
       x_fvar(k) = fvar<T>(x(k), i == k);
     Matrix<fvar<T>, Dynamic, 1> fx_fvar
       = f(x_fvar);
     if (i == 0) {
       J.resize(x.size(), fx_fvar.size());
       fx.resize(fx_fvar.size());
       for (int k = 0; k < fx_fvar.size(); ++k)
         fx(k) = fx_fvar(k).val_;
     }
     for (int k = 0; k < fx_fvar.size(); ++k) {
       J(i, k) = fx_fvar(k).d_;
     }
   }
 }
Example #24
0
    typename boost::math::tools::promote_args<T_prob>::type
    categorical_logit_log(const std::vector<int>& ns, 
                          const Eigen::Matrix<T_prob,Eigen::Dynamic,1>& beta) {
      static const char* function = "stan::prob::categorical_logit_log(%1%)";

      using stan::math::check_bounded;
      using stan::math::check_finite;
      using stan::math::log_softmax;
      using stan::math::sum;

      double lp = 0.0;
      for (size_t k = 0; k < ns.size(); ++k)
        if (!check_bounded(function, ns[k], 1, beta.size(),
                           "categorical outcome out of support",
                           &lp))
          return lp;

      if (!check_finite(function, beta, "log odds parameter", &lp))
        return lp;

      if (!include_summand<propto,T_prob>::value)
        return 0.0;
      
      if (ns.size() == 0)
        return 0.0;
        
      Eigen::Matrix<T_prob,Eigen::Dynamic,1> log_softmax_beta
        = log_softmax(beta);

      // FIXME:  replace with more efficient sum()
      Eigen::Matrix<typename boost::math::tools::promote_args<T_prob>::type,
                    Eigen::Dynamic,1> results(ns.size());
      for (size_t i = 0; i < ns.size(); ++i)
        results[i] = log_softmax_beta(ns[i] - 1);
      return sum(results);
    }
void cv2eigen( const Mat& src,
               Eigen::Matrix<_Tp, 1, Eigen::Dynamic>& dst )
{
    CV_Assert(src.rows == 1);
    dst.resize(src.cols);
    if( !(dst.Flags & Eigen::RowMajorBit) )
    {
        Mat _dst(src.cols, src.rows, DataType<_Tp>::type,
                 dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
        if( src.type() == _dst.type() )
            transpose(src, _dst);
        else
            Mat(src.t()).convertTo(_dst, _dst.type());
        CV_DbgAssert(_dst.data == (uchar*)dst.data());
    }
    else
    {
        Mat _dst(src.rows, src.cols, DataType<_Tp>::type,
                 dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
        src.convertTo(_dst, _dst.type());
        CV_DbgAssert(_dst.data == (uchar*)dst.data());
    }
}                     
double GreenStrain_LIMSolver2D::computeFunction(const Eigen::Matrix<double,Eigen::Dynamic,1>& x)
{
  // green strain energy
  double shape = 0;
  Eigen::Matrix<double,2,2> I = Eigen::Matrix<double,2,2>::Identity();
  for(int t=0;t<mesh->Triangles->rows();t++)
  {
    Eigen::Vector2d A(x[TriangleVertexIdx.coeff(0,t)],x[TriangleVertexIdx.coeff(1,t)]);
    Eigen::Vector2d B(x[TriangleVertexIdx.coeff(2,t)],x[TriangleVertexIdx.coeff(3,t)]);
    Eigen::Vector2d C(x[TriangleVertexIdx.coeff(4,t)],x[TriangleVertexIdx.coeff(5,t)]);

    Eigen::Matrix<double,2,3> V;
    V.col(0) = A;
    V.col(1) = B;
    V.col(2) = C;

    Eigen::Matrix<double,2,2> F = V*Ms.block<3,2>(0,2*t);
    Eigen::Matrix<double,2,2> E = (F.transpose()*F - I);
    shape += E.squaredNorm()*Divider;
  }

    return shape;
}
Example #27
0
bool GimbalCalibResidual::operator()(const T *const tau_s,
                                     const T *const tau_d,
                                     const T *const w1,
                                     const T *const w2,
                                     const T *const Lambda1,
                                     const T *const Lambda2,
                                     T *residual) const {
  // clang-format off
  // Form the transform from static camera to dynamic camera
  const Eigen::Matrix<T, 4, 4> T_ds = this->T_ds(tau_s, tau_d, w1, w2, Lambda1, Lambda2);

  // Calculate reprojection error by projecting 3D world point observed in
  // dynamic camera to static camera
  // -- Transform 3D world point from dynamic to static camera
  const Eigen::Matrix<T, 3, 1> P_d{T(this->P_d[0]), T(this->P_d[1]), T(this->P_d[2])};
  const Eigen::Matrix<T, 3, 1> P_s_cal = (T_ds.inverse() * P_d.homogeneous()).head(3);
  // -- Project 3D world point to image plane
  const Eigen::Matrix<T, 3, 3> K_s = this->K(T(this->fx_s), T(this->fy_s), T(this->cx_s), T(this->cy_s));
  // const Eigen::Matrix<T, 4, 1> D_s = this->D(T(this->k1_s), T(this->k2_s), T(this->k3_s), T(this->k4_s));
  // const Eigen::Matrix<T, 2, 1> Q_s_cal = this->project_pinhole_equi(K_s, D_s, P_s_cal);
  const Eigen::Matrix<T, 2, 1> Q_s_cal = this->project_pinhole(K_s, P_s_cal);
  // -- Calculate reprojection error
  residual[0] = T(this->Q_s[0]) - Q_s_cal(0);
  residual[1] = T(this->Q_s[1]) - Q_s_cal(1);

  // Calculate reprojection error by projecting 3D world point observed in
  // static camera to dynamic camera
  // -- Transform 3D world point from static to dynamic camera
  const Eigen::Matrix<T, 3, 1> P_s{T(this->P_s[0]), T(this->P_s[1]), T(this->P_s[2])};
  const Eigen::Matrix<T, 3, 1> P_d_cal = (T_ds * P_s.homogeneous()).head(3);
  // -- Project 3D world point to image plane
  const Eigen::Matrix<T, 3, 3> K_d = this->K(T(this->fx_d), T(this->fy_d), T(this->cx_d), T(this->cy_d));
  // const Eigen::Matrix<T, 4, 1> D_d = this->D(T(this->k1_d), T(this->k2_d), T(this->k3_d), T(this->k4_d));
  // const Eigen::Matrix<T, 2, 1> Q_d_cal = this->project_pinhole_equi(K_d, D_d, P_d_cal);
  const Eigen::Matrix<T, 2, 1> Q_d_cal = this->project_pinhole(K_d, P_d_cal);
  // -- Calculate reprojection error
  residual[2] = T(this->Q_d[0]) - Q_d_cal(0);
  residual[3] = T(this->Q_d[1]) - Q_d_cal(1);

  return true;
  // clang-format on
}
Example #28
0
    inline int
    ordered_logistic_rng(const double eta,
                         const Eigen::Matrix<double,Eigen::Dynamic,1>& c,
                         RNG& rng) {
      using boost::variate_generator;
      using stan::math::inv_logit;

      static const char* function = "stan::prob::ordered_logistic(%1%)";
      
      using stan::math::check_finite;
      using stan::math::check_positive;
      using stan::math::check_nonnegative;
      using stan::math::check_less;
      using stan::math::check_less_or_equal;
      using stan::math::check_greater;
      using stan::math::check_bounded;

      check_finite(function, eta, "Location parameter", (double*)0);
      check_greater(function, c.size(), 0, "Size of cut points parameter", 
                    (double*)0);
      for (int i = 1; i < c.size(); ++i) {
        check_greater(function, c(i), c(i - 1),
                      "Cut points parameter", (double*)0);
      }
      check_finite(function, c(c.size()-1), 
                   "Cut points parameter", (double*)0);
      check_finite(function, c(0),
                   "Cut points parameter", (double*)0);

      Eigen::VectorXd cut(c.rows()+1);
      cut(0) = 1 - inv_logit(eta - c(0));
      for(int j = 1; j < c.rows(); j++)
        cut(j) = inv_logit(eta - c(j - 1)) - inv_logit(eta - c(j));
      cut(c.rows()) = inv_logit(eta - c(c.rows() - 1));

      return stan::prob::categorical_rng(cut, rng);
    }
Example #29
0
 Eigen::Matrix<double,3,3>
 Triangle::localStiffMatrix() const{
   Eigen::Matrix<double,3,2> n;
   Triangle const & t(*this);
   double meas2=1/(2.0*t.measure());
   for (int i=0;i<3;++i){
     int j =  (i+1)% 3;
     int k = (j+1) % 3;
     n.row(i)<< (-t[k][1]+t[j][1]),(t[k][0]-t[j][0]);
   }
   n*=meas2;
   double off01=n.row(0).dot(n.row(1));
   double off02=n.row(0).dot(n.row(2));
   double off12=n.row(1).dot(n.row(2));
   Eigen::Matrix<double,3,3> s;
   s<<-(off01+off02),off01,off02,
     off01,-(off01+off12),off12,
     off02,off12,-(off02+off12);
   return s;
 }
Example #30
0
    inline 
    Eigen::Matrix<fvar<T>,R,C> 
    inverse(const Eigen::Matrix<fvar<T>, R, C>& m) {
      using stan::math::multiply;
      stan::math::validate_square(m, "inverse");
      Eigen::Matrix<T,R,C> m_deriv(m.rows(), m.cols());
      Eigen::Matrix<T,R,C> m_inv(m.rows(), m.cols());

      for(size_type i = 0; i < m.rows(); i++) {
        for(size_type j = 0; j < m.cols(); j++) {
          m_inv(i,j) = m(i,j).val_;
          m_deriv(i,j) = m(i,j).d_;
        }
      }

      m_inv = m_inv.inverse();
      m_deriv = -1 * multiply(multiply(m_inv, m_deriv), m_inv);
    
      return to_fvar(m_inv, m_deriv);
    }