Precision::Int operator"" _Precision_Int_E(char const *const raw, size_t)
{
using str_type = Precision::Int::str_type;
const str_type convert(raw);
Precision::Int::size_type e_pos(convert.find('E'));
if(e_pos == str_type::npos) e_pos = convert.find('e');
if(e_pos == str_type::npos)
return Precision::Int(str_type(raw));
//Negative exponent
if(convert[e_pos+1] == '-') return 0;
str_type base(convert.substr(0, e_pos));
Precision::Int::size_type exp(0);
std::stringstream catalyst(convert.substr(e_pos+1));
catalyst >> exp;
Precision::Int::size_type point(base.find('.'));
if(point == str_type::npos || point == base.size()-1){
if(point == convert.size()-1)
base.erase(point, 1);
base.insert(base.size(), exp, '0');
return base;
}
base.erase(point, 1);
while(exp > 0 && point < base.size())
--exp, ++point;
if(point < base.size())
base.erase(point);
if(exp > 0)
base.insert(base.size(), exp, '0');
return base;
}
Precision::Float operator"" _Precision_Float_E(
char const *const raw,
size_t
){
using str_type = Precision::Float::str_type;
const str_type convert(raw);
Precision::Float::size_type e_pos(convert.find('E'));
if(e_pos == str_type::npos) e_pos = convert.find('e');
if(e_pos == str_type::npos)
return Precision::Float(str_type(raw));
Precision::Float toreturn(convert.substr(0, e_pos));
long long int exp(0);
std::istringstream catalyst(convert.substr(e_pos+1));
catalyst >> exp;
toreturn.shift(exp);
return toreturn;
}
int main(int argc, char *argv[])
{
#include "setRootCase.H"
#include "createTime.H"
#include "createMesh.H"
#include "createFields.H"
#include "readThermophysicalProperties.H"
#include "readTimeControls.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#include "readFluxScheme.H"
dimensionedScalar v_zero("v_zero", dimVolume/dimTime, 0.0);
Info<< "\nStarting time loop\n" << endl;
while (runTime.run())
{
// --- upwind interpolation of primitive fields on faces
surfaceScalarField rho_pos
(
fvc::interpolate(rho, pos, "reconstruct(rho)")
);
surfaceScalarField rho_neg
(
fvc::interpolate(rho, neg, "reconstruct(rho)")
);
surfaceVectorField rhoU_pos
(
fvc::interpolate(rhoU, pos, "reconstruct(U)")
);
surfaceVectorField rhoU_neg
(
fvc::interpolate(rhoU, neg, "reconstruct(U)")
);
volScalarField rPsi(1.0/psi);
surfaceScalarField rPsi_pos
(
fvc::interpolate(rPsi, pos, "reconstruct(T)")
);
surfaceScalarField rPsi_neg
(
fvc::interpolate(rPsi, neg, "reconstruct(T)")
);
surfaceScalarField e_pos
(
fvc::interpolate(e, pos, "reconstruct(T)")
);
surfaceScalarField e_neg
(
fvc::interpolate(e, neg, "reconstruct(T)")
);
surfaceVectorField U_pos(rhoU_pos/rho_pos);
surfaceVectorField U_neg(rhoU_neg/rho_neg);
surfaceScalarField p_pos(rho_pos*rPsi_pos);
surfaceScalarField p_neg(rho_neg*rPsi_neg);
surfaceScalarField phiv_pos(U_pos & mesh.Sf());
surfaceScalarField phiv_neg(U_neg & mesh.Sf());
volScalarField c(sqrt(thermo.Cp()/thermo.Cv()*rPsi));
surfaceScalarField cSf_pos
(
fvc::interpolate(c, pos, "reconstruct(T)")*mesh.magSf()
);
surfaceScalarField cSf_neg
(
fvc::interpolate(c, neg, "reconstruct(T)")*mesh.magSf()
);
surfaceScalarField ap
(
max(max(phiv_pos + cSf_pos, phiv_neg + cSf_neg), v_zero)
);
surfaceScalarField am
(
min(min(phiv_pos - cSf_pos, phiv_neg - cSf_neg), v_zero)
);
surfaceScalarField a_pos(ap/(ap - am));
surfaceScalarField amaxSf("amaxSf", max(mag(am), mag(ap)));
surfaceScalarField aSf(am*a_pos);
if (fluxScheme == "Tadmor")
{
aSf = -0.5*amaxSf;
a_pos = 0.5;
}
surfaceScalarField a_neg(1.0 - a_pos);
phiv_pos *= a_pos;
phiv_neg *= a_neg;
surfaceScalarField aphiv_pos(phiv_pos - aSf);
surfaceScalarField aphiv_neg(phiv_neg + aSf);
// Reuse amaxSf for the maximum positive and negative fluxes
// estimated by the central scheme
amaxSf = max(mag(aphiv_pos), mag(aphiv_neg));
#include "compressibleCourantNo.H"
#include "readTimeControls.H"
#include "setDeltaT.H"
runTime++;
Info<< "Time = " << runTime.timeName() << nl << endl;
phi = aphiv_pos*rho_pos + aphiv_neg*rho_neg;
surfaceVectorField phiUp
(
(aphiv_pos*rhoU_pos + aphiv_neg*rhoU_neg)
+ (a_pos*p_pos + a_neg*p_neg)*mesh.Sf()
);
surfaceScalarField phiEp
(
aphiv_pos*(rho_pos*(e_pos + 0.5*magSqr(U_pos)) + p_pos)
+ aphiv_neg*(rho_neg*(e_neg + 0.5*magSqr(U_neg)) + p_neg)
+ aSf*p_pos - aSf*p_neg
);
volScalarField muEff(turbulence->muEff());
volTensorField tauMC("tauMC", muEff*dev2(Foam::T(fvc::grad(U))));
// --- Solve density
solve(fvm::ddt(rho) + fvc::div(phi));
// --- Solve momentum
solve(fvm::ddt(rhoU) + fvc::div(phiUp));
U.dimensionedInternalField() =
rhoU.dimensionedInternalField()
/rho.dimensionedInternalField();
U.correctBoundaryConditions();
rhoU.boundaryField() = rho.boundaryField()*U.boundaryField();
volScalarField rhoBydt(rho/runTime.deltaT());
if (!inviscid)
{
solve
(
fvm::ddt(rho, U) - fvc::ddt(rho, U)
- fvm::laplacian(muEff, U)
- fvc::div(tauMC)
);
rhoU = rho*U;
}
// --- Solve energy
surfaceScalarField sigmaDotU
(
(
fvc::interpolate(muEff)*mesh.magSf()*fvc::snGrad(U)
+ (mesh.Sf() & fvc::interpolate(tauMC))
)
& (a_pos*U_pos + a_neg*U_neg)
);
solve
(
fvm::ddt(rhoE)
+ fvc::div(phiEp)
- fvc::div(sigmaDotU)
);
e = rhoE/rho - 0.5*magSqr(U);
e.correctBoundaryConditions();
thermo.correct();
rhoE.boundaryField() =
rho.boundaryField()*
(
e.boundaryField() + 0.5*magSqr(U.boundaryField())
);
if (!inviscid)
{
volScalarField k("k", thermo.Cp()*muEff/Pr);
solve
(
fvm::ddt(rho, e) - fvc::ddt(rho, e)
- fvm::laplacian(turbulence->alphaEff(), e)
+ fvc::laplacian(turbulence->alpha(), e)
- fvc::laplacian(k, T)
);
thermo.correct();
rhoE = rho*(e + 0.5*magSqr(U));
}
p.dimensionedInternalField() =
rho.dimensionedInternalField()
/psi.dimensionedInternalField();
p.correctBoundaryConditions();
rho.boundaryField() = psi.boundaryField()*p.boundaryField();
turbulence->correct();
runTime.write();
Info<< "ExecutionTime = " << runTime.elapsedCpuTime() << " s"
<< " ClockTime = " << runTime.elapsedClockTime() << " s"
<< nl << endl;
}
Info<< "End\n" << endl;
return 0;
}
int main(int argc, char *argv[])
{
#include "setRootCase.H"
#include "createTime.H"
#include "createMesh.H"
#include "createFields.H"
#include "createTimeControls.H"
#include "createRDeltaT.H"
turbulence->validate();
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#include "readFluxScheme.H"
dimensionedScalar v_zero("v_zero", dimVolume/dimTime, 0.0);
// Courant numbers used to adjust the time-step
scalar CoNum = 0.0;
scalar meanCoNum = 0.0;
Info<< "\nStarting time loop\n" << endl;
while (runTime.run())
{
// --- Directed interpolation of primitive fields onto faces
surfaceScalarField rho_pos(interpolate(rho, pos));
surfaceScalarField rho_neg(interpolate(rho, neg));
surfaceVectorField rhoU_pos(interpolate(rhoU, pos, U.name()));
surfaceVectorField rhoU_neg(interpolate(rhoU, neg, U.name()));
volScalarField rPsi("rPsi", 1.0/psi);
surfaceScalarField rPsi_pos(interpolate(rPsi, pos, T.name()));
surfaceScalarField rPsi_neg(interpolate(rPsi, neg, T.name()));
surfaceScalarField e_pos(interpolate(e, pos, T.name()));
surfaceScalarField e_neg(interpolate(e, neg, T.name()));
surfaceVectorField U_pos("U_pos", rhoU_pos/rho_pos);
surfaceVectorField U_neg("U_neg", rhoU_neg/rho_neg);
surfaceScalarField p_pos("p_pos", rho_pos*rPsi_pos);
surfaceScalarField p_neg("p_neg", rho_neg*rPsi_neg);
surfaceScalarField phiv_pos("phiv_pos", U_pos & mesh.Sf());
surfaceScalarField phiv_neg("phiv_neg", U_neg & mesh.Sf());
volScalarField c("c", sqrt(thermo.Cp()/thermo.Cv()*rPsi));
surfaceScalarField cSf_pos
(
"cSf_pos",
interpolate(c, pos, T.name())*mesh.magSf()
);
surfaceScalarField cSf_neg
(
"cSf_neg",
interpolate(c, neg, T.name())*mesh.magSf()
);
surfaceScalarField ap
(
"ap",
max(max(phiv_pos + cSf_pos, phiv_neg + cSf_neg), v_zero)
);
surfaceScalarField am
(
"am",
min(min(phiv_pos - cSf_pos, phiv_neg - cSf_neg), v_zero)
);
surfaceScalarField a_pos("a_pos", ap/(ap - am));
surfaceScalarField amaxSf("amaxSf", max(mag(am), mag(ap)));
surfaceScalarField aSf("aSf", am*a_pos);
if (fluxScheme == "Tadmor")
{
aSf = -0.5*amaxSf;
a_pos = 0.5;
}
surfaceScalarField a_neg("a_neg", 1.0 - a_pos);
phiv_pos *= a_pos;
phiv_neg *= a_neg;
surfaceScalarField aphiv_pos("aphiv_pos", phiv_pos - aSf);
surfaceScalarField aphiv_neg("aphiv_neg", phiv_neg + aSf);
// Reuse amaxSf for the maximum positive and negative fluxes
// estimated by the central scheme
amaxSf = max(mag(aphiv_pos), mag(aphiv_neg));
#include "centralCourantNo.H"
#include "readTimeControls.H"
if (LTS)
{
#include "setRDeltaT.H"
}
else
{
#include "setDeltaT.H"
}
runTime++;
Info<< "Time = " << runTime.timeName() << nl << endl;
phi = aphiv_pos*rho_pos + aphiv_neg*rho_neg;
surfaceVectorField phiUp
(
(aphiv_pos*rhoU_pos + aphiv_neg*rhoU_neg)
+ (a_pos*p_pos + a_neg*p_neg)*mesh.Sf()
);
surfaceScalarField phiEp
(
"phiEp",
aphiv_pos*(rho_pos*(e_pos + 0.5*magSqr(U_pos)) + p_pos)
+ aphiv_neg*(rho_neg*(e_neg + 0.5*magSqr(U_neg)) + p_neg)
+ aSf*p_pos - aSf*p_neg
);
volScalarField muEff("muEff", turbulence->muEff());
volTensorField tauMC("tauMC", muEff*dev2(Foam::T(fvc::grad(U))));
// --- Solve density
solve(fvm::ddt(rho) + fvc::div(phi));
// --- Solve momentum
solve(fvm::ddt(rhoU) + fvc::div(phiUp));
U.dimensionedInternalField() =
rhoU.dimensionedInternalField()
/rho.dimensionedInternalField();
U.correctBoundaryConditions();
rhoU.boundaryField() == rho.boundaryField()*U.boundaryField();
if (!inviscid)
{
solve
(
fvm::ddt(rho, U) - fvc::ddt(rho, U)
- fvm::laplacian(muEff, U)
- fvc::div(tauMC)
);
rhoU = rho*U;
}
// --- Solve energy
surfaceScalarField sigmaDotU
(
"sigmaDotU",
(
fvc::interpolate(muEff)*mesh.magSf()*fvc::snGrad(U)
+ (mesh.Sf() & fvc::interpolate(tauMC))
)
& (a_pos*U_pos + a_neg*U_neg)
);
solve
(
fvm::ddt(rhoE)
+ fvc::div(phiEp)
- fvc::div(sigmaDotU)
);
e = rhoE/rho - 0.5*magSqr(U);
e.correctBoundaryConditions();
thermo.correct();
rhoE.boundaryField() ==
rho.boundaryField()*
(
e.boundaryField() + 0.5*magSqr(U.boundaryField())
);
if (!inviscid)
{
solve
(
fvm::ddt(rho, e) - fvc::ddt(rho, e)
- fvm::laplacian(turbulence->alphaEff(), e)
);
thermo.correct();
rhoE = rho*(e + 0.5*magSqr(U));
}
p.dimensionedInternalField() =
rho.dimensionedInternalField()
/psi.dimensionedInternalField();
p.correctBoundaryConditions();
rho.boundaryField() == psi.boundaryField()*p.boundaryField();
turbulence->correct();
runTime.write();
Info<< "ExecutionTime = " << runTime.elapsedCpuTime() << " s"
<< " ClockTime = " << runTime.elapsedClockTime() << " s"
<< nl << endl;
}
Info<< "End\n" << endl;
return 0;
}
void SparseTrackerAM::pointCloudCallback(const PointCloudT::ConstPtr& cloud_in_ptr)
{
boost::mutex::scoped_lock(mutex_);
struct timeval start_callback, end_callback;
struct timeval start_features, end_features;
struct timeval start_model, end_model;
struct timeval start_icp, end_icp;
gettimeofday(&start_callback, NULL);
// **** initialize ***********************************************************
if (!initialized_)
{
initialized_ = getBaseToCameraTf(cloud_in_ptr);
if (!initialized_) return;
}
// **** extract features *****************************************************
gettimeofday(&start_features, NULL);
// **** ab prediction
ros::Time cur_time = cloud_in_ptr->header.stamp;
double dt = (cur_time - prev_time_).toSec();
prev_time_ = cur_time;
tf::Transform predicted_f2b;
double pr_x, pr_y, pr_z, pr_roll, pr_pitch, pr_yaw;
if (use_alpha_beta_)
{
double cx, cy, cz, croll, cpitch, cyaw;
getXYZRPY(f2b_, cx, cy, cz, croll, cpitch, cyaw);
pr_x = cx + v_x_ * dt;
pr_y = cy + v_y_ * dt;
pr_z = cz + v_z_ * dt;
pr_roll = croll + v_roll_ * dt;
pr_pitch = cpitch + v_pitch_ * dt;
pr_yaw = cyaw + v_yaw_ * dt;
btVector3 pr_pos(pr_x, pr_y, pr_z);
btQuaternion pr_q;
pr_q.setRPY(pr_roll, pr_pitch, pr_yaw);
predicted_f2b.setOrigin(pr_pos);
predicted_f2b.setRotation(pr_q);
}
else
{
predicted_f2b = f2b_;
}
PointCloudOrb::Ptr orb_features_ptr = boost::make_shared<PointCloudOrb>();
bool orb_extract_result = extractOrbFeatures(cloud_in_ptr, orb_features_ptr);
pcl::transformPointCloud (*orb_features_ptr , *orb_features_ptr, eigenFromTf(predicted_f2b * b2c_));
orb_features_ptr->header.frame_id = fixed_frame_;
// FIXME - removed canny
PointCloudCanny::Ptr canny_features_ptr = boost::make_shared<PointCloudCanny>();
bool canny_extract_result;// = extractCannyFeatures(cloud_in_ptr, canny_features_ptr);
pcl::transformPointCloud (*canny_features_ptr , *canny_features_ptr, eigenFromTf(predicted_f2b * b2c_));
canny_features_ptr->header.frame_id = fixed_frame_;
gettimeofday(&end_features, NULL);
// **** ICP ******************************
gettimeofday(&start_icp, NULL);
bool orb_icp_result = false;
bool canny_icp_result = false;
if (orb_extract_result)
{
//printf("~ORB~\n");
orb_icp_result = OrbICP(orb_features_ptr);
double eps_roll_ = 0.3;
double eps_pitch_ = 0.3;
double eps_yaw_ = 0.3;
double eps_x_ = 0.3;
double eps_y_ = 0.3;
double eps_z_ = 0.3;
if (orb_icp_result)
{
tf::Transform corr = tfFromEigen(orb_reg_.getFinalTransformation());
// particles add error to motion
double c_x, c_y, c_z, c_roll, c_pitch, c_yaw;
double e_x, e_y, e_z, e_roll, e_pitch, e_yaw;
getXYZRPY(corr, c_x, c_y, c_z, c_roll, c_pitch, c_yaw);
for (int i = 0; i < 20; i++)
{
e_roll = getUrand() * eps_roll_ * c_roll;
e_pitch = getUrand() * eps_pitch_ * c_pitch;
e_yaw = getUrand() * eps_yaw_ * c_yaw;
e_x = getUrand() * eps_x_ * c_x;
e_y = getUrand() * eps_y_ * c_y;
e_z = getUrand() * eps_z_ * c_z;
btVector3 e_pos(c_x + e_x, c_y + e_y, c_z + e_z);
btQuaternion e_q;
e_q.setRPY(c_roll + e_roll, c_pitch + e_pitch, c_yaw + e_yaw);
tf::Transform e_corr;
e_corr.setOrigin(e_pos);
e_corr.setRotation(e_q);
pf2b_[i] = e_corr * pf2b_[i];
}
// end particles
tf::Transform measured_f2b = corr * predicted_f2b;
// **** ab estmation
if (use_alpha_beta_)
{
double m_x, m_y, m_z, m_roll, m_pitch, m_yaw;
getXYZRPY(measured_f2b, m_x, m_y, m_z, m_roll, m_pitch, m_yaw);
// residuals
double r_x = m_x - pr_x;
double r_y = m_y - pr_y;
double r_z = m_z - pr_z;
double r_roll = m_roll - pr_roll;
double r_pitch = m_pitch - pr_pitch;
double r_yaw = m_yaw - pr_yaw;
fixAngleD(r_roll);
fixAngleD(r_pitch);
fixAngleD(r_yaw);
// final position
double f_x = pr_x + alpha_ * r_x;
double f_y = pr_y + alpha_ * r_y;
double f_z = pr_z + alpha_ * r_z;
double f_roll = pr_roll + alpha_ * r_roll;
double f_pitch = pr_pitch + alpha_ * r_pitch;
double f_yaw = pr_yaw + alpha_ * r_yaw;
btVector3 f_pos(f_x, f_y, f_z);
btQuaternion f_q;
f_q.setRPY(f_roll, f_pitch, f_yaw);
f2b_.setOrigin(f_pos);
f2b_.setRotation(f_q);
// final velocity
v_x_ = v_x_ + beta_ * r_x / dt;
v_y_ = v_y_ + beta_ * r_y / dt;
v_z_ = v_z_ + beta_ * r_z / dt;
v_roll_ = v_roll_ + beta_ * r_roll / dt;
v_pitch_ = v_pitch_ + beta_ * r_pitch / dt;
v_yaw_ = v_yaw_ + beta_ * r_yaw / dt;
//printf("VEL: %f, %f, %f\n", v_x_, v_y_, v_z_);
}
else
{
f2b_ = measured_f2b;
}
}
}
if (!orb_icp_result)
printf("ERROR\n");
gettimeofday(&end_icp, NULL);
// **** ADD features to model
gettimeofday(&start_model, NULL);
if (model_->points.size() == 0)
{
*model_ += *orb_features_ptr;
}
else
{
pcl::KdTreeFLANN<PointOrb> tree_model;
tree_model.setInputCloud(model_);
std::vector<int> indices;
std::vector<float> distances;
indices.resize(1);
distances.resize(1);
for (int i = 0; i < orb_features_ptr->points.size(); ++i)
{
PointOrb& p = orb_features_ptr->points[i];
int n_found = tree_model.nearestKSearch(p, 1, indices, distances);
if (n_found == 0)
{
model_->points.push_back(p);
model_->width++;
}
else
{
if ( distances[0] > 0.01)
{
//found far away, insert new one
model_->points.push_back(p);
model_->width++;
}
else
{
// found near, modify old one
//PointOrb& q = model_->points[indices[0]];
//q.x = 0.5*(p.x + q.x);
//q.y = 0.5*(p.y + q.y);
//q.z = 0.5*(p.z + q.z);
}
}
}
}
gettimeofday(&end_model, NULL);
// *** broadcast tf **********************************************************
broadcastTF(cloud_in_ptr);
// *** counter **************************************************************
frame_count_++;
// **** print diagnostics ****************************************************
gettimeofday(&end_callback, NULL);
double features_dur = msDuration(start_features, end_features);
double model_dur = msDuration(start_model, end_model);
double icp_dur = msDuration(start_icp, end_icp);
double callback_dur = msDuration(start_callback, end_callback);
//int model_frames = orb_history_.getSize();
int icp_iterations = orb_reg_.getFinalIterations();
int orb_features_count = orb_features_ptr->points.size();
int canny_features_count =canny_features_ptr->points.size();
int model_size = model_->points.size();
printf("F[%d][%d] %.1f \t M[%d] %.1f \t ICP[%d] %.1f \t TOTAL %.1f\n",
orb_features_count, canny_features_count, features_dur,
model_size, model_dur,
icp_iterations,
icp_dur, callback_dur);
}
