/* The Hantush Function
* The function is calculated by logarithmic integration using Simpson's rule
* source: W.Kinzelbach - Groundwater modelling
*/
double W( double ra, double rb )
{
double ug, og, hi, sub1, sub2, x2, x4, s2, s4;
int i;
if( ra < 1e-20 )
return( 2 * K0( rb ) );
if( ra > 2000.0 )
return( 0.0 );
ug = log( ra );
og = 10.0;
hi = ( og - ug ) / 24;
sub1 = rb * rb / 4.0; sub2 = hi * 2;
x4 = ug + hi; x2 = ug;
s4 = s2 = 0.0; i = 0;
while( 1 )
{
s4 += exp( - exp( x4 ) - sub1 / exp( x4 ) );
x4 += sub2;
if( i == 10 ) break;
x2 += sub2;
s2 += exp( - exp( x2 ) - sub1 / exp( x2 ) );
i++;
}
return( hi * ( exp( - exp( ug ) - sub1 / exp( ug ) ) + 4 * s4 + 2 * s2 + exp( - exp( og ) - sub1 / exp( og ) ) ) / 3 );
}
double KN(int n, double x){//Modified Bessel function of the second kindlol
int i;
double k0=K0(x),k1=K1(x),kn=0;
if(n==0)return k0;
else if(n==1)return k1;
for(i=1;i<n;i++){
kn = k0+(2./x)*i*k1;
k0=k1;
k1=kn;
}
return kn;
}
void bessel_k(_FLOAT_ x, _FLOAT_ k[], int n)
{
int l;
_FLOAT_ one_over_x;
one_over_x = 1.0/x;
k[0] = K0(x);
k[1] = K1(x);
for (l=2; l<n; l++) {
k[l] = (2*l-2)*k[l-1]*one_over_x + k[l-2];
}
}
static
V
calc_edge_score(const boost::multi_array<V,2>& si_subst,
const std::vector<V>& gap,
const Data& xx, const Data& yy,
uint x_i, uint x_j, uint y_i, uint y_j)
{
typedef V value_type;
typedef typename Data::Seq Seq;
typedef boost::multi_array<value_type,2> dp_type;
const Seq& x(xx.seq);
const Seq& y(yy.seq);
const std::vector<float>& w_x(xx.weight);
const std::vector<float>& w_y(yy.weight);
uint sz_x = x_i<=x_j ? x_j-x_i+1 : 0;
uint sz_y = y_i<=y_j ? y_j-y_i+1 : 0;
if (sz_x==0) { return gap[sz_y]; }
if (sz_y==0) { return gap[sz_x]; }
dp_type K0(boost::extents[sz_x+1][sz_y+1]);
dp_type G0(boost::extents[sz_x+1][sz_y+1]);
std::vector<value_type> K1(sz_y+1);
std::vector<value_type> G1(sz_y+1);
K0[0][0] = G0[0][0] = 1.0;
for (uint i=1; i!=sz_x+1; ++i) {
K0[i][0] = 1.0;
G0[i][0] = G0[i-1][0]*gap[1];
}
for (uint j=1; j!=sz_y+1; ++j) {
K0[0][j] = 1.0;
G0[0][j] = G0[0][j-1]*gap[1];
}
for (uint i=1; i!=sz_x+1; ++i) {
K1[0] = G1[0] = 0.0;
uint ii=x_i+i;
for (uint j=1; j!=sz_y+1; ++j) {
uint jj=y_i+j;
value_type v = G0[i-1][j-1];
v *= subst_score(si_subst,x[ii-1],y[jj-1]);
v *= w_x[ii-1]*w_y[jj-1];
K1[j] = v + K1[j-1];
G1[j] = v + G1[j-1]*gap[1];
K0[i][j] = K1[j] + K0[i-1][j];
G0[i][j] = G1[j] + G0[i-1][j]*gap[1];
}
}
return K0[sz_x][sz_y];
}
void bessel_k_scaled(_FLOAT_ x, _FLOAT_ k[], int n)
/* computes k_n(x)*x^(n+1) */
{
int l;
_FLOAT_ x2;
x2 = x*x;
k[0] = K0(x)*x;
k[1] = K1(x)*x2;
for (l=2; l<n; l++) {
k[l] = (2*l-2)*k[l-1] + x2*k[l-2];
}
}
int main( int argc, char* argv[] )
{
// Initialise window
pangolin::View& container = SetupPangoGLWithCuda(1024, 768);
size_t cu_mem_start, cu_mem_end, cu_mem_total;
cudaMemGetInfo( &cu_mem_start, &cu_mem_total );
glClearColor(1,1,1,0);
// Open video device
hal::Camera video = OpenRpgCamera(argc,argv);
// Capture first image
pb::ImageArray images;
// N cameras, each w*h in dimension, greyscale
const size_t N = video.NumChannels();
if( N != 2 ) {
std::cerr << "Two images are required to run this program!" << std::endl;
exit(1);
}
const size_t nw = video.Width();
const size_t nh = video.Height();
// Capture first image
video.Capture(images);
// Downsample this image to process less pixels
const int max_levels = 6;
const int level = roo::GetLevelFromMaxPixels( nw, nh, 640*480 );
// const int level = 4;
assert(level <= max_levels);
// Find centered image crop which aligns to 16 pixels at given level
const NppiRect roi = roo::GetCenteredAlignedRegion(nw,nh,16 << level,16 << level);
// Load Camera intrinsics from file
GetPot clArgs( argc, argv );
const std::string filename = clArgs.follow("","-cmod");
if( filename.empty() ) {
std::cerr << "Camera models file is required!" << std::endl;
exit(1);
}
const calibu::CameraRig rig = calibu::ReadXmlRig(filename);
if( rig.cameras.size() != 2 ) {
std::cerr << "Two camera models are required to run this program!" << std::endl;
exit(1);
}
Eigen::Matrix3f CamModel0 = rig.cameras[0].camera.K().cast<float>();
Eigen::Matrix3f CamModel1 = rig.cameras[1].camera.K().cast<float>();
roo::ImageIntrinsics camMod[] = {
{CamModel0(0,0),CamModel0(1,1),CamModel0(0,2),CamModel0(1,2)},
{CamModel1(0,0),CamModel1(1,1),CamModel1(0,2),CamModel1(1,2)}
};
for(int i=0; i<2; ++i ) {
// Adjust to match camera image dimensions
const double scale = nw / rig.cameras[i].camera.Width();
roo::ImageIntrinsics camModel = camMod[i].Scale( scale );
// Adjust to match cropped aligned image
camModel = camModel.CropToROI( roi );
camMod[i] = camModel;
}
const unsigned int w = roi.width;
const unsigned int h = roi.height;
const unsigned int lw = w >> level;
const unsigned int lh = h >> level;
const Eigen::Matrix3d& K0 = camMod[0].Matrix();
const Eigen::Matrix3d& Kl = camMod[0][level].Matrix();
std::cout << "K Matrix: " << std::endl << K0 << std::endl;
std::cout << "K Matrix - Level: " << std::endl << Kl << std::endl;
std::cout << "Video stream dimensions: " << nw << "x" << nh << std::endl;
std::cout << "Chosen Level: " << level << std::endl;
std::cout << "Processing dimensions: " << lw << "x" << lh << std::endl;
std::cout << "Offset: " << roi.x << "x" << roi.y << std::endl;
// print selected camera model
std::cout << "Camera Model used: " << std::endl << camMod[0][level].Matrix() << std::endl;
Eigen::Matrix3d RDFvision;RDFvision<< 1,0,0, 0,1,0, 0,0,1;
Eigen::Matrix3d RDFrobot; RDFrobot << 0,1,0, 0,0, 1, 1,0,0;
Eigen::Matrix4d T_vis_ro = Eigen::Matrix4d::Identity();
T_vis_ro.block<3,3>(0,0) = RDFvision.transpose() * RDFrobot;
Eigen::Matrix4d T_ro_vis = Eigen::Matrix4d::Identity();
T_ro_vis.block<3,3>(0,0) = RDFrobot.transpose() * RDFvision;
const Sophus::SE3d T_rl_orig = T_rlFromCamModelRDF(rig.cameras[0], rig.cameras[1], RDFvision);
// TODO(jmf): For now, assume cameras are rectified. Later allow unrectified cameras.
/*
double k1 = 0;
double k2 = 0;
if(cam[0].Type() == MVL_CAMERA_WARPED)
{
k1 = cam[0].GetModel()->warped.kappa1;
k2 = cam[0].GetModel()->warped.kappa2;
}
*/
const bool rectify = false;
if(!rectify) {
std::cout << "Using pre-rectified images" << std::endl;
}
// Check we received at least two images
if(images.Size() < 2) {
std::cerr << "Failed to capture first stereo pair from camera" << std::endl;
return -1;
}
// Define Camera Render Object (for view / scene browsing)
pangolin::OpenGlRenderState s_cam(
pangolin::ProjectionMatrixRDF_TopLeft(w,h,K0(0,0),K0(1,1),K0(0,2),K0(1,2),0.1,10000),
pangolin::IdentityMatrix(pangolin::GlModelViewStack)
);
pangolin::GlBufferCudaPtr vbo(pangolin::GlArrayBuffer, lw*lh,GL_FLOAT, 4, cudaGraphicsMapFlagsWriteDiscard, GL_STREAM_DRAW );
pangolin::GlBufferCudaPtr cbo(pangolin::GlArrayBuffer, lw*lh,GL_UNSIGNED_BYTE, 4, cudaGraphicsMapFlagsWriteDiscard, GL_STREAM_DRAW );
pangolin::GlBuffer ibo = pangolin::MakeTriangleStripIboForVbo(lw,lh);
// Allocate Camera Images on device for processing
roo::Image<unsigned char, roo::TargetHost, roo::DontManage> hCamImg[] = {{0,nw,nh},{0,nw,nh}};
roo::Image<float2, roo::TargetDevice, roo::Manage> dLookup[] = {{w,h},{w,h}};
roo::Image<unsigned char, roo::TargetDevice, roo::Manage> upload(w,h);
roo::Pyramid<unsigned char, max_levels, roo::TargetDevice, roo::Manage> img_pyr[] = {{w,h},{w,h}};
roo::Image<float, roo::TargetDevice, roo::Manage> img[] = {{lw,lh},{lw,lh}};
roo::Volume<float, roo::TargetDevice, roo::Manage> vol[] = {{lw,lh,MAXD},{lw,lh,MAXD}};
roo::Image<float, roo::TargetDevice, roo::Manage> disp[] = {{lw,lh},{lw,lh}};
roo::Image<float, roo::TargetDevice, roo::Manage> meanI(lw,lh);
roo::Image<float, roo::TargetDevice, roo::Manage> varI(lw,lh);
roo::Image<float, roo::TargetDevice, roo::Manage> temp[] = {{lw,lh},{lw,lh},{lw,lh},{lw,lh},{lw,lh}};
roo::Image<float,roo::TargetDevice, roo::Manage>& imgd = disp[0];
roo::Image<float,roo::TargetDevice, roo::Manage> depthmap(lw,lh);
roo::Image<float,roo::TargetDevice, roo::Manage> imga(lw,lh);
roo::Image<float2,roo::TargetDevice, roo::Manage> imgq(lw,lh);
roo::Image<float,roo::TargetDevice, roo::Manage> imgw(lw,lh);
roo::Image<float4, roo::TargetDevice, roo::Manage> d3d(lw,lh);
roo::Image<unsigned char, roo::TargetDevice,roo::Manage> Scratch(lw*sizeof(roo::LeastSquaresSystem<float,6>),lh);
typedef ulong4 census_t;
roo::Image<census_t, roo::TargetDevice, roo::Manage> census[] = {{lw,lh},{lw,lh}};
// Stereo transformation (post-rectification)
Sophus::SE3d T_rl = T_rl_orig;
const double baseline = T_rl.translation().norm();
std::cout << "Baseline: " << baseline << std::endl;
cudaMemGetInfo( &cu_mem_end, &cu_mem_total );
std::cout << "CuTotal: " << cu_mem_total/(1024*1024) << ", Available: " << cu_mem_end/(1024*1024) << ", Used: " << (cu_mem_start-cu_mem_end)/(1024*1024) << std::endl;
pangolin::Var<bool> step("ui.step", false, false);
pangolin::Var<bool> run("ui.run", false, true);
pangolin::Var<bool> lockToCam("ui.Lock to cam", false, true);
pangolin::Var<int> show_slice("ui.show slice",MAXD/2, 0, MAXD-1);
pangolin::Var<int> maxdisp("ui.maxdisp",MAXD, 0, MAXD);
pangolin::Var<bool> subpix("ui.subpix", true, true);
pangolin::Var<bool> use_census("ui.use census", true, true);
pangolin::Var<int> avg_rad("ui.avg_rad",0, 0, 100);
pangolin::Var<bool> do_dtam("ui.do dtam", false, true);
pangolin::Var<bool> dtam_reset("ui.reset", false, false);
pangolin::Var<float> g_alpha("ui.g alpha", 14, 0,4);
pangolin::Var<float> g_beta("ui.g beta", 2.5, 0,2);
pangolin::Var<float> theta("ui.theta", 100, 0,100);
pangolin::Var<float> lambda("ui.lambda", 20, 0,20);
pangolin::Var<float> sigma_q("ui.sigma q", 0.7, 0, 1);
pangolin::Var<float> sigma_d("ui.sigma d", 0.7, 0, 1);
pangolin::Var<float> huber_alpha("ui.huber alpha", 0.002, 0, 0.01);
pangolin::Var<float> beta("ui.beta", 0.00001, 0, 0.01);
pangolin::Var<float> alpha("ui.alpha", 0.9, 0,1);
pangolin::Var<float> r1("ui.r1", 100, 0,0.01);
pangolin::Var<float> r2("ui.r2", 100, 0,0.01);
pangolin::Var<bool> filter("ui.filter", false, true);
pangolin::Var<float> eps("ui.eps",0.01*0.01, 0, 0.01);
pangolin::Var<int> rad("ui.radius",9, 1, 20);
pangolin::Var<bool> leftrightcheck("ui.left-right check", false, true);
pangolin::Var<float> maxdispdiff("ui.maxdispdiff",1, 0, 5);
pangolin::Var<int> domedits("ui.median its",1, 1, 10);
pangolin::Var<bool> domed9x9("ui.median 9x9", false, true);
pangolin::Var<bool> domed7x7("ui.median 7x7", false, true);
pangolin::Var<bool> domed5x5("ui.median 5x5", false, true);
pangolin::Var<int> medi("ui.medi",12, 0, 24);
pangolin::Var<float> filtgradthresh("ui.filt grad thresh", 0, 0, 20);
pangolin::Var<bool> save_depthmaps("ui.save_depthmaps", false, true);
int jump_frames = 0;
pangolin::RegisterKeyPressCallback(' ', [&run](){run = !run;} );
pangolin::RegisterKeyPressCallback('l', [&lockToCam](){lockToCam = !lockToCam;} );
pangolin::RegisterKeyPressCallback(pangolin::PANGO_SPECIAL + GLUT_KEY_RIGHT, [&step](){step=true;} );
pangolin::RegisterKeyPressCallback(']', [&jump_frames](){jump_frames=100;} );
pangolin::RegisterKeyPressCallback('}', [&jump_frames](){jump_frames=1000;} );
pangolin::Handler2dImageSelect handler2d(lw,lh,level);
// ActivateDrawPyramid<unsigned char,max_levels> adleft(img_pyr[0],GL_LUMINANCE8, false, true);
// ActivateDrawPyramid<unsigned char,max_levels> adright(img_pyr[1],GL_LUMINANCE8, false, true);
pangolin::ActivateDrawImage<float> adleft(img[0],GL_LUMINANCE32F_ARB, false, true);
pangolin::ActivateDrawImage<float> adright(img[1],GL_LUMINANCE32F_ARB, false, true);
pangolin::ActivateDrawImage<float> adisp(disp[0],GL_LUMINANCE32F_ARB, false, true);
pangolin::ActivateDrawImage<float> adw(imgw,GL_LUMINANCE32F_ARB, false, true);
// ActivateDrawImage<float> adCrossSection(dCrossSection,GL_RGBA_FLOAT32_APPLE, false, true);
pangolin::ActivateDrawImage<float> adVol(vol[0].ImageXY(show_slice),GL_LUMINANCE32F_ARB, false, true);
SceneGraph::GLSceneGraph graph;
SceneGraph::GLVbo glvbo(&vbo,&ibo,&cbo);
graph.AddChild(&glvbo);
SetupContainer(container, 6, (float)w/h);
container[0].SetDrawFunction(boost::ref(adleft)).SetHandler(&handler2d);
container[1].SetDrawFunction(boost::ref(adright)).SetHandler(&handler2d);
container[2].SetDrawFunction(boost::ref(adisp)).SetHandler(&handler2d);
container[3].SetDrawFunction(boost::ref(adVol)).SetHandler(&handler2d);
container[4].SetDrawFunction(SceneGraph::ActivateDrawFunctor(graph, s_cam))
.SetHandler( new pangolin::Handler3D(s_cam, pangolin::AxisNone) );
container[5].SetDrawFunction(boost::ref(adw)).SetHandler(&handler2d);
for(unsigned long frame=0; !pangolin::ShouldQuit();)
{
bool go = frame==0 || jump_frames > 0 || run || Pushed(step);
for(; jump_frames > 0; jump_frames--) {
video.Capture(images);
}
if(go) {
if(frame>0) {
if( video.Capture(images) == false) {
exit(1);
}
}
frame++;
/////////////////////////////////////////////////////////////
// Upload images to device (Warp / Decimate if necessery)
for(int i=0; i<2; ++i ) {
hCamImg[i].ptr = images[i].data();
if(rectify) {
upload.CopyFrom(hCamImg[i].SubImage(roi));
Warp(img_pyr[i][0], upload, dLookup[i]);
}else{
img_pyr[i][0].CopyFrom(hCamImg[i].SubImage(roi));
}
roo::BoxReduce<unsigned char, max_levels, unsigned int>(img_pyr[i]);
}
}
go |= avg_rad.GuiChanged() | use_census.GuiChanged();
if( go ) {
for(int i=0; i<2; ++i ) {
roo::ElementwiseScaleBias<float,unsigned char,float>(img[i], img_pyr[i][level],1.0f/255.0f);
if(avg_rad > 0 ) {
roo::BoxFilter<float,float,float>(temp[0],img[i],Scratch,avg_rad);
roo::ElementwiseAdd<float,float,float,float>(img[i], img[i], temp[0], 1, -1, 0.5);
}
if(use_census) {
Census(census[i], img[i]);
}
}
}
if( go | g_alpha.GuiChanged() || g_beta.GuiChanged() ) {
ExponentialEdgeWeight(imgw, img[0], g_alpha, g_beta);
}
go |= filter.GuiChanged() | leftrightcheck.GuiChanged() | rad.GuiChanged() | eps.GuiChanged() | alpha.GuiChanged() | r1.GuiChanged() | r2.GuiChanged();
if(go) {
if(use_census) {
roo::CensusStereoVolume<float, census_t>(vol[0], census[0], census[1], maxdisp, -1);
if(leftrightcheck) roo::CensusStereoVolume<float, census_t>(vol[1], census[1], census[0], maxdisp, +1);
}else{
CostVolumeFromStereoTruncatedAbsAndGrad(vol[0], img[0], img[1], -1, alpha, r1, r2);
if(leftrightcheck) CostVolumeFromStereoTruncatedAbsAndGrad(vol[1], img[1], img[0], +1, alpha, r1, r2);
}
if(filter) {
// Filter Cost volume
for(int v=0; v<(leftrightcheck?2:1); ++v)
{
roo::Image<float, roo::TargetDevice, roo::Manage>& I = img[v];
roo::ComputeMeanVarience<float,float,float>(varI, temp[0], meanI, I, Scratch, rad);
for(int d=0; d<maxdisp; ++d)
{
roo::Image<float> P = vol[v].ImageXY(d);
roo::ComputeCovariance(temp[0],temp[2],temp[1],P,meanI,I,Scratch,rad);
GuidedFilter(P,temp[0],varI,temp[1],meanI,I,Scratch,temp[2],temp[3],temp[4],rad,eps);
}
}
}
}
static int n = 0;
// static float theta = 0;
// go |= Pushed(dtam_reset);
// if(go )
if(Pushed(dtam_reset))
{
n = 0;
theta.Reset();
// Initialise primal and auxillary variables
CostVolMinimumSubpix(imgd,vol[0], maxdisp,-1);
imga.CopyFrom(imgd);
// Initialise dual variable
imgq.Memset(0);
}
const double min_theta = 1E-0;
if(do_dtam && theta > min_theta)
{
for(int i=0; i<5; ++i ) {
// Auxillary exhaustive search
CostVolMinimumSquarePenaltySubpix(imga, vol[0], imgd, maxdisp, -1, lambda, (theta) );
// Dual Ascent
roo::WeightedHuberGradU_DualAscentP(imgq, imgd, imgw, sigma_q, huber_alpha);
// Primal Descent
roo::WeightedL2_u_minus_g_PrimalDescent(imgd, imgq, imga, imgw, sigma_d, 1.0f / (theta) );
theta= theta * (1-beta*n);
++n;
}
if( theta <= min_theta && save_depthmaps ) {
cv::Mat dmap = cv::Mat( lh, lw, CV_32FC1 );
// convert disparity to depth
roo::Disp2Depth(imgd, depthmap, Kl(0,0), baseline );
depthmap.MemcpyToHost( dmap.data );
// save depth image
char Index[10];
sprintf( Index, "%05d", frame );
std::string DepthPrefix = "SDepth-";
std::string DepthFile;
DepthFile = DepthPrefix + Index + ".pdm";
std::cout << "Depth File: " << DepthFile << std::endl;
std::ofstream pDFile( DepthFile.c_str(), std::ios::out | std::ios::binary );
pDFile << "P7" << std::endl;
pDFile << dmap.cols << " " << dmap.rows << std::endl;
unsigned int Size = dmap.elemSize1() * dmap.rows * dmap.cols;
pDFile << 4294967295 << std::endl;
pDFile.write( (const char*)dmap.data, Size );
pDFile.close();
// save grey image
std::string GreyPrefix = "Left-";
std::string GreyFile;
GreyFile = GreyPrefix + Index + ".pgm";
std::cout << "Grey File: " << GreyFile << std::endl;
cv::Mat gimg = cv::Mat( lh, lw, CV_8UC1 );
img_pyr[0][level].MemcpyToHost( gimg.data );
cv::imwrite( GreyFile, gimg );
// reset
step = true;
dtam_reset = true;
}
}
go |= pangolin::GuiVarHasChanged();
// if(go) {
// if(subpix) {
// CostVolMinimumSubpix(disp[0],vol[0], maxdisp,-1);
// if(leftrightcheck) CostVolMinimumSubpix(disp[1],vol[1], maxdisp,+1);
// }else{
// CostVolMinimum<float,float>(disp[0],vol[0], maxdisp);
// if(leftrightcheck) CostVolMinimum<float,float>(disp[1],vol[1], maxdisp);
// }
// }
if(go) {
for(int di=0; di<(leftrightcheck?2:1); ++di) {
for(int i=0; i < domedits; ++i ) {
if(domed9x9) MedianFilterRejectNegative9x9(disp[di],disp[di], medi);
if(domed7x7) MedianFilterRejectNegative7x7(disp[di],disp[di], medi);
if(domed5x5) MedianFilterRejectNegative5x5(disp[di],disp[di], medi);
}
}
if(leftrightcheck ) {
LeftRightCheck(disp[1], disp[0], +1, maxdispdiff);
LeftRightCheck(disp[0], disp[1], -1, maxdispdiff);
}
if(filtgradthresh > 0) {
FilterDispGrad(disp[0], disp[0], filtgradthresh);
}
}
// if(go)
{
// Generate point cloud from disparity image
DisparityImageToVbo(d3d, disp[0], baseline, Kl(0,0), Kl(1,1), Kl(0,2), Kl(1,2) );
// if(container[3].IsShown())
{
// Copy point cloud into VBO
{
pangolin::CudaScopedMappedPtr var(vbo);
roo::Image<float4> dVbo((float4*)*var,lw,lh);
dVbo.CopyFrom(d3d);
}
// Generate CBO
{
pangolin::CudaScopedMappedPtr var(cbo);
roo::Image<uchar4> dCbo((uchar4*)*var,lw,lh);
roo::ConvertImage<uchar4,unsigned char>(dCbo, img_pyr[0][level]);
}
}
// Update texture views
adisp.SetImageScale(1.0f/maxdisp);
// adleft.SetLevel(show_level);
// adright.SetLevel(show_level);
adVol.SetImage(vol[0].ImageXY(show_slice));
}
/////////////////////////////////////////////////////////////
// Draw
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glColor3f(1,1,1);
pangolin::FinishFrame();
}
}
ValueType
StringKernel<ValueType>::
operator()(const std::string& x, const std::string& y) const
{
const value_type& g=gap_;
value_type g2=g*g;
typedef boost::multi_array<value_type,2> dp_type;
#if 1
dp_type K0(boost::extents[x.size()+1][y.size()+1]);
dp_type G0(boost::extents[x.size()+1][y.size()+1]);
std::vector<value_type> K1(y.size()+1);
std::vector<value_type> G1(y.size()+1);
K0[0][0]=G0[0][0]=1.0;
for (uint i=1; i!=x.size()+1; ++i) {
K0[i][0]=1.0;
G0[i][0]=G0[i-1][0]*g;
}
for (uint j=1; j!=y.size()+1; ++j) {
K0[0][j]=1.0;
G0[0][j]=G0[0][j-1]*g;
}
for (uint i=1; i!=x.size()+1; ++i) {
K1[0]=G1[0]=0.0;
for (uint j=1; j!=y.size()+1; ++j) {
K1[j] = K1[j-1];
G1[j] = G1[j-1]*g;
if (x[i-1]==y[j-1]) {
K1[j] += G0[i-1][j-1]*g2;
G1[j] += G0[i-1][j-1]*g2;
}
K0[i][j] = K0[i-1][j] + K1[j];
G0[i][j] = G0[i-1][j]*g + G1[j];
}
}
return K0[x.size()][y.size()];
#else
dp_type K0(boost::extents[x.size()+1][y.size()+1]);
dp_type G0(boost::extents[x.size()+1][y.size()+1]);
dp_type K1(boost::extents[x.size()+1][y.size()+1]);
dp_type G1(boost::extents[x.size()+1][y.size()+1]);
K0[0][0]=G0[0][0]=1.0;
for (uint i=1; i!=x.size()+1; ++i) {
K0[i][0]=1.0;
G0[i][0]=G0[i-1][0]*g;
}
for (uint j=1; j!=y.size()+1; ++j) {
K0[0][j]=1.0;
G0[0][j]=G0[0][j-1]*g;
}
for (uint i=1; i!=x.size()+1; ++i) {
K1[i][0]=G1[i][0]=0.0;
for (uint j=1; j!=y.size()+1; ++j) {
K1[i][j] = K1[i][j-1];
G1[i][j] = G1[i][j-1]*g;
if (x[i-1]==y[j-1]) {
K1[i][j] += G0[i-1][j-1]*g2;
G1[i][j] += G0[i-1][j-1]*g2;
}
K0[i][j] = K0[i-1][j] + K1[i][j];
G0[i][j] = G0[i-1][j]*g + G1[i][j];
}
}
return K0[x.size()][y.size()];
#endif
}
double mmm1d_coulomb_pair_energy(Particle *p1, Particle *p2, double d[3], double r2, double r)
{
double chpref = p1->p.q*p2->p.q;
double rxy2, rxy2_d, z_d;
double E;
if (chpref == 0)
return 0;
rxy2 = d[0]*d[0] + d[1]*d[1];
rxy2_d = rxy2*uz2;
z_d = d[2]*uz;
if (rxy2 <= mmm1d_params.far_switch_radius_2) {
/* near range formula */
double r2n, rt, shift_z;
int n;
E = -2*C_GAMMA;
/* polygamma summation */
r2n = 1.0;
for (n = 0; n < n_modPsi; n++) {
double add = mod_psi_even(n, z_d)*r2n;
E -= add;
if (fabs(add) < mmm1d_params.maxPWerror)
break;
r2n *= rxy2_d;
}
E *= coulomb.prefactor*uz;
/* real space parts */
E += coulomb.prefactor/r;
shift_z = d[2] + box_l[2];
rt = sqrt(rxy2 + shift_z*shift_z);
E += coulomb.prefactor/rt;
shift_z = d[2] - box_l[2];
rt = sqrt(rxy2 + shift_z*shift_z);
E += coulomb.prefactor/rt;
}
else {
/* far range formula */
double rxy = sqrt(rxy2);
double rxy_d = rxy*uz;
int bp;
/* The first Bessel term will compensate a little bit the
log term, so add them close together */
E = -0.25*log(rxy2_d) + 0.5*(M_LN2 - C_GAMMA);
for (bp = 1; bp < mmm1d_params.bessel_cutoff; bp++) {
double fq = C_2PI*bp;
E += K0(fq*rxy_d)*cos(fq*z_d);
}
E *= 4*coulomb.prefactor*uz;
}
return chpref*E;
}
void add_mmm1d_coulomb_pair_force(double chpref, double d[3], double r2, double r, double force[3])
{
int dim;
double F[3];
double rxy2, rxy2_d, z_d;
double pref;
double Fx, Fy, Fz;
rxy2 = d[0]*d[0] + d[1]*d[1];
rxy2_d = rxy2*uz2;
z_d = d[2]*uz;
if (rxy2 <= mmm1d_params.far_switch_radius_2) {
/* near range formula */
double sr, sz, r2nm1, rt, rt2, shift_z;
int n;
/* polygamma summation */
sr = 0;
sz = mod_psi_odd(0, z_d);
r2nm1 = 1.0;
for (n = 1; n < n_modPsi; n++) {
double deriv = 2*n;
double mpe = mod_psi_even(n, z_d);
double mpo = mod_psi_odd(n, z_d);
double r2n = r2nm1*rxy2_d;
sz += r2n*mpo;
sr += deriv*r2nm1*mpe;
if (fabs(deriv*r2nm1*mpe) < mmm1d_params.maxPWerror)
break;
r2nm1 = r2n;
}
// fprintf(stderr, "max_n %d\n", n);
Fx = prefL3_i*sr*d[0];
Fy = prefL3_i*sr*d[1];
Fz = prefuz2*sz;
/* real space parts */
pref = coulomb.prefactor/(r2*r);
Fx += pref*d[0];
Fy += pref*d[1];
Fz += pref*d[2];
shift_z = d[2] + box_l[2];
rt2 = rxy2 + shift_z*shift_z;
rt = sqrt(rt2);
pref = coulomb.prefactor/(rt2*rt);
Fx += pref*d[0];
Fy += pref*d[1];
Fz += pref*shift_z;
shift_z = d[2] - box_l[2];
rt2 = rxy2 + shift_z*shift_z;
rt = sqrt(rt2);
pref = coulomb.prefactor/(rt2*rt);
Fx += pref*d[0];
Fy += pref*d[1];
Fz += pref*shift_z;
F[0] = Fx;
F[1] = Fy;
F[2] = Fz;
}
else {
/* far range formula */
double rxy = sqrt(rxy2);
double rxy_d = rxy*uz;
double sr = 0, sz = 0;
int bp;
for (bp = 1; bp < mmm1d_params.bessel_cutoff; bp++) {
double fq = C_2PI*bp, k0, k1;
#ifdef BESSEL_MACHINE_PREC
k0 = K0(fq*rxy_d);
k1 = K1(fq*rxy_d);
#else
LPK01(fq*rxy_d, &k0, &k1);
#endif
sr += bp*k1*cos(fq*z_d);
sz += bp*k0*sin(fq*z_d);
}
sr *= uz2*4*C_2PI;
sz *= uz2*4*C_2PI;
pref = coulomb.prefactor*(sr/rxy + 2*uz/rxy2);
F[0] = pref*d[0];
F[1] = pref*d[1];
F[2] = coulomb.prefactor*sz;
}
for (dim = 0; dim < 3; dim++)
force[dim] += chpref * F[dim];
}
int test()
{
std::vector<std::string> neutrals;
std::vector<std::string> ions;
neutrals.push_back("N2");
neutrals.push_back("CH4");
ions.push_back("N2+");
ions.push_back("e");
//photons
std::vector<Scalar> lambda,phy1AU;
read_hv_flux<Scalar>(lambda,phy1AU,"./input/hv_SSI.dat");
Scalar chi(100.);
Planet::Chapman<Scalar> chapman(chi);
Planet::PhotonFlux<Scalar,std::vector<Scalar>, std::vector<std::vector<Scalar> > > photon(chapman);
photon.set_photon_flux_top_atmosphere(lambda,phy1AU,Planet::Constants::Saturn::d_Sun<Scalar>());
//densities
std::vector<Scalar> molar_frac = {0.96,0.04,0.,0.};
Scalar dens_tot(1e12);
//diffusion
Planet::BinaryDiffusion<Scalar> N2N2( Antioch::Species::N2, Antioch::Species::N2 , 5.09e+16,0.81, Planet::DiffusionType::Massman);
Planet::BinaryDiffusion<Scalar> N2CH4( Antioch::Species::N2, Antioch::Species::CH4, 7.34e+16,0.75, Planet::DiffusionType::Massman);
Planet::BinaryDiffusion<Scalar> CH4CH4( Antioch::Species::CH4, Antioch::Species::CH4, 5.73e+16,0.50, Planet::DiffusionType::Massman);
//atmosphere
Planet::Atmosphere<Scalar, std::vector<Scalar>, std::vector<std::vector<Scalar> > > atm(neutrals,ions,photon);
atm.make_altitude_grid(600.,1400.,10.);
//temperature
std::vector<Scalar> T0,Tz;
read_temperature<Scalar>(T0,Tz,"input/temperature.dat");
atm.set_temperature(T0,Tz);
//cross-section
std::vector<Scalar> sigma;
read_crossSection<Scalar>(lambda,sigma,"./input/N2_hv_cross-sections.dat",3);
atm.add_photoabsorption("N2",lambda,sigma);
read_crossSection<Scalar>(lambda,sigma,"./input/CH4_hv_cross-sections.dat",9);
atm.add_photoabsorption("N2",lambda,sigma);
atm.add_photoabsorption("CH4",lambda,sigma);
//init compo
atm.init_composition(molar_frac,dens_tot);
//diffusion
atm.diffusion().set_binary_coefficient(0,0,N2N2);
atm.diffusion().set_binary_coefficient(0,1,N2CH4);
atm.diffusion().set_binary_coefficient(1,1,CH4CH4);
atm.make_diffusion();
//thermal coefficient
Scalar K0(4.3e6L);
atm.set_K0(K0);
atm.make_thermal_coefficient();
//solver
Planet::CrankNicholson<Scalar,std::vector<Scalar>, std::vector<std::vector<Scalar> > > solver;
int return_flag(0);
return return_flag;
}
