Beispiel #1
0
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);
}
Beispiel #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;
}
Beispiel #9
0
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;    
}