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());
}
}
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> ¢roid)
{
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);
}
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);
}
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);
}
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));
}
}
}
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 °ree)
{
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;
}
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);
}
}
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;
}
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;
}
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));
}
}
}
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));
}
/* 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;
}
}
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);
}
}
}
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);
}
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()));
}
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());
}
}
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;
}
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);
}
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);
}
}
}
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_;
}
}
}
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;
}
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
}
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);
}
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;
}
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);
}
