void ProjectorBase::setCameraParams(InputArray _K, InputArray _R, InputArray _T)
{
Mat K = _K.getMat(), R = _R.getMat(), T = _T.getMat();
CV_Assert(K.size() == Size(3, 3) && K.type() == CV_32F);
CV_Assert(R.size() == Size(3, 3) && R.type() == CV_32F);
CV_Assert((T.size() == Size(1, 3) || T.size() == Size(3, 1)) && T.type() == CV_32F);
Mat_<float> K_(K);
k[0] = K_(0,0); k[1] = K_(0,1); k[2] = K_(0,2);
k[3] = K_(1,0); k[4] = K_(1,1); k[5] = K_(1,2);
k[6] = K_(2,0); k[7] = K_(2,1); k[8] = K_(2,2);
Mat_<float> Rinv = R.t();
rinv[0] = Rinv(0,0); rinv[1] = Rinv(0,1); rinv[2] = Rinv(0,2);
rinv[3] = Rinv(1,0); rinv[4] = Rinv(1,1); rinv[5] = Rinv(1,2);
rinv[6] = Rinv(2,0); rinv[7] = Rinv(2,1); rinv[8] = Rinv(2,2);
Mat_<float> R_Kinv = R * K.inv();
r_kinv[0] = R_Kinv(0,0); r_kinv[1] = R_Kinv(0,1); r_kinv[2] = R_Kinv(0,2);
r_kinv[3] = R_Kinv(1,0); r_kinv[4] = R_Kinv(1,1); r_kinv[5] = R_Kinv(1,2);
r_kinv[6] = R_Kinv(2,0); r_kinv[7] = R_Kinv(2,1); r_kinv[8] = R_Kinv(2,2);
Mat_<float> K_Rinv = K * Rinv;
k_rinv[0] = K_Rinv(0,0); k_rinv[1] = K_Rinv(0,1); k_rinv[2] = K_Rinv(0,2);
k_rinv[3] = K_Rinv(1,0); k_rinv[4] = K_Rinv(1,1); k_rinv[5] = K_Rinv(1,2);
k_rinv[6] = K_Rinv(2,0); k_rinv[7] = K_Rinv(2,1); k_rinv[8] = K_Rinv(2,2);
Mat_<float> T_(T.reshape(0, 3));
t[0] = T_(0,0); t[1] = T_(1,0); t[2] = T_(2,0);
}
void ProjectorBase::setCameraParams(const Mat &K, const Mat &R, const Mat &T)
{
CV_Assert(K.size() == Size(3, 3) && K.type() == CV_32F);
CV_Assert(R.size() == Size(3, 3) && R.type() == CV_32F);
CV_Assert((T.size() == Size(1, 3) || T.size() == Size(3, 1)) && T.type() == CV_32F);
Mat_<float> K_(K);
k[0] = K_(0,0); k[1] = K_(0,1); k[2] = K_(0,2);
k[3] = K_(1,0); k[4] = K_(1,1); k[5] = K_(1,2);
k[6] = K_(2,0); k[7] = K_(2,1); k[8] = K_(2,2);
Mat_<float> Rinv = R.t();
rinv[0] = Rinv(0,0); rinv[1] = Rinv(0,1); rinv[2] = Rinv(0,2);
rinv[3] = Rinv(1,0); rinv[4] = Rinv(1,1); rinv[5] = Rinv(1,2);
rinv[6] = Rinv(2,0); rinv[7] = Rinv(2,1); rinv[8] = Rinv(2,2);
Mat_<float> R_Kinv = R * K.inv();
r_kinv[0] = R_Kinv(0,0); r_kinv[1] = R_Kinv(0,1); r_kinv[2] = R_Kinv(0,2);
r_kinv[3] = R_Kinv(1,0); r_kinv[4] = R_Kinv(1,1); r_kinv[5] = R_Kinv(1,2);
r_kinv[6] = R_Kinv(2,0); r_kinv[7] = R_Kinv(2,1); r_kinv[8] = R_Kinv(2,2);
Mat_<float> K_Rinv = K * Rinv;
k_rinv[0] = K_Rinv(0,0); k_rinv[1] = K_Rinv(0,1); k_rinv[2] = K_Rinv(0,2);
k_rinv[3] = K_Rinv(1,0); k_rinv[4] = K_Rinv(1,1); k_rinv[5] = K_Rinv(1,2);
k_rinv[6] = K_Rinv(2,0); k_rinv[7] = K_Rinv(2,1); k_rinv[8] = K_Rinv(2,2);
Mat_<float> T_(T.reshape(0, 3));
t[0] = T_(0,0); t[1] = T_(1,0); t[2] = T_(2,0);
}
gaussian_process::gaussian_process( const Eigen::MatrixXd& domains
, const Eigen::VectorXd& targets
, const gaussian_process::covariance& covariance
, double self_covariance )
: domains_( domains )
, targets_( targets )
, covariance_( covariance )
, self_covariance_( self_covariance )
, offset_( targets.sum() / targets.rows() )
, K_( domains.rows(), domains.rows() )
{
if( domains.rows() != targets.rows() ) { COMMA_THROW( comma::exception, "expected " << domains.rows() << " row(s) in targets, got " << targets.rows() << " row(s)" ); }
targets_.array() -= offset_; // normalise
//use m_K as Kxx + variance*I, then invert it
//fill Kxx with values from covariance function
//for elements r,c in upper triangle
for( std::size_t r = 0; r < std::size_t( domains.rows() ); ++r )
{
K_( r, r ) = self_covariance_;
const Eigen::VectorXd& row = domains.row( r );
for( std::size_t c = r + 1; c < std::size_t( domains.rows() ); ++c )
{
K_( c, r ) = K_( r, c ) = covariance_( row, domains.row( c ) );
}
}
L_.compute( K_ ); // invert Kxx + variance * I to become (by definition) B
alpha_ = L_.solve( targets_ );
}
bool CameraProjection
::write(std::ostream& os) const
{
double f_x, f_y, c_x, c_y;
f_x = K_(0,0);
f_y = K_(1,1);
c_x = K_(0,2);
c_y = K_(1,2);
os << f_x << " ";
os << f_y << " ";
os << c_x << " ";
os << c_y << " ";
return true;
}
void Foam::constHTemperatureFvPatchScalarField::updateCoeffs()
{
if (this->updated())
{
return;
}
// scalarField K_ = patch().lookupPatchField<volScalarField, scalar>("K");
const fvMesh& mesh = patch().boundaryMesh().mesh();
const solidThermo& thermo =
mesh.lookupObject<solidThermo>("thermophysicalProperties");
scalarField K_(thermo.kappa(patch().index()));
refValue() = Tinf_;
refGrad() = 0.0;
valueFraction() =
1.0/(1.0 + K_/max(h_,SMALL)*patch().deltaCoeffs());
mixedFvPatchField<scalar>::updateCoeffs();
}
cv::Point2d PinholeCameraModel::unrectifyPoint(const cv::Point2d& uv_rect) const
{
assert( initialized() );
if (cache_->distortion_state == NONE)
return uv_rect;
if (cache_->distortion_state == UNKNOWN)
throw Exception("Cannot call unrectifyPoint when distortion is unknown.");
assert(cache_->distortion_state == CALIBRATED);
/// @todo Make this just call projectPixelTo3dRay followed by cv::projectPoints. But
/// cv::projectPoints requires 32-bit float, which is annoying.
// Formulae from docs for cv::initUndistortRectifyMap,
// http://opencv.willowgarage.com/documentation/cpp/camera_calibration_and_3d_reconstruction.html
// x <- (u - c'x) / f'x
// y <- (v - c'y) / f'y
// c'x, f'x, etc. (primed) come from "new camera matrix" P[0:3, 0:3].
double x = (uv_rect.x - cx() - Tx()) / fx();
double y = (uv_rect.y - cy() - Ty()) / fy();
// [X Y W]^T <- R^-1 * [x y 1]^T
double X = R_(0,0)*x + R_(1,0)*y + R_(2,0);
double Y = R_(0,1)*x + R_(1,1)*y + R_(2,1);
double W = R_(0,2)*x + R_(1,2)*y + R_(2,2);
// x' <- X/W, y' <- Y/W
double xp = X / W;
double yp = Y / W;
// x'' <- x'(1+k1*r^2+k2*r^4+k3*r^6) + 2p1*x'*y' + p2(r^2+2x'^2)
// y'' <- y'(1+k1*r^2+k2*r^4+k3*r^6) + p1(r^2+2y'^2) + 2p2*x'*y'
// where r^2 = x'^2 + y'^2
double r2 = xp*xp + yp*yp;
double r4 = r2*r2;
double r6 = r4*r2;
double a1 = 2*xp*yp;
double k1 = D_(0,0), k2 = D_(0,1), p1 = D_(0,2), p2 = D_(0,3), k3 = D_(0,4);
double barrel_correction = 1 + k1*r2 + k2*r4 + k3*r6;
if (D_.cols == 8)
barrel_correction /= (1.0 + D_(0,5)*r2 + D_(0,6)*r4 + D_(0,7)*r6);
double xpp = xp*barrel_correction + p1*a1 + p2*(r2+2*(xp*xp));
double ypp = yp*barrel_correction + p1*(r2+2*(yp*yp)) + p2*a1;
// map_x(u,v) <- x''fx + cx
// map_y(u,v) <- y''fy + cy
// cx, fx, etc. come from original camera matrix K.
return cv::Point2d(xpp*K_(0,0) + K_(0,2), ypp*K_(1,1) + K_(1,2));
}
bool CameraProjection
::read(std::istream& is)
{
K_ = Eigen::Matrix3d::Identity();
double f_x, f_y, c_x, c_y;
is >> f_x;
is >> f_y;
is >> c_x;
is >> c_y;
//populate K with these values
K_(0,0) = f_x;
K_(1,1) = f_y;
K_(0,2) = c_x;
K_(1,2) = c_y;
K_(2,2) = 1.0;
if(K_(0,0) == 0.0 || K_(1,1) == 0.0) //check K is populated
{
std::cout << " error loading projection pointer\n";
return false;
}
return true;
}
bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& msg)
{
// Create our repository of cached data (rectification maps, etc.)
if (!cache_)
cache_ = boost::make_shared<Cache>();
// Binning = 0 is considered the same as binning = 1 (no binning).
uint32_t binning_x = msg.binning_x ? msg.binning_x : 1;
uint32_t binning_y = msg.binning_y ? msg.binning_y : 1;
// ROI all zeros is considered the same as full resolution.
sensor_msgs::RegionOfInterest roi = msg.roi;
if (roi.x_offset == 0 && roi.y_offset == 0 && roi.width == 0 && roi.height == 0) {
roi.width = msg.width;
roi.height = msg.height;
}
// Update time stamp (and frame_id if that changes for some reason)
cam_info_.header = msg.header;
// Update any parameters that have changed. The full rectification maps are
// invalidated by any change in the calibration parameters OR binning.
bool &full_dirty = cache_->full_maps_dirty;
full_dirty |= update(msg.height, cam_info_.height);
full_dirty |= update(msg.width, cam_info_.width);
full_dirty |= update(msg.distortion_model, cam_info_.distortion_model);
full_dirty |= updateMat(msg.D, cam_info_.D, D_, 1, msg.D.size());
full_dirty |= updateMat(msg.K, cam_info_.K, K_full_);
full_dirty |= updateMat(msg.R, cam_info_.R, R_);
full_dirty |= updateMat(msg.P, cam_info_.P, P_full_);
full_dirty |= update(binning_x, cam_info_.binning_x);
full_dirty |= update(binning_y, cam_info_.binning_y);
// The reduced rectification maps are invalidated by any of the above or a
// change in ROI.
bool &reduced_dirty = cache_->reduced_maps_dirty;
reduced_dirty = full_dirty;
reduced_dirty |= update(roi.x_offset, cam_info_.roi.x_offset);
reduced_dirty |= update(roi.y_offset, cam_info_.roi.y_offset);
reduced_dirty |= update(roi.height, cam_info_.roi.height);
reduced_dirty |= update(roi.width, cam_info_.roi.width);
reduced_dirty |= update(roi.do_rectify, cam_info_.roi.do_rectify);
// As is the rectified ROI
cache_->rectified_roi_dirty = reduced_dirty;
// Figure out how to handle the distortion
if (cam_info_.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB ||
cam_info_.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL) {
cache_->distortion_state = (cam_info_.D.size() == 0 || (cam_info_.D[0] == 0.0)) ? NONE : CALIBRATED;
}
else
cache_->distortion_state = UNKNOWN;
// If necessary, create new K_ and P_ adjusted for binning and ROI
/// @todo Calculate and use rectified ROI
bool adjust_binning = (binning_x > 1) || (binning_y > 1);
bool adjust_roi = (roi.x_offset != 0) || (roi.y_offset != 0);
if (!adjust_binning && !adjust_roi) {
K_ = K_full_;
P_ = P_full_;
}
else {
K_ = K_full_;
P_ = P_full_;
// ROI is in full image coordinates, so change it first
if (adjust_roi) {
// Move principal point by the offset
/// @todo Adjust P by rectified ROI instead
K_(0,2) -= roi.x_offset;
K_(1,2) -= roi.y_offset;
P_(0,2) -= roi.x_offset;
P_(1,2) -= roi.y_offset;
}
if (binning_x > 1) {
double scale_x = 1.0 / binning_x;
K_(0,0) *= scale_x;
K_(0,2) *= scale_x;
P_(0,0) *= scale_x;
P_(0,2) *= scale_x;
P_(0,3) *= scale_x;
}
if (binning_y > 1) {
double scale_y = 1.0 / binning_y;
K_(1,1) *= scale_y;
K_(1,2) *= scale_y;
P_(1,1) *= scale_y;
P_(1,2) *= scale_y;
P_(1,3) *= scale_y;
}
}
return reduced_dirty;
}
void Image3D::GenNewViews(){
char fn[128];
static int no = 0;
Eigen::Matrix3d R, R_, K, K_, H;
Eigen::Vector3d t;
cam.GetK(K);
cam.GetRT(R, t);
double scale = 2.0;
double scale_ = 1.0 / scale, scale2_ = scale_ * scale_;
int w_ = cam.W(), w = w_ * scale;
int h_ = cam.H(), h = h_ * scale;
K_(0, 0) = 1.0 / K(0, 0); K_(0, 1) = 0.0; K_(0, 2) = -K(0, 2) / K(0, 0);
K_(1, 0) = 0.0; K_(1, 1) = 1.0 / K(1, 1); K_(1, 2) = -K(1, 2) / K(1, 1);
K_(2, 0) = 0.0; K_(2, 1) = 0.0; K_(2, 2) = 1.0;
//K_ = K.inverse();
//Eigen::Vector3d axis(R(axis, 0), R(axis, 1), R(axis, 2));
Eigen::Vector3d axis(R(ParamParser::axis, 0), R(ParamParser::axis, 1), R(ParamParser::axis, 2));
std::vector<double> angle;
for (int i = ParamParser::view_count / 2; i > 0; --i){ angle.push_back(-ParamParser::rot_angle * i); }
for (int i = 0; i <= ParamParser::view_count / 2; ++i){ angle.push_back(ParamParser::rot_angle * i); }
std::cout << "Generating views#: ";
for (int k = 0; k < ParamParser::view_count; ++k){
std::cout << k << " ";
std::vector<int> texIndex_(w_ * h_, -1);
cv::Mat img(h_, w_, CV_8UC3);
RotationMatrix(angle[k] / 180 * M_PI, axis, R_);
H = K * (R_ * K_);
std::vector<Eigen::Vector2d> xy(w * h);
double minu, minv, maxu, maxv;
minu = minv = 1000000000.0;
maxu = maxv = -1000000000.0;
for (int i = 0; i < w * h; ++i){
int u = i % w - w * scale2_;
int v = i / w - h * scale2_;
double wf = H(2, 0) * u + H(2, 1) * v + H(2, 2);
double uf = (H(0, 0) * u + H(0, 1) * v + H(0, 2)) / wf;
double vf = (H(1, 0) * u + H(1, 1) * v + H(1, 2)) / wf;
if (CheckRange(uf, vf, w_, h_)){
minu = min(minu, u + w * scale2_);
minv = min(minv, v + h * scale2_);
maxu = max(maxu, u + w * scale2_);
maxv = max(maxv, v + h * scale2_);
}
xy[i] = Eigen::Vector2d(uf, vf);
}
double centerx = (maxu + minu) * 0.5;
double centery = (maxv + minv) * 0.5;
double offsetx = centerx - w * scale2_;
double offsety = centery - h * scale2_;
for (int i = 0; i < w * h; ++i){
double uf = xy[i][0];
double vf = xy[i][1];
double u11 = floor(uf), v11 = floor(vf);
double u22 = ceil(uf), v22 = ceil(vf);
int u = int(i % w - offsetx + 0.5);
int v = int(i / w - offsety + 0.5);
if (CheckRange(u11, v11, w_, h_) && CheckRange(u22, v22, w_, h_) && CheckRange(u, v, w_, h_)){
if (fabs(u11 - u22) <= 1e-9 && fabs(v11 - v22) <= 1e-9){
img.at<cv::Vec3b>(v, u) = image.at<cv::Vec3b>(v11, u11);
texIndex_[v * w_ + u] = (v11 * w_ + u11);
}
else{
cv::Vec3b rgb11 = image.at<cv::Vec3b>(v11, u11);
cv::Vec3b rgb12 = image.at<cv::Vec3b>(v22, u11);
cv::Vec3b rgb21 = image.at<cv::Vec3b>(v11, u22);
cv::Vec3b rgb22 = image.at<cv::Vec3b>(v22, u22);
cv::Vec3b rgb;
if (fabs(u11 - u22) <= 1e-9){
double s1 = (vf - v11) / (v22 - v11);
double s2 = 1 - s1;
rgb[0] = uchar(rgb11[0] * s2 + rgb12[0] * s1);
rgb[1] = uchar(rgb11[1] * s2 + rgb12[1] * s1);
rgb[2] = uchar(rgb11[2] * s2 + rgb12[2] * s1);
}
else if (fabs(v11 - v22) <= 1e-9){
double s1 = (uf - u11) / (u22 - u11);
double s2 = 1 - s1;
rgb[0] = uchar(rgb11[0] * s2 + rgb21[0] * s1);
rgb[1] = uchar(rgb11[1] * s2 + rgb21[1] * s1);
rgb[2] = uchar(rgb11[2] * s2 + rgb21[2] * s1);
}
else{
double s1 = (u22 - uf) * (v22 - vf);
double s2 = (uf - u11) * (v22 - vf);
double s3 = (u22 - uf) * (vf - v11);
double s4 = (uf - u11) * (vf - v11);
rgb[0] = uchar(rgb11[0] * s1 + rgb21[0] * s2 + rgb12[0] * s3 + rgb22[0] * s4);
rgb[1] = uchar(rgb11[1] * s1 + rgb21[1] * s2 + rgb12[1] * s3 + rgb22[1] * s4);
rgb[2] = uchar(rgb11[2] * s1 + rgb21[2] * s2 + rgb12[2] * s3 + rgb22[2] * s4);
}
img.at<cv::Vec3b>(v, u) = rgb;
texIndex_[v * w_ + u] = (int(vf + 0.5) * w_ + int(uf + 0.5));
}
}
}
texIndex.push_back(texIndex_);
sprintf_s(fn, "%s/proj%d_%d.jpg", path.c_str(), frmNo, k);
cv::imwrite(fn, img);
}
std::cout << std::endl;
}
void Foam::kineticTheoryModel::solve(const volTensorField& gradUat)
{
if(kineticTheory_)
{
//if (!kineticTheory_)
//{
// return;
//}
//const scalar sqrtPi = sqrt(mathematicalConstant::pi);
if(Berzi_)
{
Info << "Berzi Model is used" << endl;
}
else{
const scalar sqrtPi = sqrt(constant::mathematical::pi);
surfaceScalarField phi = 1.5*rhoa_*phia_*fvc::interpolate(alpha_);
volTensorField dU = gradUat.T();//fvc::grad(Ua_);
volSymmTensorField D = symm(dU);
// NB, drag = K*alpha*beta,
// (the alpha and beta has been extracted from the drag function for
// numerical reasons)
volScalarField Ur = mag(Ua_ - Ub_);
volScalarField betaPrim = alpha_*(1.0 - alpha_)*draga_.K(Ur);
// Calculating the radial distribution function (solid volume fraction is
// limited close to the packing limit, but this needs improvements)
// The solution is higly unstable close to the packing limit.
gs0_ = radialModel_->g0
(
min(max(alpha_, 1e-6), alphaMax_ - 0.01),
alphaMax_
);
// particle pressure - coefficient in front of Theta (Eq. 3.22, p. 45)
volScalarField PsCoeff = granularPressureModel_->granularPressureCoeff
(
alpha_,
gs0_,
rhoa_,
e_
);
// 'thermal' conductivity (Table 3.3, p. 49)
kappa_ = conductivityModel_->kappa(alpha_, Theta_, gs0_, rhoa_, da_, e_);
// particle viscosity (Table 3.2, p.47)
mua_ = viscosityModel_->mua(alpha_, Theta_, gs0_, rhoa_, da_, e_);
dimensionedScalar Tsmall
(
"small",
dimensionSet(0 , 2 ,-2 ,0 , 0, 0, 0),
1.0e-6
);
dimensionedScalar TsmallSqrt = sqrt(Tsmall);
volScalarField ThetaSqrt = sqrt(Theta_);
// dissipation (Eq. 3.24, p.50)
volScalarField gammaCoeff =
12.0*(1.0 - sqr(e_))*sqr(alpha_)*rhoa_*gs0_*(1.0/da_)*ThetaSqrt/sqrtPi;
// Eq. 3.25, p. 50 Js = J1 - J2
volScalarField J1 = 3.0*betaPrim;
volScalarField J2 =
0.25*sqr(betaPrim)*da_*sqr(Ur)
/(max(alpha_, 1e-6)*rhoa_*sqrtPi*(ThetaSqrt + TsmallSqrt));
// bulk viscosity p. 45 (Lun et al. 1984).
lambda_ = (4.0/3.0)*sqr(alpha_)*rhoa_*da_*gs0_*(1.0+e_)*ThetaSqrt/sqrtPi;
// stress tensor, Definitions, Table 3.1, p. 43
volSymmTensorField tau = 2.0*mua_*D + (lambda_ - (2.0/3.0)*mua_)*tr(D)*I;
if (!equilibrium_)
{
// construct the granular temperature equation (Eq. 3.20, p. 44)
// NB. note that there are two typos in Eq. 3.20
// no grad infront of Ps
// wrong sign infront of laplacian
fvScalarMatrix ThetaEqn
(
fvm::ddt(1.5*alpha_*rhoa_, Theta_)
+ fvm::div(phi, Theta_, "div(phi,Theta)")
==
fvm::SuSp(-((PsCoeff*I) && dU), Theta_)
+ (tau && dU)
+ fvm::laplacian(kappa_, Theta_, "laplacian(kappa,Theta)")
+ fvm::Sp(-gammaCoeff, Theta_)
+ fvm::Sp(-J1, Theta_)
+ fvm::Sp(J2/(Theta_ + Tsmall), Theta_)
);
ThetaEqn.relax();
ThetaEqn.solve();
}
else
{
// equilibrium => dissipation == production
// Eq. 4.14, p.82
volScalarField K1 = 2.0*(1.0 + e_)*rhoa_*gs0_;
volScalarField K3 = 0.5*da_*rhoa_*
(
(sqrtPi/(3.0*(3.0-e_)))
*(1.0 + 0.4*(1.0 + e_)*(3.0*e_ - 1.0)*alpha_*gs0_)
+1.6*alpha_*gs0_*(1.0 + e_)/sqrtPi
);
volScalarField K2 =
4.0*da_*rhoa_*(1.0 + e_)*alpha_*gs0_/(3.0*sqrtPi) - 2.0*K3/3.0;
volScalarField K4 = 12.0*(1.0 - sqr(e_))*rhoa_*gs0_/(da_*sqrtPi);
volScalarField trD = tr(D);
volScalarField tr2D = sqr(trD);
volScalarField trD2 = tr(D & D);
volScalarField t1 = K1*alpha_ + rhoa_;
volScalarField l1 = -t1*trD;
volScalarField l2 = sqr(t1)*tr2D;
volScalarField l3 = 4.0*K4*max(alpha_, 1e-6)*(2.0*K3*trD2 + K2*tr2D);
Theta_ = sqr((l1 + sqrt(l2 + l3))/(2.0*(alpha_ + 1.0e-4)*K4));
}
Theta_.max(1.0e-15);
Theta_.min(1.0e+3);
volScalarField pf = frictionalStressModel_->frictionalPressure
(
alpha_,
alphaMinFriction_,
alphaMax_,
Fr_,
eta_,
p_
);
PsCoeff += pf/(Theta_+Tsmall);
PsCoeff.min(1.0e+10);
PsCoeff.max(-1.0e+10);
// update particle pressure
pa_ = PsCoeff*Theta_;
// frictional shear stress, Eq. 3.30, p. 52
volScalarField muf = frictionalStressModel_->muf
(
alpha_,
alphaMax_,
pf,
D,
phi_
);
// add frictional stress
mua_ += muf;
//-AO Inconsistency of equations
const scalar constSMALL = 0.001; //1.e-06;
mua_ /= (fvc::average(alpha_) + scalar(constSMALL));
lambda_ /= (fvc::average(alpha_) + scalar(constSMALL));
//-AO
mua_.min(1.0e+2);
mua_.max(0.0);
Info<< "kinTheory: max(Theta) = " << max(Theta_).value() << endl;
volScalarField ktn = mua_/rhoa_;
Info<< "kinTheory: min(nua) = " << min(ktn).value()
<< ", max(nua) = " << max(ktn).value() << endl;
Info<< "kinTheory: min(pa) = " << min(pa_).value()
<< ", max(pa) = " << max(pa_).value() << endl;
//}
/*
volScalarField& Foam::kineticTheoryModel::ppMagf(const volScalarField& alphaUpdate)
{
volScalarField alpha = alphaUpdate;
gs0_ = radialModel_->g0(min(alpha, alphaMinFriction_), alphaMax_);
gs0Prime_ = radialModel_->g0prime(min(alpha, alphaMinFriction_), alphaMax_);
// Computing ppMagf
ppMagf_ = Theta_*granularPressureModel_->granularPressureCoeffPrime
(
alpha,
gs0_,
gs0Prime_,
rhoa_,
e_
);
volScalarField ppMagfFriction = frictionalStressModel_->frictionalPressurePrime
(
alpha,
alphaMinFriction_,
alphaMax_,
Fr_,
eta_,
p_
);
// NOTE: this might not be appropriate if J&J model is used (verify)
forAll(alpha, cellI)
{
if(alpha[cellI] >= alphaMinFriction_.value())
{
ppMagf_[cellI] = ppMagfFriction[cellI];
}
}
ppMagf_.correctBoundaryConditions();
return ppMagf_;
}
*/}
}
else if(mofidiedKineticTheoryPU_)
{
//if (!mofidiedKineticTheoryPU_)
//{
// return;
//}
Info << " " << endl;
Info << "Modified kinetic theory model - Chialvo-Sundaresan " << endl;
bool testMKTimp(false);
if(kineticTheoryProperties_.found("testMKTimp"))
{
testMKTimp = true;
Info << "Modified kinetic theory model - testing implementation (chi=1,eEff=e, ksi=1) " << endl;
}
bool diluteCorrection(false);
if(kineticTheoryProperties_.found("diluteCorrection"))
{
testMKTimp = false;
diluteCorrection = true;
Info << "Modified kinetic theory model - Only dilute correction " << endl;
}
bool denseCorrection(false);
if(kineticTheoryProperties_.found("denseCorrection"))
{
testMKTimp = false;
diluteCorrection = false;
denseCorrection = true;
Info << "Modified kinetic theory model - Only dense correction " << endl;
}
bool frictionBlending(false);
if(kineticTheoryProperties_.found("frictionBlending"))
{
frictionBlending = true;
Info << "Modified kinetic theory model - Include Friction Blneding " << endl;
}
if(decomposePp_) Info << "Decompose Pp into Pp - PpStar " << endl;
bool verboseMKT(false);
if(kineticTheoryProperties_.found("verboseMKT")) verboseMKT = true;
const scalar Pi = constant::mathematical::pi;
const scalar sqrtPi = sqrt(constant::mathematical::pi);
const scalar constSMALL = 1.e-06; //1.e-06; 1.e-03;
// Read from dictionary
muFric_ = readScalar(kineticTheoryProperties_.lookup("muFriction"));
eEff_ = e_ - 3.0 / 2.0 * muFric_ * exp(-3.0 * muFric_);
// If only test MKT implementation
if(testMKTimp) eEff_ = e_;
alphaf_ = readScalar(kineticTheoryProperties_.lookup("alphaDiluteInertialUpperLimit"));
alphac_ = readScalar(kineticTheoryProperties_.lookup("alphaCritical"));
alphad_ = readScalar(kineticTheoryProperties_.lookup("alphaDelta"));
upsilons_ = readScalar(kineticTheoryProperties_.lookup("yieldStressRatio"));
// Model parameters
dimensionedScalar I0(0.2); // Table 2, p.15
dimensionedScalar const_alpha(0.36); // Table 2, p.15
dimensionedScalar const_alpha1(0.06); // Table 2, p.15
// Calculating the radial distribution function (solid volume fraction is
// limited close to the packing limit, but this needs improvements)
// The solution is higly unstable close to the packing limit.
gs0_ = radialModel_->g0jamming
(
Ua_.mesh(),
//max(alpha, scalar(constSMALL)),
min(max(alpha_, scalar(constSMALL)),alphaMax_ - 0.01), //changed by YG
alphaMax_,
alphad_, ///changed by YG
alphac_
);
// particle pressure - coefficient in front of T (Eq. 1, p. 3)
volScalarField PsCoeff // -> rho_p * H
(
granularPressureModel_->granularPressureCoeff
(
alpha_,
gs0_,
rhoa_,
e_
)
);
PsCoeff.max(1.0e-15);
// PsCoeff.min(1.0e+10);
// PsCoeff.max(-1.0e+10);
// Solid kinetic+collisional viscosity mua_ = nu_k^star + nu_c^star, Eq. 8,9, p.4
// If Garzo-Dufty viscosity is used (viscosity is dimensionless), there is issue with dimension of mu1
mua_ = viscosityModel_->mua(alpha_, Theta_, gs0_, rhoa_, da_, e_);
// Solid bulk viscosity mua_ = nu_k^star + nu_c^star, Eq. 10, p.4
// If Garzo-Dufty viscosity is used (viscosity is dimensionless), there is issue with dimension of mu1
// Create dimensionedScalar
dimensionedScalar viscDim("zero", dimensionSet(1, -1, -1, 0, 0), 1.0);
lambda_ = viscDim * 384.0 / ( 25.0 * Pi ) * ( 1.0 + e_ ) * alpha_ * alpha_ * gs0_ ;
//lambda_ = (4.0/3.0)*sqr(alpha_)*rhoa_*da_*gs0_*(1.0+e_)*sqrt(Theta_)/sqrtPi;
volScalarField ratioBulkShearVisc(lambda_/(mua_+lambda_));
// J Eq.5, p3
volScalarField J_( 5.0 * sqrtPi / 96.0 * ( mua_ + lambda_ ) / viscDim ); // Dimension issue
// K Eq.6, p3
volScalarField K_(12.0/sqrtPi*alpha_*alpha_*gs0_*(1.0-e_*e_));
// K' Eq.26, p8 modified dissipation due to friction
volScalarField Kmod_(K_*(1.0 - eEff_*eEff_)/(1.0 - e_*e_));
// M Eq.30 p.9
volScalarField M_( max( J_ / max( Kmod_, constSMALL) , const_alpha1 / sqrt( max(alphac_ - alpha_, constSMALL) ) ) );
// Shear stress rate tensor
volTensorField dU(gradUat.T());
volSymmTensorField D(symm(dU));
// Shear stress rate (gammaDot)
volScalarField gammaDot(sqrt(2.*magSqr(D)));
dimensionedScalar gammaDotSmall("gammaDotSmall",dimensionSet(0 , 0 , -1 , 0 , 0, 0, 0), constSMALL);
// Dilute inertia temperature Eq.24, p8
volScalarField ThetaDil_ = ( J_ / max ( Kmod_ , 1e-1 ) ) * ( gammaDot * da_ ) * ( gammaDot * da_ );
// Dense inertia temperature Eq.27, p8
// volScalarField ThetaDense_ = const_alpha1 * ( gammaDot * da_ ) * ( gammaDot * da_ )
// / sqrt( max(alphac_ - alpha_, constSMALL) );
volScalarField ThetaDense_ = const_alpha1 * ( gammaDot * da_ ) * ( gammaDot * da_ )
/ sqrt( max(alphac_ - alpha_, alphad_) )
+ max(alpha_ - (alphac_ - alphad_),0.0) * 0.5 *const_alpha1*( gammaDot * da_ ) * ( gammaDot * da_)*pow(alphad_,-1.5);
// Theta
Theta_ = max(ThetaDil_,ThetaDense_) ;
if(testMKTimp || diluteCorrection) Theta_ = ThetaDil_;
if(denseCorrection) Theta_ = ThetaDense_;
// Limit granular temperature
Theta_.max(1.0e-15);
Theta_.min(1.0e+3);
// Particle pressure
pa_ = PsCoeff * Theta_;
if(frictionBlending)
{
/* volScalarField pf = frictionalStressModel_->frictionalPressure
(
alpha_,
alphaMinFriction_-0.001,
alphaMax_,
Fr_,
eta_,
p_
);
pa_ = pa_ + pf;
*/
// pa_ =pa_ + dimensionedScalar("1e24", dimensionSet(1, -1, -2, 0, 0), Fr_.value())*pow(max(alpha_ - (alphaMinFriction_), scalar(0)), 2/3);
pa_ =pa_ + dimensionedScalar("5810", dimensionSet(1, 0, -2, 0, 0), 581.0)/da_*pow(max(alpha_ - (alphaMinFriction_-0.0), scalar(0)), 2.0/3.0);
// pa_ =pa_ + dimensionedScalar("4.7e9", dimensionSet(1, -1, -2, 0, 0), 4.7e9)*pow(max(alpha_ - (alphaMinFriction_-0.0), scalar(0)), 1.56);
// forAll(alpha_, cellI)
// {
// if(alpha_[cellI] >= (alphaMinFriction_.value()-0.00001))
// {
// pa_[cellI] = pa_[cellI] + 581.0/da_.value()*pow(alpha_[cellI] - (alphaMinFriction_.value()-0.00001), 2.0/3.0);
// }
// }
}
// Psi Eq.32, p.12
dimensionedScalar psi(1.0 + 3.0/10.0*pow((1.0-e_*e_),-1.5)*(1.0-exp(-8.0*muFric_)));
if(testMKTimp) psi = 1.0;
// Shear stress ratio in dilute regime, Eq.33, p.12
dimensionedScalar paSmall("paSmall",dimensionSet(1, -1, -2, 0, 0), constSMALL);
volScalarField inertiaNumber( gammaDot * da_ / sqrt( (pa_ + paSmall) / rhoa_ ) );
// Modified inertia number Eq.35, p.13
volScalarField modInertiaNumber( inertiaNumber / max( alpha_, constSMALL ) );
// Model parameters
volScalarField chi( 1.0 / ( pow( I0 / max( modInertiaNumber,constSMALL ) , 1.5 ) + 1.0 ));
if(testMKTimp || diluteCorrection) chi = max( modInertiaNumber,constSMALL ) / max( modInertiaNumber,constSMALL ) ;
if(denseCorrection) chi= modInertiaNumber - modInertiaNumber;
// Beta + Sigma_tau Eq.49 p.14
volScalarField beta(alpha_ * psi * J_ * sqrt( K_ /( max ( (Kmod_ * ( PsCoeff / rhoa_)), constSMALL ) ) ) );
volScalarField sigmaTau( const_alpha / max( beta, constSMALL ) + ( 1 - const_alpha / max( beta, constSMALL ) ) * chi);
// Sigma_gamma Eq.51 p.14
volScalarField sigmaGamma( beta * sqrt( PsCoeff/rhoa_ ) / max( ( Kmod_ * M_ ), constSMALL ) * sigmaTau);
// dissipation
volScalarField gammaCoeff
(
// van Wachem (Eq. 3.24, p.50) 12.0*(1.0 - sqr(e_))*sqr(alpha_)*rhoa_*gs0_*(1.0/da_)*ThetaSqrt/sqrtPi
// Chialvo & Sundaresan Eq.50 p.14
//rhoa_ / da_ * Kmod_ * Theta_ * sqrt(Theta_) * sigmaGamma
rhoa_ / da_ * Kmod_ * sqrt(Theta_) * sigmaGamma
);
// Blending function
volScalarField func_B( const_alpha + ( beta-const_alpha ) * chi );
// Shear stress ratio
upsilon_ = upsilons_ * (1 - chi) + func_B * modInertiaNumber;
// Shear stress
volSymmTensorField S( D - 1./3.*tr(D)*I );
volSymmTensorField hatS( 2. * S / max( gammaDot, gammaDotSmall ) );
// Shear stress based on pressure and ratio
tau_ = pa_ * upsilon_ * hatS;
// Viscosity
mua_ = ( pa_ * upsilon_ ) / (max( gammaDot, gammaDotSmall )) ;
// Divide by alpha (to be consistent with OpenFOAM implementation)
/* mua_ /= (fvc::average(alpha_) + scalar(0.001));
tau_ /= (fvc::average(alpha_) + scalar(0.001));
lambda_ /= (fvc::average(alpha_) + scalar(0.001)); */
mua_ /= max(alpha_, scalar(constSMALL));
// Limit mua
mua_.min(3e+02);
mua_.max(0.0);
// Limit lambda
lambda_ = mua_ * ratioBulkShearVisc;
// Limit shear stress
tau_ = mua_ * gammaDot * hatS;
// tau_ /= max(alpha_, scalar(constSMALL));
// lambda_ /= max(alpha_, scalar(constSMALL));
//mua_ /= max(alpha_, scalar(constSMALL));
//tau_ /= max(alpha_, scalar(constSMALL));
//lambda_ /= max(alpha_, scalar(constSMALL));
if(verboseMKT)
{
#include "verboseMKT.H"
}
//-AO, YG - Decompose particle pressure, Sundar's idea
if(decomposePp_)
{
pa_ /= (fvc::average(alpha_) + scalar(0.001));
//pa_.correctBoundaryConditions();
pa_.max(1.0e-15);
}
}
else
{
return;
}
}
/** Estimate monocular visual odometry.
* @param std::vector<Match> vector with matches
* @param Eigen::Matrix3f& (output) estimated rotation matrix
* @param Eigen::Vector3f& (output) estimated translation vector
* @param bool show optical flow (true), don't show otherwise
* @param std::vector<Match> output vector with all inlier matches
* @param std::vector<Eigen::Vector3f> output vector with 3D points, triangulated from all inlier matches
* @return bool true is motion successfully estimated, false otherwise */
bool MonoOdometer5::estimateMotion(std::vector<Match> matches, Eigen::Matrix3f &R, Eigen::Vector3f &t, bool showOpticalFlow, std::vector<Match> &inlierMatches, std::vector<Eigen::Vector3f> &points3D)
{
counter_total++;
// check number of correspondences
int N = matches.size();
if(N < param_odometerMinNumberMatches_)
{
// too few matches to compute F
std::cout << "------------ NOT ENOUGH MATCHES" << std::endl;
R = Eigen::Matrix3f::Identity();
t << 0.0, 0.0, 0.0;
std::cout << "SINGULARITIES / TOTAL: " << counter_sing << " / " << counter_total << std::endl;
return false;
}
std::vector<cv::Point2f> prevPoints = getPrevPoints(matches);
std::vector<cv::Point2f> currPoints = getCurrPoints(matches);
cv::Mat E, Rcv, tcv, mask;
double focal = K_(0, 0);
cv::Point2f pp(K_(0, 2), K_(1, 2));
E = cv::findEssentialMat(currPoints, prevPoints, focal, pp, cv::RANSAC, 0.999, 1.0, mask);
if(!testE(E))
{
// too few matches to compute F
std::cout << "------------ SINGULARITY IN ESSENTIAL MATRIX" << std::endl;
counter_sing++;
R = Eigen::Matrix3f::Identity();
t << 0.0, 0.0, 0.0;
std::cout << "SINGULARITIES / TOTAL: " << counter_sing << " / " << counter_total << std::endl;
return false;
}
cv::recoverPose(E, currPoints, prevPoints, Rcv, tcv, focal, pp, mask);
R << Rcv.at<double>(0,0), Rcv.at<double>(0,1), Rcv.at<double>(0,2),
Rcv.at<double>(1,0), Rcv.at<double>(1,1), Rcv.at<double>(1,2),
Rcv.at<double>(2,0), Rcv.at<double>(2,1), Rcv.at<double>(2,2);
t << tcv.at<double>(0,0), tcv.at<double>(1,0), tcv.at<double>(2,0);
// save inlier and outlier matches
std::vector<Match> outlierMatches;
for(int i=0; i<matches.size(); i++)
{
if(mask.at<char>(i, 0) == 1)
{
inlierMatches.push_back(matches[i]);
}
else
{
outlierMatches.push_back(matches[i]);
}
}
if(showOpticalFlow)
{
//std::cout << "R" << std::endl << R << std::endl;
//std::cout << "t" << std::endl << t << std::endl;
cv::Mat image(1024, 768, CV_8UC3, cv::Scalar(0,0,0));
cv::Mat of1 = highlightOpticalFlow(image, inlierMatches, cv::Scalar(0, 255, 0));
cv::Mat of2 = highlightOpticalFlow(of1, outlierMatches, cv::Scalar(0, 0, 255));
cv::namedWindow("Optical flow", CV_WINDOW_AUTOSIZE);
cv::imshow("Optical flow", of1);
cv::waitKey(10);
}
//std::cout << "SINGULARITIES / TOTAL: " << counter_sing << " / " << counter_total << std::endl;
return true;
}
