double Angle(Star * A, Star * B, Star *C) {
double angle = AlKashi(A->distance(B), C->distance(A), B->distance(C));
if (C->x()>A->x() && B->x()>A->x()) {
if (C->y()>yc(A, B, C)) angle*=-1;
}
if (C->x()<A->x() && B->x()<A->x()) {
if (C->y()<yc(A, B, C)) angle*=-1;
}
if (C->x()<A->x() && B->x()>A->x()) {
if (C->y()>yc(A, B, C)) angle=(2*M_PI-angle);
}
if (C->x()>A->x() && B->x()<A->x()) {
if (C->y()<yc(A, B, C)) angle=(2*M_PI-angle);
}
return angle;
}
ExecStatus
LqView<VX,VY,VZ,shr>::propagate(Space& home, const ModEventDelta&) {
count(home);
GECODE_ME_CHECK(z.gq(home,atleast()));
if (z.max() == atleast()) {
GECODE_ES_CHECK(post_false(home,x,y));
return home.ES_SUBSUMED(*this);
}
if (x.size() == 0)
return home.ES_SUBSUMED(*this);
if (z.assigned()) {
VY yc(y);
GECODE_REWRITE(*this,(LqInt<VX,VY>::post(home(*this),x,yc,z.val()+c)));
}
return shr ? ES_NOFIX : ES_FIX;
}
ExecStatus
EqView<VX,VY,VZ,shr,dom>::propagate(Space& home, const ModEventDelta&) {
count(home);
GECODE_ME_CHECK(z.gq(home,atleast()));
GECODE_ME_CHECK(z.lq(home,atmost()));
if (z.assigned()) {
if (z.val() == atleast()) {
GECODE_ES_CHECK(post_false(home,x,y));
return home.ES_SUBSUMED(*this);
}
if (z.val() == atmost()) {
GECODE_ES_CHECK(post_true(home,x,y));
return home.ES_SUBSUMED(*this);
}
if (!dom || (vtd(y) != VTD_VARVIEW)) {
VY yc(y);
GECODE_REWRITE(*this,(EqInt<VX,VY>
::post(home(*this),x,yc,z.val()+c)));
}
}
if (dom && (vtd(y) == VTD_VARVIEW) && (z.min() > 0)) {
/*
* Only if the propagator is at fixpoint here, continue
* when things are shared: the reason is that prune
* requires that the views in x overlap with y!
*/
if (shr && (VX::me(Propagator::modeventdelta()) != ME_INT_NONE))
return ES_NOFIX;
GECODE_ES_CHECK(prune(home,x,y));
return ES_NOFIX;
}
return shr ? ES_NOFIX : ES_FIX;
}
void BinaryStar::set_refine_flags() {
double refine_adjustment = 1;
ChildIndex c;
if (get_level() < 1) {
for (int i = 0; i < OCT_NCHILD; i++) {
set_refine_flag(i, true);
}
} else if (get_level() < get_max_level_allowed()) {
Real mass_min, dxmin, vmin, this_mass;
dxmin = dynamic_cast<BinaryStar*>(get_root())->get_dx() / Real(1 << OctNode::get_max_level_allowed());
vmin = dxmin * dxmin * dxmin;
mass_min = refine_floor * vmin * refine_adjustment;
for (int k = BW; k < GNX - BW; k++) {
c.set_z(2 * k / GNX);
for (int j = BW; j < GNX - BW; j++) {
c.set_y(2 * j / GNX);
for (int i = BW; i < GNX - BW; i++) {
c.set_x(2 * i / GNX);
#ifdef REFINE_ACC_MORE
if ((get_level() == get_max_level_allowed() - 1 && (*this)(i, j, k).frac(0) > refine_floor * refine_adjustment)
|| get_level() < get_max_level_allowed() - 1) {
#else
if (get_level() < get_max_level_allowed()) {
#endif
if (!get_refine_flag(c)) {
// set_refine_flag(c, true);
Real ra = (X(i, j, k) - bparam.x1).mag();
Real rd = (X(i, j, k) - bparam.x2).mag();
if ((*this)(i, j, k).rho() > refine_floor * refine_adjustment) {
set_refine_flag(c, true);
} else if (get_time() == 0.0 && (ra < get_dx() || rd < get_dx())) {
set_refine_flag(c, true);
} else/* if (get_time() != 0.0)*/{
this_mass = pow(get_dx(), 3) * (*this)(i, j, k).rho();
if (this_mass > mass_min) {
set_refine_flag(c, true);
}
}
}
}
}
}
}
}
}
/*
void BinaryStar::set_refine_flags() {
ChildIndex c;
if (get_level() < 1) {
for (int i = 0; i < OCT_NCHILD; i++) {
set_refine_flag(i, true);
}
} else if (get_level() < get_max_level_allowed()) {
Real mass_min, dxmin, vmin, this_mass;
dxmin = dynamic_cast<BinaryStar*>(get_root())->get_dx() / Real(1 << OctNode::get_max_level_allowed());
vmin = dxmin * dxmin * dxmin;
mass_min = refine_floor * vmin;
for (int k = BW; k < GNX - BW; k++) {
c.set_z(2 * k / GNX);
for (int j = BW; j < GNX - BW; j++) {
c.set_y(2 * j / GNX);
for (int i = BW; i < GNX - BW; i++) {
c.set_x(2 * i / GNX);
if ((get_level() == get_max_level_allowed() - 1 && (*this)(i, j, k).frac(0) > refine_floor) || get_level() < get_max_level_allowed() - 1) {
if (!get_refine_flag(c)) {
// set_refine_flag(c, true);
Real ra = (X(i, j, k) - a0).mag();
Real rd = (X(i, j, k) - d0).mag();
if ((*this)(i, j, k).rho() > refine_floor) {
set_refine_flag(c, true);
} else if (get_time() == 0.0 && (ra < get_dx() || rd < get_dx())) {
set_refine_flag(c, true);
} else{
this_mass = pow(get_dx(), 3) * (*this)(i, j, k).rho();
if (this_mass > mass_min) {
set_refine_flag(c, true);
}
}
}
}
}
}
}
}
}
*/
void BinaryStar::initialize() {
if (!bparam_init) {
bparam_init = true;
bparam.fill_factor = 0.97;
binary_parameters_compute(&bparam);
State::rho_floor = 1.0e-12 * bparam.rho1;
refine_floor = 1.0e-4 * bparam.rho1;
dynamic_cast<HydroGrid*>(get_root())->HydroGrid::mult_dx(bparam.a * 5.0);
#ifndef USE_FMM
dynamic_cast<MultiGrid*>(get_root())->MultiGrid::mult_dx(bparam.a * 5.0);
#endif
State::set_omega(bparam.omega);
}
for (int k = BW - 1; k < GNX - BW + 1; k++) {
for (int j = BW - 1; j < GNX - BW + 1; j++) {
for (int i = BW - 1; i < GNX - BW + 1; i++) {
int id;
State U = Vector<Real, STATE_NF>(0.0);
Real R2 = (xc(i) * xc(i) + yc(j) * yc(j));
Real rho = density_at(&bparam, xc(i), yc(j), zc(k), &id);
rho = max(rho, State::rho_floor);
Real tau = pow(State::ei_floor, 1.0 / State::gamma);
U.set_rho(rho);
U.set_et(U.ed());
U.set_tau(tau);
U.set_sx(0.0);
U.set_sy(bparam.omega * R2 * U.rho());
U.set_sz(0.0);
if (id == 1) {
U.set_frac(0, U.rho());
U.set_frac(1, 0.0);
} else if (id == -1) {
U.set_frac(1, U.rho());
U.set_frac(0, 0.0);
} else {
U.set_frac(1, 0.0);
U.set_frac(0, 0.0);
}
(*this)(i, j, k) = U;
}
}
} /*Real K, E1, E2, period, rho_c1, rho_c2;
a0 = d0 = 0.0;
if (scf_code) {
const Real a = 0.075;
const Real R2 = 0.0075;
const Real n = 1.5;
rho_c2 = q_ratio * M1 * (pow(3.65375 / R2, 3.0) / (2.71406 * 4.0 * M_PI));
rho_c1 = rho_c2 / pow(q_ratio, 2.0 * n / (3.0 - n));
a0[0] = a * q_ratio / (q_ratio + 1.0);
d0[0] = -a / (q_ratio + 1.0);
K = pow(R2 / 3.65375, 2) * 4.0 * M_PI / (2.5) * pow(rho_c2, 1.0 / 3.0);
Ka = Kd = (5.0 / 8.0) * K;
polyK = K;
E1 = 1.0 / (sqrt((4.0 * M_PI * pow(rho_c1, 1.0 - 1.0 / n)) / ((n + 1.0) * K)));
E2 = 1.0 / (sqrt((4.0 * M_PI * pow(rho_c2, 1.0 - 1.0 / n)) / ((n + 1.0) * K)));
// printf("%e %e %e %e\n", rho_c1, rho_c2, E1, E2);
period = sqrt(pow(a, 3) / (q_ratio + 1.0) / M1) * 2.0 * M_PI;
} else {
rho_c1 = rho_c2 = 1.0;
a0[0] = 0.025;
d0[0] = -0.025;
E1 = E2 = 0.0015;
K = pow(E1, 2) * 4.0 * M_PI / (2.5);
period = sqrt(pow((a0 - d0).mag(), 3) / 2.303394e-07) * 2.0 * M_PI;
}
State U;
Real d, e, gamma, tau;
gamma = 5.0 / 3.0;
Real ra, rd, r0, ek;
_3Vec v;
const Real Omega = 2.0 * M_PI / period;
State::set_omega(Omega);
Real f1, f2;
if (get_level() == 0) {
printf("Period = %e Omega = %e\n", period, Omega);
}
Real d_floor = 10.0 * State::rho_floor;
for (int k = BW - 1; k < GNX - BW + 1; k++) {
for (int j = BW - 1; j < GNX - BW + 1; j++) {
for (int i = BW - 1; i < GNX - BW + 1; i++) {
U = Vector<Real, STATE_NF>(0.0);
ra = (X(i, j, k) - a0).mag();
rd = (X(i, j, k) - d0).mag();
d = +rho_c1 * pow(lane_emden(ra / E1), 1.5);
d += rho_c2 * pow(lane_emden(rd / E2), 1.5);
d = max(d, d_floor);
if (ra < rd) {
f1 = d - d_floor / 2.0;
f2 = d_floor / 2.0;
} else {
f2 = d - d_floor / 2.0;
f1 = d_floor / 2.0;
}
if (State::cylindrical) {
Real R2 = (HydroGrid::xc(i) * HydroGrid::xc(i) + HydroGrid::yc(j) * HydroGrid::yc(j));
v[0] = 0.0;
v[1] = R2 * State::get_omega();
} else {
v[0] = -HydroGrid::yc(j) * State::get_omega();
v[1] = +HydroGrid::xc(i) * State::get_omega();
}
v[2] = 0.0;
e = K * pow(d, gamma) / (gamma - 1.0);
tau = pow(e, 1.0 / gamma);
U.set_rho(d);
U.set_frac(0, f1);
U.set_frac(1, f2);
U.set_et(e);
U.set_tau(tau);
U.set_sx(d * v[0]);
U.set_sy(d * v[1]);
U.set_sz(d * v[2]);
(*this)(i, j, k) = U;
}
}
}
*/
}
Real BinaryStar::radius(int i, int j, int k) {
return sqrt(xc(i) * xc(i) + yc(j) * yc(j) + zc(k) * zc(k));
}
void process_file(png_bytep * row_pointers, png_bytep *row_pointers2)
{
if (png_get_color_type(png_ptr,info_ptr) != PNG_COLOR_TYPE_RGB)
abort_("[process_file] color_type of input file must be PNG_COLOR_TYPE_RGBA (is %d)", png_get_color_type(png_ptr,info_ptr));
int bytes_per_component = 3;
YV12_BUFFER_CONFIG src;
src.y_height = height;
src.y_width = width;
src.y_stride = width;
src.uv_height = height;
src.uv_width = width;
src.uv_stride = width;
src.y_buffer = malloc(height*width);
src.y_buffer = malloc(height*width);
src.u_buffer = malloc(height*width);
src.v_buffer = malloc(height*width);
YV12_BUFFER_CONFIG dest;
dest.y_buffer = malloc(height*width);
dest.u_buffer = malloc(height*width);
dest.v_buffer = malloc(height*width);
dest.y_height = height;
dest.y_width = width;
dest.y_stride = width;
dest.uv_height = height;
dest.uv_width = width;
dest.uv_stride = width;
for (y=0; y<height; y++) {
png_byte* row1 = row_pointers[y];
png_byte* row2 = row_pointers2[y];
for (x=0; x<width; x++) {
int src_r = row1[x*3 + 0];
int src_g = row1[x*3 + 1];
int src_b = row1[x*3 + 2];
//int src_a = row1[x*3 + 3];
src.y_buffer[y*width + x] = yc(src_r, src_g, src_b);
src.u_buffer[y*width + x] = uc(src_r, src_g, src_b);
src.v_buffer[y*width + x] = vc(src_r, src_g, src_b);
int dst_r = row2[x*3 + 0];
int dst_g = row2[x*3 + 1];
int dst_b = row2[x*3 + 2];
//int dst_a = row2[x*4 + 3];
dest.y_buffer[y*width + x] = yc(dst_r, dst_g, dst_b);
dest.u_buffer[y*width + x] = uc(dst_r, dst_g, dst_b);
dest.v_buffer[y*width + x] = vc(dst_r, dst_g, dst_b);
}
}
double ssim_y;
double ssim_u;
double ssim_v;
double ssim = vp8_calc_ssimg
(
&src,
&dest,
&ssim_y,
&ssim_u,
&ssim_v
);
printf("ssimg: %f\n\ty: %f\n\tu: %f\n\tv: %f\n", ssim, 1/(1-ssim_y), 1/(1-ssim_u), 1/(1-ssim_v));
double weight;
double p = vp8_calc_ssim
(
&src, &dest,
1, &weight
);
printf("ssim: %f\n", p);
}
void historicalForwardRatesAnalysis(
SequenceStatistics& statistics,
std::vector<Date>& skippedDates,
std::vector<std::string>& skippedDatesErrorMessage,
std::vector<Date>& failedDates,
std::vector<std::string>& failedDatesErrorMessage,
std::vector<Period>& fixingPeriods,
const Date& startDate,
const Date& endDate,
const Period& step,
const boost::shared_ptr<InterestRateIndex>& fwdIndex,
const Period& initialGap,
const Period& horizon,
const std::vector<boost::shared_ptr<IborIndex> >& iborIndexes,
const std::vector<boost::shared_ptr<SwapIndex> >& swapIndexes,
const DayCounter& yieldCurveDayCounter,
Real yieldCurveAccuracy = 1.0e-12,
const Interpolator& i = Interpolator()) {
statistics.reset();
skippedDates.clear();
skippedDatesErrorMessage.clear();
failedDates.clear();
failedDatesErrorMessage.clear();
fixingPeriods.clear();
SavedSettings backup;
Settings::instance().enforcesTodaysHistoricFixings() = true;
std::vector<boost::shared_ptr<RateHelper> > rateHelpers;
// Create DepositRateHelper
std::vector<boost::shared_ptr<SimpleQuote> > iborQuotes;
std::vector<boost::shared_ptr<IborIndex> >::const_iterator ibor;
for (ibor=iborIndexes.begin(); ibor!=iborIndexes.end(); ++ibor) {
boost::shared_ptr<SimpleQuote> quote(new SimpleQuote);
iborQuotes.push_back(quote);
Handle<Quote> quoteHandle(quote);
rateHelpers.push_back(boost::shared_ptr<RateHelper> (new
DepositRateHelper(quoteHandle,
(*ibor)->tenor(),
(*ibor)->fixingDays(),
(*ibor)->fixingCalendar(),
(*ibor)->businessDayConvention(),
(*ibor)->endOfMonth(),
(*ibor)->dayCounter())));
}
// Create SwapRateHelper
std::vector<boost::shared_ptr<SimpleQuote> > swapQuotes;
std::vector<boost::shared_ptr<SwapIndex> >::const_iterator swap;
for (swap=swapIndexes.begin(); swap!=swapIndexes.end(); ++swap) {
boost::shared_ptr<SimpleQuote> quote(new SimpleQuote);
swapQuotes.push_back(quote);
Handle<Quote> quoteHandle(quote);
rateHelpers.push_back(boost::shared_ptr<RateHelper> (new
SwapRateHelper(quoteHandle,
(*swap)->tenor(),
(*swap)->fixingCalendar(),
(*swap)->fixedLegTenor().frequency(),
(*swap)->fixedLegConvention(),
(*swap)->dayCounter(),
(*swap)->iborIndex())));
}
// Set up the forward rates time grid
Period indexTenor = fwdIndex->tenor();
Period fixingPeriod = initialGap;
while (fixingPeriod<=horizon) {
fixingPeriods.push_back(fixingPeriod);
fixingPeriod += indexTenor;
}
Size nRates = fixingPeriods.size();
statistics.reset(nRates);
std::vector<Rate> fwdRates(nRates);
std::vector<Rate> prevFwdRates(nRates);
std::vector<Rate> fwdRatesDiff(nRates);
DayCounter indexDayCounter = fwdIndex->dayCounter();
Calendar cal = fwdIndex->fixingCalendar();
// Bootstrap the yield curve at the currentDate
Natural settlementDays = 0;
PiecewiseYieldCurve<Traits, Interpolator> yc(settlementDays,
cal,
rateHelpers,
yieldCurveDayCounter,
std::vector<Handle<Quote> >(),
std::vector<Date>(),
yieldCurveAccuracy,
i);
// start with a valid business date
Date currentDate = cal.advance(startDate, 1*Days, Following);
bool isFirst = true;
// Loop over the historical dataset
for (; currentDate<=endDate;
currentDate = cal.advance(currentDate, step, Following)) {
// move the evaluationDate to currentDate
// and update ratehelpers dates...
Settings::instance().evaluationDate() = currentDate;
try {
// update the quotes...
for (Size i=0; i<iborIndexes.size(); ++i) {
Rate fixing = iborIndexes[i]->fixing(currentDate, false);
iborQuotes[i]->setValue(fixing);
}
for (Size i=0; i<swapIndexes.size(); ++i) {
Rate fixing = swapIndexes[i]->fixing(currentDate, false);
swapQuotes[i]->setValue(fixing);
}
} catch (std::exception& e) {
skippedDates.push_back(currentDate);
skippedDatesErrorMessage.push_back(e.what());
continue;
}
try {
for (Size i=0; i<nRates; ++i) {
// Time-to-go forwards
Date d = currentDate + fixingPeriods[i];
fwdRates[i] = yc.forwardRate(d,
indexTenor,
indexDayCounter,
Simple);
}
} catch (std::exception& e) {
failedDates.push_back(currentDate);
failedDatesErrorMessage.push_back(e.what());
continue;
}
// From 2nd step onwards, calculate forward rate
// relative differences
if (!isFirst){
for (Size i=0; i<nRates; ++i)
fwdRatesDiff[i] = fwdRates[i]/prevFwdRates[i] -1.0;
// add observation
statistics.add(fwdRatesDiff.begin(), fwdRatesDiff.end());
}
else
isFirst = false;
// Store last calculated forward rates
std::swap(prevFwdRates, fwdRates);
}
}
int main(int argc, char **argv)
{
ap::real_1d_array x;
ap::real_1d_array y;
ap::real_1d_array w;
ap::real_1d_array xc;
ap::real_1d_array yc;
ap::integer_1d_array dc;
int n;
int i;
int info;
spline1dinterpolant s;
double t;
spline1dfitreport rep;
//
// Fitting by constrained Hermite spline
//
printf("FITTING BY CONSTRAINED HERMITE SPLINE\n\n");
printf("F(x)=sin(x) function being fitted\n");
printf("[0, pi] interval\n");
printf("M=6 number of basis functions to use\n");
printf("S(0)=0 first constraint\n");
printf("S(pi)=0 second constraint\n");
printf("N=100 number of points to fit\n");
//
// Create and fit:
// * X contains points
// * Y contains values
// * W contains weights
// * XC contains constraints locations
// * YC contains constraints values
// * DC contains derivative indexes (0 = constrained function value)
//
n = 100;
x.setlength(n);
y.setlength(n);
w.setlength(n);
for(i = 0; i <= n-1; i++)
{
x(i) = ap::pi()*i/(n-1);
y(i) = sin(x(i));
w(i) = 1;
}
xc.setlength(2);
yc.setlength(2);
dc.setlength(2);
xc(0) = 0;
yc(0) = 0;
dc(0) = 0;
xc(0) = ap::pi();
yc(0) = 0;
dc(0) = 0;
spline1dfithermitewc(x, y, w, n, xc, yc, dc, 2, 6, info, s, rep);
//
// Output results
//
if( info>0 )
{
printf("\nOK, we have finished\n\n");
printf(" x F(x) S(x) Error\n");
t = 0;
while(ap::fp_less(t,0.999999*ap::pi()))
{
printf("%6.3lf %6.3lf %6.3lf %6.3lf\n",
double(t),
double(sin(t)),
double(spline1dcalc(s, t)),
double(fabs(spline1dcalc(s, t)-sin(t))));
t = ap::minreal(ap::pi(), t+0.25);
}
printf("%6.3lf %6.3lf %6.3lf %6.3lf\n\n",
double(t),
double(sin(t)),
double(spline1dcalc(s, t)),
double(fabs(spline1dcalc(s, t)-sin(t))));
printf("rms error is %6.3lf\n",
double(rep.rmserror));
printf("max error is %6.3lf\n",
double(rep.maxerror));
printf("S(0) = S(pi) = 0 (exactly)\n\n");
}
else
{
printf("\nSomething wrong, Info=%0ld",
long(info));
}
return 0;
}
int main(int argc, char **argv)
{
int m;
int n;
int d;
ap::real_1d_array x;
ap::real_1d_array y;
ap::real_1d_array w;
ap::real_1d_array xc;
ap::real_1d_array yc;
ap::integer_1d_array dc;
barycentricfitreport rep;
int info;
barycentricinterpolant r;
int i;
int j;
double a;
double b;
double v;
double dv;
printf("\n\nFitting exp(2*x) at [-1,+1] by:\n1. constrained/unconstrained Floater-Hormann functions\n");
printf("\n");
printf("Fit type rms.err max.err p(0) dp(0) DBest\n");
//
// Prepare points
//
m = 5;
a = -1;
b = +1;
n = 10000;
x.setlength(n);
y.setlength(n);
w.setlength(n);
for(i = 0; i <= n-1; i++)
{
x(i) = a+(b-a)*i/(n-1);
y(i) = exp(2*x(i));
w(i) = 1.0;
}
//
// Fitting:
// a) f(x)=exp(2*x) at [-1,+1]
// b) by 5 Floater-Hormann functions
// c) without constraints
//
barycentricfitfloaterhormann(x, y, n, m, info, r, rep);
barycentricdiff1(r, 0.0, v, dv);
printf("Unconstrained FH %7.4lf %7.4lf %7.4lf %7.4lf %0ld\n",
double(rep.rmserror),
double(rep.maxerror),
double(v),
double(dv),
long(rep.dbest));
//
// Fitting:
// a) f(x)=exp(2*x) at [-1,+1]
// b) by 5 Floater-Hormann functions
// c) constrained: p(0)=1
//
xc.setlength(1);
yc.setlength(1);
dc.setlength(1);
xc(0) = 0;
yc(0) = 1;
dc(0) = 0;
barycentricfitfloaterhormannwc(x, y, w, n, xc, yc, dc, 1, m, info, r, rep);
barycentricdiff1(r, 0.0, v, dv);
printf("Constrained FH, p(0)=1 %7.4lf %7.4lf %7.4lf %7.4lf %0ld\n",
double(rep.rmserror),
double(rep.maxerror),
double(v),
double(dv),
long(rep.dbest));
//
// Fitting:
// a) f(x)=exp(2*x) at [-1,+1]
// b) by 5 Floater-Hormann functions
// c) constrained: dp(0)=2
//
xc.setlength(1);
yc.setlength(1);
dc.setlength(1);
xc(0) = 0;
yc(0) = 2;
dc(0) = 1;
barycentricfitfloaterhormannwc(x, y, w, n, xc, yc, dc, 1, m, info, r, rep);
barycentricdiff1(r, 0.0, v, dv);
printf("Constrained FH, dp(0)=2 %7.4lf %7.4lf %7.4lf %7.4lf %0ld\n",
double(rep.rmserror),
double(rep.maxerror),
double(v),
double(dv),
long(rep.dbest));
//
// Fitting:
// a) f(x)=exp(2*x) at [-1,+1]
// b) by 5 Floater-Hormann functions
// c) constrained: p(0)=1, dp(0)=2
//
xc.setlength(2);
yc.setlength(2);
dc.setlength(2);
xc(0) = 0;
yc(0) = 1;
dc(0) = 0;
xc(1) = 0;
yc(1) = 2;
dc(1) = 1;
barycentricfitfloaterhormannwc(x, y, w, n, xc, yc, dc, 2, m, info, r, rep);
barycentricdiff1(r, 0.0, v, dv);
printf("Constrained FH, both %7.4lf %7.4lf %7.4lf %7.4lf %0ld\n",
double(rep.rmserror),
double(rep.maxerror),
double(v),
double(dv),
long(rep.dbest));
printf("\n\n");
return 0;
}
int
main (int argc, char** argv)
{
// ros::init(argc, argv, "extract_sec");
// ros::NodeHandle node_handle;
// pcl::visualization::PCLVisualizer result_viewer("planar_segmentation");
boost::shared_ptr<pcl::visualization::PCLVisualizer> result_viewer (new pcl::visualization::PCLVisualizer ("planar_segmentation"));
result_viewer->addCoordinateSystem(0.3, "reference", 0);
result_viewer->setCameraPosition(-0.499437, 0.111597, -0.758007, -0.443141, 0.0788583, -0.502855, -0.034703, -0.992209, -0.119654);
result_viewer->setCameraClipDistances(0.739005, 2.81526);
// result_viewer->setCameraPosition(Position, Focal point, View up);
// result_viewer->setCameraClipDistances(Clipping plane);
/***************************************
* parse arguments
***************************************/
if(argc<5)
{
pcl::console::print_info("Usage: extract_sec DATA_PATH/PCD_FILE_FORMAT START_INDEX END_INDEX DEMO_NAME (opt)STEP_SIZE(1)");
exit(1);
}
int view_id=0;
int step=1;
std::string basename_cloud=argv[1];
unsigned int index_start = std::atoi(argv[2]);
unsigned int index_end = std::atoi(argv[3]);
std::string demo_name=argv[4];
if(argc>5) step=std::atoi(argv[5]);
/***************************************
* set up result directory
***************************************/
mkdir("/home/zhen/Documents/Dataset/human_result", S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
char result_folder[50];
std::snprintf(result_folder, sizeof(result_folder), "/home/zhen/Documents/Dataset/human_result/%s", demo_name.c_str());
mkdir(result_folder, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
std::string basename_pcd = (basename_cloud.find(".pcd") == std::string::npos) ? (basename_cloud + ".pcd") : basename_cloud;
std::string filename_pcd;
std::string mainGraph_file;
mainGraph_file = std::string(result_folder) + "/mainGraph.txt";
// write video config
char video_file[100];
std::snprintf(video_file, sizeof(video_file), "%s/video.txt", result_folder);
std::ofstream video_config(video_file);
if (video_config.is_open())
{
video_config << index_start << " " << index_end << " " << demo_name << " " << step;
video_config.close();
}
/***************************************
* set up cloud, segmentation, tracker, detectors, graph, features
***************************************/
TableObject::Segmentation tableObjSeg;
TableObject::Segmentation initialSeg;
TableObject::track3D tracker(false);
TableObject::colorDetector finger1Detector(0,100,0,100,100,200); //right
TableObject::colorDetector finger2Detector(150,250,0,100,0,100); //left
TableObject::touchDetector touchDetector(0.01);
TableObject::bottleDetector bottleDetector;
TableObject::mainGraph mainGraph((int)index_start);
std::vector<manipulation_features> record_features;
manipulation_features cur_features;
TableObject::pcdCloud pcdSceneCloud;
CloudPtr sceneCloud;
CloudPtr planeCloud(new Cloud);
CloudPtr cloud_objects(new Cloud);
CloudPtr cloud_finger1(new Cloud);
CloudPtr cloud_finger2(new Cloud);
CloudPtr cloud_hull(new Cloud);
CloudPtr track_target(new Cloud);
CloudPtr tracked_cloud(new Cloud);
std::vector<pcl::PointIndices> clusters;
pcl::ModelCoefficients coefficients;
pcl::PointIndices f1_indices;
pcl::PointIndices f2_indices;
Eigen::Affine3f toBottleCoordinate;
Eigen::Affine3f transformation;
Eigen::Vector3f bottle_init_ori;
// set threshold of size of clustered cloud
tableObjSeg.setThreshold(30);
initialSeg.setThreshold(500);
// downsampler
pcl::ApproximateVoxelGrid<pcl::PointXYZRGBA> grid;
float leaf_size=0.005;
grid.setLeafSize (leaf_size, leaf_size, leaf_size);
/***************************************
* start processing
***************************************/
unsigned int idx = index_start;
int video_id=0;
bool change = false;
while( idx <= index_end && !result_viewer->wasStopped())
{
std::cout << std::endl;
std::cout << "frame id=" << idx << std::endl;
filename_pcd = cv::format(basename_cloud.c_str(), idx);
if(idx==index_start) {
/***************************************
* Intialization:
* -plane localization
* -object cluster extraction
* -bottle cluster localization
***************************************/
initialSeg.resetCloud(filename_pcd, false);
initialSeg.seg(false);
initialSeg.getObjects(cloud_objects, clusters);
initialSeg.getCloudHull(cloud_hull);
initialSeg.getPlaneCoefficients(coefficients);
initialSeg.getsceneCloud(pcdSceneCloud);
initialSeg.getTableTopCloud(planeCloud);
sceneCloud=pcdSceneCloud.getCloud();
/***************************************
* fingertip, hand_arm removal
***************************************/
//opencv color filtering for fingertip_1
{
pcl::ScopeTime t_finger1("Finger 1(blue) detection");
finger1Detector.setInputCloud(cloud_objects, clusters);
finger1Detector.filter(f1_indices,cloud_finger1);
}
finger1Detector.showDetectedCloud(result_viewer, "finger1");
//opencv color filtering for fingertip_2
{
pcl::ScopeTime t_finger2("Finger 2(orange) detection");
finger2Detector.setInputCloud(cloud_objects, clusters);
finger2Detector.filter(f2_indices,cloud_finger2);
}
finger2Detector.showDetectedCloud(result_viewer, "finger2");
// remove hand (include cluster that contains the detected fingertips and also the other clusters that are touching the cluster)
std::vector<int> hand_arm1=TableObject::findHand(cloud_objects, clusters, f1_indices);
for(int i=hand_arm1.size()-1; i>=0; i--)
{
clusters.erase(clusters.begin()+hand_arm1[i]);
std::cout << "removing hand_arm : cluster index = " << hand_arm1[i] << std::endl;
}
std::vector<int> hand_arm2=TableObject::findHand(cloud_objects, clusters, f2_indices);
for(int i=hand_arm2.size()-1; i>=0; i--)
{
clusters.erase(clusters.begin()+hand_arm2[i]);
std::cout << "removing hand_arm : cluster index = " << hand_arm2[i] << std::endl;
}
// DEBUG
// pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> plane(planeCloud);
// result_viewer->addPointCloud<RefPointType>(planeCloud, plane, "tabletop");
// CloudPtr debug(new Cloud);
// initialSeg.getOutPlaneCloud(debug);
// pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> out_plane(debug);
// result_viewer->addPointCloud<RefPointType>(debug, out_plane, "out_plane");
// choose bottle_id at 1st frame & confirm fitted model is correct
TableObject::view3D::drawClusters(result_viewer, cloud_objects, clusters, true);
while (!result_viewer->wasStopped ())
{
result_viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
std::cout << "cluster size = " << clusters.size() << std::endl;
/***************************************
* Localizing cylinder
***************************************/
CloudPtr cluster_bottle (new Cloud);
int bottle_id = 0;
pcl::copyPointCloud (*cloud_objects, clusters[bottle_id], *cluster_bottle);
bottleDetector.setInputCloud(cluster_bottle);
bottleDetector.fit();
bottleDetector.getTransformation(toBottleCoordinate);
bottle_init_ori= bottleDetector.getOrientation();
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(toBottleCoordinate.inverse(), x, y, z, roll, pitch, yaw);
result_viewer->removeCoordinateSystem("reference", 0);
result_viewer->addCoordinateSystem(0.3, toBottleCoordinate.inverse(), "reference", 0);
bottleDetector.drawOrientation(result_viewer);
/***************************************
* Record features
***************************************/
bottle_features cur_bottle_features;
cur_bottle_features.loc[0] = x;
cur_bottle_features.loc[1] = y;
cur_bottle_features.loc[2] = z;
cur_bottle_features.ori[0] = roll;
cur_bottle_features.ori[1] = pitch;
cur_bottle_features.ori[2] = yaw;
cur_bottle_features.color[0] = bottleDetector.getCenter().r;
cur_bottle_features.color[1] = bottleDetector.getCenter().g;
cur_bottle_features.color[2] = bottleDetector.getCenter().b;
cur_bottle_features.size[0] = bottleDetector.getHeight();
cur_bottle_features.size[1] = bottleDetector.getRadius();
cur_features.bottle = cur_bottle_features;
pcl::PointXYZ center_finger1 = TableObject::computeObjCentroid(cloud_finger1);
pcl::PointXYZ center_finger2 = TableObject::computeObjCentroid(cloud_finger2);
center_finger1 = pcl::transformPoint<pcl::PointXYZ>(center_finger1, toBottleCoordinate);
center_finger2 = pcl::transformPoint<pcl::PointXYZ>(center_finger2, toBottleCoordinate);
cur_features.gripper_1.loc[0] = center_finger1.x;
cur_features.gripper_1.loc[1] = center_finger1.y;
cur_features.gripper_1.loc[2] = center_finger1.z;
cur_features.gripper_2.loc[0] = center_finger2.x;
cur_features.gripper_2.loc[1] = center_finger2.y;
cur_features.gripper_2.loc[2] = center_finger2.z;
record_features.push_back(cur_features);
/***************************************
* Tracking initialization
***************************************/
{
pcl::ScopeTime t_track("Tracker initialization");
tracker.setTarget(cluster_bottle, bottleDetector.getCenter());
tracker.initialize();
}
/***************************************
* Touch detection
***************************************/
std::vector<CloudPtr> touch_clouds;
touch_clouds.push_back(cluster_bottle);
touch_clouds.push_back(cloud_finger1);
touch_clouds.push_back(cloud_finger2);
// touch detection between each pair of objects (including fingertips, tabletop objects and tabletop)
for(int i=0; i<touch_clouds.size(); i++)
{
int j;
bool touch;
for(j=i+1; j<touch_clouds.size(); j++)
{
// touch detection between object_i and object_j
char relation [50];
std::sprintf(relation, "object%d_object%d", i, j);
std::cout << relation << std::endl;
{
pcl::ScopeTime t("Touch detection");
touch=touchDetector.detect(touch_clouds[i], touch_clouds[j]);
}
// touchDetector.showTouch(result_viewer, relation, 100+250*(j-i-1), 40+20*i);
// relational scene graph -> main graph
if(touch) {
mainGraph.addInitialRelationalGraph(2);
} else {
mainGraph.addInitialRelationalGraph(0);
}
}
// touch detection between each objects and tabletop
char relation [50];
std::sprintf (relation, "object%d_object%d", i, (int)touch_clouds.size());
std::cout << relation << std::endl;
{
pcl::ScopeTime t("Touch detection");
touch=touchDetector.detectTableTouch(touch_clouds[i], coefficients);
}
// touchDetector.showTouch(result_viewer, relation, 100+250*(j-i-1), 40+20*i);
// relational scene graph -> main graph
if(touch) {
mainGraph.addInitialRelationalGraph(2);
} else {
mainGraph.addInitialRelationalGraph(0);
}
}
/***************************************
* Visualization
***************************************/
// draw extracted object clusters
// TableObject::view3D::drawClusters(result_viewer, cloud_objects, touch_clusters);
// draw extracted plane points
// pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> plane(planeCloud);
// result_viewer->addPointCloud<RefPointType>(planeCloud, plane, "tabletop");
// std::stringstream ss;
// ss << (int)touch_clusters.size();
// result_viewer->addText3D(ss.str(), planeCloud->points.at(334*640+78),0.1);
// draw extracted plane contour polygon
result_viewer->addPolygon<RefPointType>(cloud_hull, 0, 255, 0, "polygon");
change = true;
} else
{
/***************************************
* object cloud extraction
***************************************/
tableObjSeg.resetCloud(filename_pcd, false);
tableObjSeg.seg(cloud_hull,false);
tableObjSeg.getObjects(cloud_objects, clusters);
tableObjSeg.getsceneCloud(pcdSceneCloud);
sceneCloud=pcdSceneCloud.getCloud();
/***************************************
* fingertip extraction
***************************************/
//opencv color filtering for fingertip_1
{
pcl::ScopeTime t_finger1("Finger 1(blue) detection");
finger1Detector.setInputCloud(cloud_objects, clusters);
finger1Detector.filter(f1_indices,cloud_finger1);
}
finger1Detector.showDetectedCloud(result_viewer, "finger1");
//opencv color filtering for fingertip_2
{
pcl::ScopeTime t_finger1("Finger 2(orange) detection");
finger2Detector.setInputCloud(cloud_objects, clusters);
finger2Detector.filter(f2_indices,cloud_finger2);
}
finger2Detector.showDetectedCloud(result_viewer, "finger2");
/***************************************
* filter out black glove cluster & gray sleeve, also update cloud_objects with removed cluster indices
***************************************/
for(int i=0; i<clusters.size(); i++)
{
pcl::CentroidPoint<RefPointType> color_points;
for(int j=0; j<clusters[i].indices.size(); j++)
{
color_points.add(cloud_objects->at(clusters[i].indices[j]));
}
RefPointType mean_color;
color_points.get(mean_color);
if(mean_color.r>30 & mean_color.r<70 & mean_color.g>30 & mean_color.g<70 & mean_color.b>30 & mean_color.b<70)
{
clusters.erase(clusters.begin()+ i);
i=i-1;
}
}
/***************************************
* Tracking objects
***************************************/
{
pcl::ScopeTime t_track("Tracking");
grid.setInputCloud (sceneCloud);
grid.filter (*track_target);
tracker.track(track_target, transformation);
tracker.getTrackedCloud(tracked_cloud);
}
tracker.viewTrackedCloud(result_viewer);
// tracker.drawParticles(result_viewer);
/***************************************
* compute tracked <center, orientation>
***************************************/
pcl::PointXYZ bottle_loc_point(0,0,0);
bottle_loc_point = pcl::transformPoint<pcl::PointXYZ>(bottle_loc_point, transformation);
result_viewer->removeShape("bottle_center");
// result_viewer->addSphere<pcl::PointXYZ>(bottle_loc_point, 0.05, "bottle_center");
Eigen::Vector3f bottle_ori;
pcl::transformVector(bottle_init_ori,bottle_ori,transformation);
TableObject::view3D::drawArrow(result_viewer, bottle_loc_point, bottle_ori, "bottle_arrow");
/***************************************
* calculate toTrackedBottleCoordinate
***************************************/
Eigen::Affine3f toTrackedBottleCoordinate;
Eigen::Vector3f p( bottle_loc_point.x, bottle_loc_point.y, bottle_loc_point.z ); // position
// get a vector that is orthogonal to _orientation ( yc = _orientation x [1,0,0]' )
Eigen::Vector3f yc( 0, bottle_ori[2], -bottle_ori[1] );
yc.normalize();
// get a transform that rotates _orientation into z and moves cloud into origin.
pcl::getTransformationFromTwoUnitVectorsAndOrigin(yc, bottle_ori, p, toTrackedBottleCoordinate);
result_viewer->removeCoordinateSystem("reference");
result_viewer->addCoordinateSystem(0.3, toTrackedBottleCoordinate.inverse(), "reference", 0);
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(toTrackedBottleCoordinate.inverse(), x, y, z, roll, pitch, yaw);
/***************************************
* setup bottle feature
***************************************/
cur_features = record_features[video_id-1];
cur_features.bottle.loc[0] = x;
cur_features.bottle.loc[1] = y;
cur_features.bottle.loc[2] = z;
cur_features.bottle.ori[0] = roll;
cur_features.bottle.ori[1] = pitch;
cur_features.bottle.ori[2] = yaw;
pcl::PointXYZ center_finger1 = TableObject::computeObjCentroid(cloud_finger1);
pcl::PointXYZ center_finger2 = TableObject::computeObjCentroid(cloud_finger2);
center_finger1 = pcl::transformPoint<pcl::PointXYZ>(center_finger1, toTrackedBottleCoordinate);
center_finger2 = pcl::transformPoint<pcl::PointXYZ>(center_finger2, toTrackedBottleCoordinate);
cur_features.gripper_1.loc[0] = center_finger1.x;
cur_features.gripper_1.loc[1] = center_finger1.y;
cur_features.gripper_1.loc[2] = center_finger1.z;
cur_features.gripper_2.loc[0] = center_finger2.x;
cur_features.gripper_2.loc[1] = center_finger2.y;
cur_features.gripper_2.loc[2] = center_finger2.z;
record_features.push_back(cur_features);
/***************************************
* Touch detection
***************************************/
std::vector<CloudPtr> touch_clouds;
touch_clouds.push_back(tracked_cloud);
touch_clouds.push_back(cloud_finger1);
touch_clouds.push_back(cloud_finger2);
// touch detection between each pair of objects (including fingertips, tabletop objects and tabletop)
for(int i=0; i<touch_clouds.size(); i++)
{
int j;
bool touch;
for(j=i+1; j<touch_clouds.size(); j++)
{
// touch detection between object_i and object_j
char relation [50];
std::sprintf(relation, "object%d_object%d", i, j);
std::cout << relation << std::endl;
{
pcl::ScopeTime t("Touch detection");
touch=touchDetector.detect(touch_clouds[i], touch_clouds[j]);
}
// touchDetector.showTouch(result_viewer, relation, 100+250*(j-i-1), 40+20*i);
// relational scene graph -> main graph
if(touch) {
mainGraph.addRelationalGraph(2);
} else {
mainGraph.addRelationalGraph(0);
}
}
// touch detection between each objects and tabletop
char relation [50];
std::sprintf (relation, "object%d_object%d", i, (int)touch_clouds.size());
std::cout << relation << std::endl;
{
pcl::ScopeTime t("Touch detection");
touch=touchDetector.detectTableTouch(touch_clouds[i], coefficients);
}
// touchDetector.showTouch(result_viewer, relation, 100+250*(j-i-1), 40+20*i);
// relational scene graph -> main graph
if(touch) {
mainGraph.addRelationalGraph(2);
} else {
mainGraph.addRelationalGraph(0);
}
}
/***************************************
* Visualization
***************************************/
// draw extracted point clusters
// TableObject::view3D::drawText(result_viewer, cloud_objects, touch_clusters);
/***************************************
* Main Graph
***************************************/
change = mainGraph.compareRelationGraph((int)idx);
}
// darw original cloud
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> rgb(sceneCloud);
if(!result_viewer->updatePointCloud<RefPointType>(sceneCloud, rgb, "new frame"))
result_viewer->addPointCloud<RefPointType>(sceneCloud, rgb, "new frame");
result_viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
//debug
std::cout << cur_features.bottle.loc[0] << " " << cur_features.bottle.loc[1] << " " << cur_features.bottle.loc[2]
<< " " << cur_features.bottle.ori[0] << " " << cur_features.bottle.ori[1] << " " << cur_features.bottle.ori[2]
<< " " << cur_features.bottle.color[0] << " " << cur_features.bottle.color[1] << " " << cur_features.bottle.color[2]
<< " " << cur_features.bottle.size[0] << " " << cur_features.bottle.size[1]
<< " " << cur_features.gripper_1.loc[0] << " " << cur_features.gripper_1.loc[1] << " " << cur_features.gripper_1.loc[2]
<< " " << cur_features.gripper_2.loc[0] << " " << cur_features.gripper_2.loc[1] << " " << cur_features.gripper_2.loc[2]
<< std::endl;
if(change)
{
char screenshot[100]; // make sure it's big enough
std::snprintf(screenshot, sizeof(screenshot), "%s/sec_%d.png", result_folder, (int)idx);
std::cout << screenshot << std::endl;
result_viewer->saveScreenshot(screenshot);
//record features
char feature_file[100]; // make sure it's big enough
std::snprintf(feature_file, sizeof(feature_file), "%s/features_original.txt", result_folder);
std::ofstream feature_writer(feature_file, std::ofstream::out | std::ofstream::app);
feature_writer << cur_features.bottle.loc[0] << " " << cur_features.bottle.loc[1] << " " << cur_features.bottle.loc[2]
<< " " << cur_features.bottle.ori[0] << " " << cur_features.bottle.ori[1] << " " << cur_features.bottle.ori[2]
<< " " << cur_features.bottle.color[0] << " " << cur_features.bottle.color[1] << " " << cur_features.bottle.color[2]
<< " " << cur_features.bottle.size[0] << " " << cur_features.bottle.size[1]
<< " " << cur_features.gripper_1.loc[0] << " " << cur_features.gripper_1.loc[1] << " " << cur_features.gripper_1.loc[2]
<< " " << cur_features.gripper_2.loc[0] << " " << cur_features.gripper_2.loc[1] << " " << cur_features.gripper_2.loc[2]
<< std::endl;
feature_writer.close();
std::cout << "features saved at " << feature_file << std::endl;
}
char screenshot[200]; // make sure it's big enough
std::snprintf(screenshot, sizeof(screenshot), "%s/video/sec_%d.png", result_folder, (int)video_id);
std::cout << screenshot << std::endl;
result_viewer->saveScreenshot(screenshot);
idx=idx+step;
video_id=video_id+1;
}
mainGraph.displayMainGraph();
mainGraph.recordMainGraph(mainGraph_file);
while (!result_viewer->wasStopped ())
{
result_viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
}