DetectedLineResult LineDetector::DetectLine(vector<Vec4i>* lines)
{
DetectedLineResult result = DetectedLineResult();
result.detectedLine = new DetectedLine(lines->at(0));
if (lines->size() == 1) {
lines->clear();
result.remainingLines = lines;
} else {
Vec4i line1 = lines->at(0);
float slope1 = Slope(line1);
result.remainingLines = new vector<Vec4i>();
for (int i = 1; i < lines->size(); i++) {
Vec4i line2 = lines->at(i);
float slope2 = Slope(line2);
if (std::abs(1 - slope2 / slope1) < SlopeThreshold) {
float b1 = IntersectWithYAxis(line1, slope1);
float b2 = IntersectWithYAxis(line2, slope2);
//cout << "dist " << b1 << ", " << b2 << "\n";
if (std::abs(b2-b1)/std::sqrt(slope1*slope2 + 1) < DistanceThreshold) {
result.detectedLine->AddLine(line2);
} else {
result.remainingLines->push_back(line2);
}
} else {
result.remainingLines->push_back(line2);
}
}
//delete lines;
}
return result;
}
void RunningRegression::Print(FILE * pFile, const char * header) const
{
fprintf (pFile, "\n%s\n", header);
fprintf (pFile, "NumDataValues: %ld\n", NumDataValues());
fprintf (pFile, "y = A + Bx ==> y = %g %+gx\n", Intercept(), Slope());
fprintf (pFile, "Slope: %f\n", Slope());
fprintf (pFile, "Intercept: %f\n", Intercept());
fprintf (pFile, "r^2 Correlation: %f\n", Correlation());
return;
}
inline double Ask(double ai, double bi) {
double k = -ai/bi;
long t = root;
while (true) {
long l1 = Prev(root, t), l2 = Succ(root, t);
double k1 = inf, k2 = -inf;
if (l1 != inf) k1 = Slope(l1, t);
if (l2 != inf) k2 = Slope(l2, t);
if (k <= k1 && k >= k2) break;
if (k2 > k) t = S[t].r;
else if (k1 < k) t = S[t].l;
}
return ai * S[t].x + bi * S[t].y;
}
void GlobalProblem::FindOptimalBudgetAllocation() {
long double remaining_budget = budget_;
// Create list of slopes.
vector<Slope> slopes;
for (int i = 0; i < num_partitions_; ++i) {
// Reset budget allocation.
budget_allocation_[i].second = 0;
if (subproblems_[i].envelope_points_.size() > 1) {
// -1 because at last envelope return on budget is 0.
for (int j = 0; j < subproblems_[i].envelope_points_.size() - 1; ++j) {
slopes.push_back(Slope(subproblems_[i].envelope_points_[j].first, i, j));
}
}
}
// Sort list of slopes.
sort(slopes.begin(), slopes.end(), compare_Slope());
// Allocate budgets in decreasing order of slopes.
int sp_index;
for (int k = 0; k < slopes.size(); ++k) {
sp_index = slopes[k].subproblem_index_;
long double allocation_increase = min(remaining_budget,
subproblems_[sp_index].budget_cutoffs_[slopes[k].region_index_ + 1] -
subproblems_[sp_index].budget_cutoffs_[slopes[k].region_index_]);
if ((allocation_increase > 0) && (remaining_budget > 0)) {
budget_allocation_[sp_index] = make_pair(slopes[k].region_index_, budget_allocation_[sp_index].second + allocation_increase);
}
remaining_budget = remaining_budget - allocation_increase;
}
}
inline int Get(Int64 k) {
int l = 1, r = top;
while (l <= r) {
int mid = (l + r) >> 1;
if (mid == r) return stack[mid];
if (mid == 1) {
int l1 = stack[1], l2 = stack[2];
if (Slope(l1, l2, k))
return l1;
else return l2;
}
int l1 = stack[mid - 1], l2 = stack[mid + 1], t = stack[mid];
bool c1 = Slope(l1, t, k), c2 = Slope(t, l2, k);
if (c1 && c2) r = mid - 1;else
if (!c1 && !c2) l = mid + 1;else
return t;
}
}
void TestSymplecticity(Integrator const& integrator,
Energy const& expected_energy_error) {
Length const q_initial = 1 * Metre;
Speed const v_initial = 0 * Metre / Second;
Instant const t_initial;
Instant const t_final = t_initial + 500 * Second;
Time const step = 0.2 * Second;
Mass const m = 1 * Kilogram;
Stiffness const k = SIUnit<Stiffness>();
Energy const initial_energy =
0.5 * m * Pow<2>(v_initial) + 0.5 * k * Pow<2>(q_initial);
std::vector<ODE::SystemState> solution;
ODE harmonic_oscillator;
harmonic_oscillator.compute_acceleration =
std::bind(ComputeHarmonicOscillatorAcceleration,
_1, _2, _3, /*evaluations=*/nullptr);
IntegrationProblem<ODE> problem;
problem.equation = harmonic_oscillator;
ODE::SystemState const initial_state = {{q_initial}, {v_initial}, t_initial};
problem.initial_state = &initial_state;
auto append_state = [&solution](ODE::SystemState const& state) {
solution.push_back(state);
};
auto const instance =
integrator.NewInstance(problem, std::move(append_state), step);
integrator.Solve(t_final, *instance);
std::size_t const length = solution.size();
std::vector<Energy> energy_error(length);
std::vector<Time> time(length);
Energy max_energy_error;
for (std::size_t i = 0; i < length; ++i) {
Length const q_i = solution[i].positions[0].value;
Speed const v_i = solution[i].velocities[0].value;
time[i] = solution[i].time.value - t_initial;
energy_error[i] =
AbsoluteError(initial_energy,
0.5 * m * Pow<2>(v_i) + 0.5 * k * Pow<2>(q_i));
max_energy_error = std::max(energy_error[i], max_energy_error);
}
double const correlation =
PearsonProductMomentCorrelationCoefficient(time, energy_error);
LOG(INFO) << "Correlation between time and energy error : " << correlation;
EXPECT_THAT(correlation, Lt(1e-2));
Power const slope = Slope(time, energy_error);
LOG(INFO) << "Slope : " << slope;
EXPECT_THAT(Abs(slope), Lt(2e-6 * SIUnit<Power>()));
LOG(INFO) << "Maximum energy error : " <<
max_energy_error;
EXPECT_EQ(expected_energy_error, max_energy_error);
}
void DrawFullLine(cv::Mat& img, cv::Point a, cv::Point b, cv::Scalar color, int LineWidth)
{
GRANSAC::VPFloat slope = Slope(a.x, a.y, b.x, b.y);
cv::Point p(0,0), q(img.cols, img.rows);
p.y = -(a.x - p.x) * slope + a.y;
q.y = -(b.x - q.x) * slope + b.y;
cv::line(img, p, q, color, LineWidth, 8, 0);
}
TEST_P(SimpleHarmonicMotionTest, Symplecticity) {
parameters_.initial.positions.emplace_back(SIUnit<Length>());
parameters_.initial.momenta.emplace_back(Speed());
parameters_.initial.time = Time();
Stiffness const k = SIUnit<Stiffness>();
Mass const m = SIUnit<Mass>();
Length const q0 = parameters_.initial.positions[0].value;
Speed const v0 = parameters_.initial.momenta[0].value;
Energy const initial_energy = 0.5 * m * Pow<2>(v0) + 0.5 * k * Pow<2>(q0);
parameters_.tmax = 500.0 * SIUnit<Time>();
parameters_.Δt = 0.2 * Second;
parameters_.sampling_period = 1;
integrator_->SolveTrivialKineticEnergyIncrement<Length>(
&ComputeHarmonicOscillatorAcceleration,
parameters_,
&solution_);
std::size_t const length = solution_.size();
std::vector<Energy> energy_error(length);
std::vector<Time> time_steps(length);
Energy max_energy_error = 0 * SIUnit<Energy>();
for (std::size_t i = 0; i < length; ++i) {
Length const q_i = solution_[i].positions[0].value;
Speed const v_i = solution_[i].momenta[0].value;
time_steps[i] = solution_[i].time.value;
energy_error[i] = Abs(0.5 * m * Pow<2>(v_i) + 0.5 * k * Pow<2>(q_i) -
initial_energy);
max_energy_error = std::max(energy_error[i], max_energy_error);
}
#if 0
LOG(INFO) << "Energy error as a function of time:\n" <<
BidimensionalDatasetMathematicaInput(time_steps, energy_error);
#endif
double const correlation =
PearsonProductMomentCorrelationCoefficient(time_steps, energy_error);
LOG(INFO) << GetParam();
LOG(INFO) << "Correlation between time and energy error : " << correlation;
EXPECT_THAT(correlation, Lt(2E-3));
Power const slope = Slope(time_steps, energy_error);
LOG(INFO) << "Slope : " << slope;
EXPECT_THAT(Abs(slope), Lt(2E-6 * SIUnit<Power>()));
LOG(INFO) << "Maximum energy error : " <<
max_energy_error;
EXPECT_EQ(GetParam().expected_energy_error, max_energy_error);
}
void TestConvergence(Integrator const& integrator,
Time const& beginning_of_convergence) {
Length const q_initial = 1 * Metre;
Speed const v_initial = 0 * Metre / Second;
Speed const v_amplitude = 1 * Metre / Second;
AngularFrequency const ω = 1 * Radian / Second;
Instant const t_initial;
Instant const t_final = t_initial + 100 * Second;
Time step = beginning_of_convergence;
int const step_sizes = 50;
double const step_reduction = 1.1;
std::vector<double> log_step_sizes;
log_step_sizes.reserve(step_sizes);
std::vector<double> log_q_errors;
log_step_sizes.reserve(step_sizes);
std::vector<double> log_p_errors;
log_step_sizes.reserve(step_sizes);
std::vector<ODE::SystemState> solution;
ODE harmonic_oscillator;
harmonic_oscillator.compute_acceleration =
std::bind(ComputeHarmonicOscillatorAcceleration,
_1, _2, _3, /*evaluations=*/nullptr);
IntegrationProblem<ODE> problem;
problem.equation = harmonic_oscillator;
ODE::SystemState const initial_state = {{q_initial}, {v_initial}, t_initial};
problem.initial_state = &initial_state;
ODE::SystemState final_state;
auto const append_state = [&final_state](ODE::SystemState const& state) {
final_state = state;
};
for (int i = 0; i < step_sizes; ++i, step /= step_reduction) {
auto const instance = integrator.NewInstance(problem, append_state, step);
integrator.Solve(t_final, *instance);
Time const t = final_state.time.value - t_initial;
Length const& q = final_state.positions[0].value;
Speed const& v = final_state.velocities[0].value;
double const log_q_error = std::log10(
AbsoluteError(q / q_initial, Cos(ω * t)));
double const log_p_error = std::log10(
AbsoluteError(v / v_amplitude, -Sin(ω * t)));
if (log_q_error <= -13 || log_p_error <= -13) {
// If we keep going the effects of finite precision will drown out
// convergence.
break;
}
log_step_sizes.push_back(std::log10(step / Second));
log_q_errors.push_back(log_q_error);
log_p_errors.push_back(log_p_error);
}
double const q_convergence_order = Slope(log_step_sizes, log_q_errors);
double const q_correlation =
PearsonProductMomentCorrelationCoefficient(log_step_sizes, log_q_errors);
LOG(INFO) << "Convergence order in q : " << q_convergence_order;
LOG(INFO) << "Correlation : " << q_correlation;
#if !defined(_DEBUG)
EXPECT_THAT(RelativeError(integrator.order, q_convergence_order),
Lt(0.05));
EXPECT_THAT(q_correlation, AllOf(Gt(0.99), Lt(1.01)));
#endif
double const v_convergence_order = Slope(log_step_sizes, log_p_errors);
double const v_correlation =
PearsonProductMomentCorrelationCoefficient(log_step_sizes, log_p_errors);
LOG(INFO) << "Convergence order in p : " << v_convergence_order;
LOG(INFO) << "Correlation : " << v_correlation;
#if !defined(_DEBUG)
// SPRKs with odd convergence order have a higher convergence order in p.
EXPECT_THAT(
RelativeError(integrator.order + (integrator.order % 2),
v_convergence_order),
Lt(0.03));
EXPECT_THAT(v_correlation, AllOf(Gt(0.99), Lt(1.01)));
#endif
}
double RunningRegression::Intercept() const
{
return y_stats.Mean() - Slope()*x_stats.Mean();
}
TEST_P(SimpleHarmonicMotionTest, Convergence) {
parameters_.initial.positions.emplace_back(SIUnit<Length>());
parameters_.initial.momenta.emplace_back(Speed());
parameters_.initial.time = Time();
#if defined(_DEBUG)
parameters_.tmax = 1 * SIUnit<Time>();
#else
parameters_.tmax = 100 * SIUnit<Time>();
#endif
parameters_.sampling_period = 0;
parameters_.Δt = GetParam().beginning_of_convergence;
int const step_sizes = 50;
double const step_reduction = 1.1;
std::vector<double> log_step_sizes;
log_step_sizes.reserve(step_sizes);
std::vector<double> log_q_errors;
log_step_sizes.reserve(step_sizes);
std::vector<double> log_p_errors;
log_step_sizes.reserve(step_sizes);
for (int i = 0; i < step_sizes; ++i, parameters_.Δt /= step_reduction) {
integrator_->SolveTrivialKineticEnergyIncrement<Length>(
&ComputeHarmonicOscillatorAcceleration,
parameters_,
&solution_);
double const log_q_error = std::log10(
std::abs(solution_[0].positions[0].value / SIUnit<Length>() -
Cos(solution_[0].time.value *
SIUnit<AngularFrequency>())));
double const log_p_error = std::log10(
std::abs(solution_[0].momenta[0].value / SIUnit<Speed>() +
Sin(solution_[0].time.value *
SIUnit<AngularFrequency>())));
if (log_q_error <= -13 || log_p_error <= -13) {
// If we keep going the effects of finite precision will drown out
// convergence.
break;
}
log_step_sizes.push_back(std::log10(parameters_.Δt / SIUnit<Time>()));
log_q_errors.push_back(log_q_error);
log_p_errors.push_back(log_p_error);
}
double const q_convergence_order = Slope(log_step_sizes, log_q_errors);
double const q_correlation =
PearsonProductMomentCorrelationCoefficient(log_step_sizes, log_q_errors);
LOG(INFO) << GetParam();
LOG(INFO) << "Convergence order in q : " << q_convergence_order;
LOG(INFO) << "Correlation : " << q_correlation;
#if 0
LOG(INFO) << "Convergence data for q :\n" <<
BidimensionalDatasetMathematicaInput(log_step_sizes, log_q_errors);
#endif
#if !defined(_DEBUG)
EXPECT_THAT(RelativeError(GetParam().convergence_order, q_convergence_order),
Lt(0.02));
EXPECT_THAT(q_correlation, AllOf(Gt(0.99), Lt(1.01)));
#endif
double const v_convergence_order = Slope(log_step_sizes, log_p_errors);
double const v_correlation =
PearsonProductMomentCorrelationCoefficient(log_step_sizes, log_p_errors);
LOG(INFO) << "Convergence order in p : " << v_convergence_order;
LOG(INFO) << "Correlation : " << v_correlation;
#if 0
LOG(INFO) << "Convergence data for p :\n" <<
BidimensionalDatasetMathematicaInput(log_step_sizes, log_q_errors);
#endif
#if !defined(_DEBUG)
// SPRKs with odd convergence order have a higher convergence order in p.
EXPECT_THAT(
RelativeError(((GetParam().convergence_order + 1) / 2) * 2,
v_convergence_order),
Lt(0.02));
EXPECT_THAT(v_correlation, AllOf(Gt(0.99), Lt(1.01)));
#endif
}
float Normal(float x) const { return -Slope(x); }
