pa_usec_t pa_smoother_get(pa_smoother *s, pa_usec_t x) {
pa_usec_t y;
pa_assert(s);
/* Fix up x value */
if (s->paused)
x = s->pause_time;
x = PA_LIKELY(x >= s->time_offset) ? x - s->time_offset : 0;
if (s->monotonic)
if (x <= s->last_x)
x = s->last_x;
estimate(s, x, &y, NULL);
if (s->monotonic) {
/* Make sure the querier doesn't jump forth and back. */
s->last_x = x;
if (y < s->last_y)
y = s->last_y;
else
s->last_y = y;
}
#ifdef DEBUG_DATA
pa_log_debug("%p, get(%llu | %llu) = %llu", s, (unsigned long long) (x + s->time_offset), (unsigned long long) x, (unsigned long long) y);
#endif
return y;
}
bool VertexPointXYZCov::write(std::ostream& os) const
{
/// we store the id of the observation / parent vertex for faster lookups...
/// this information is also available through the connected edges
os << _parentVertex->id() << " ";
/// estimated pose of point (global coordinate frame):
/// this will be different from the measured depth after optimization!
for (int i=0; i<3; i++)
os << estimate()[i] << " ";
/// color information for the point (if available)
uint r,g,b;
r = (uint) cr;
g = (uint) cg;
b = (uint) cb;
os << r << " " << g << " " << b << " ";
///covariance of the point, computed from the neighborhood (do we really need to store in on the drive??)
for (int i=0; i<3; i++)
for (int j=i; j<3; j++){
os << _cov(i,j) << " ";
}
return os.good();
}
TagPoseMap estimate(
const cv::Mat &inputImage,
Chilitags::DetectionTrigger detectionTrigger,
cv::Vec<RealT, 4> const& camDeltaR,
cv::Vec<RealT, 3> const& camDeltaX) {
return estimate(mChilitags.find(inputImage, detectionTrigger), camDeltaR, camDeltaX);
}
pa_usec_t pa_smoother_translate(pa_smoother *s, pa_usec_t x, pa_usec_t y_delay) {
pa_usec_t ney;
double nde;
pa_assert(s);
/* Fix up x value */
if (s->paused)
x = s->pause_time;
x = PA_LIKELY(x >= s->time_offset) ? x - s->time_offset : 0;
estimate(s, x, &ney, &nde);
/* Play safe and take the larger gradient, so that we wakeup
* earlier when this is used for sleeping */
if (s->dp > nde)
nde = s->dp;
#ifdef DEBUG_DATA
pa_log_debug("translate(%llu) = %llu (%0.2f)", (unsigned long long) y_delay, (unsigned long long) ((double) y_delay / nde), nde);
#endif
return (pa_usec_t) llrint((double) y_delay / nde);
}
size_t PoseEstimator<PointSource,PointTarget>::estimate(const NDTFrame<PointSource> &f0, const NDTFrame<PointTarget> &f1)
{
// set up match lists
matches.clear();
inliers.clear();
// std::cout<<"N KPTS: "<<f0.kpts.size()<<" "<<f1.kpts.size()<<std::endl;
// std::cout<<"descriptors: "<<f0.dtors.size().width<<"x"<<f0.dtors.size().height<<" "
// <<f1.dtors.size().width<<"x"<<f1.dtors.size().height<<std::endl;
// do forward and reverse matches
std::vector<cv::DMatch> fwd_matches, rev_matches;
matchFrames(f0, f1, fwd_matches);
matchFrames(f1, f0, rev_matches);
// printf("**** Forward matches: %d, reverse matches: %d ****\n", (int)fwd_matches.size(), (int)rev_matches.size());
// combine unique matches into one list
for (int i = 0; i < (int)fwd_matches.size(); ++i)
{
if (fwd_matches[i].trainIdx >= 0)
matches.push_back( cv::DMatch(i, fwd_matches[i].trainIdx, fwd_matches[i].distance) );
//matches.push_back( cv::DMatch(fwd_matches[i].queryIdx, fwd_matches[i].trainIdx, fwd_matches[i].distance) );
}
for (int i = 0; i < (int)rev_matches.size(); ++i)
{
if (rev_matches[i].trainIdx >= 0 && i != fwd_matches[rev_matches[i].trainIdx].trainIdx)
matches.push_back( cv::DMatch(rev_matches[i].trainIdx, i, rev_matches[i].distance) );
//matches.push_back( cv::DMatch(rev_matches[i].trainIdx, rev_matches[i].queryIdx, rev_matches[i].distance) );
}
// printf("**** Total unique matches: %d ****\n", (int)matches.size());
// do it
return estimate(f0, f1, matches);
}
bool VertexSBAPointXYZ::write(std::ostream& os) const
{
Vector3d lv=estimate();
for (int i=0; i<3; i++){
os << lv[i] << " ";
}
return os.good();
}
bool TreeRegression::splitNodeInternal(size_t nodeID, std::vector<size_t>& possible_split_varIDs) {
// Check node size, stop if maximum reached
if (sampleIDs[nodeID].size() <= min_node_size) {
split_values[nodeID] = estimate(nodeID);
return true;
}
// Find best split, stop if no decrease of impurity
bool stop = findBestSplit(nodeID, possible_split_varIDs);
if (stop) {
split_values[nodeID] = estimate(nodeID);
return true;
}
return false;
}
void PlaneSupportedCuboidEstimator::cloudCallback(
const sensor_msgs::PointCloud2::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
NODELET_INFO("cloudCallback");
if (!latest_polygon_msg_ || !latest_coefficients_msg_) {
JSK_NODELET_WARN("Not yet polygon is available");
return;
}
if (!tracker_) {
NODELET_INFO("initTracker");
// Update polygons_ vector
polygons_.clear();
for (size_t i = 0; i < latest_polygon_msg_->polygons.size(); i++) {
Polygon::Ptr polygon = Polygon::fromROSMsgPtr(latest_polygon_msg_->polygons[i].polygon);
polygons_.push_back(polygon);
}
// viewpoint
tf::StampedTransform transform
= lookupTransformWithDuration(tf_, sensor_frame_, msg->header.frame_id,
ros::Time(0.0),
ros::Duration(0.0));
tf::vectorTFToEigen(transform.getOrigin(), viewpoint_);
ParticleCloud::Ptr particles = initParticles();
tracker_.reset(new pcl::tracking::ROSCollaborativeParticleFilterTracker<pcl::PointXYZ, pcl::tracking::ParticleCuboid>);
tracker_->setCustomSampleFunc(boost::bind(&PlaneSupportedCuboidEstimator::sample, this, _1));
tracker_->setLikelihoodFunc(boost::bind(&PlaneSupportedCuboidEstimator::likelihood, this, _1, _2));
tracker_->setParticles(particles);
tracker_->setParticleNum(particle_num_);
support_plane_updated_ = false;
// Publish histograms
publishHistogram(particles, 0, pub_histogram_global_x_, msg->header);
publishHistogram(particles, 1, pub_histogram_global_y_, msg->header);
publishHistogram(particles, 2, pub_histogram_global_z_, msg->header);
publishHistogram(particles, 3, pub_histogram_global_roll_, msg->header);
publishHistogram(particles, 4, pub_histogram_global_pitch_, msg->header);
publishHistogram(particles, 5, pub_histogram_global_yaw_, msg->header);
publishHistogram(particles, 6, pub_histogram_dx_, msg->header);
publishHistogram(particles, 7, pub_histogram_dy_, msg->header);
publishHistogram(particles, 8, pub_histogram_dz_, msg->header);
// Publish particles
sensor_msgs::PointCloud2 ros_particles;
pcl::toROSMsg(*particles, ros_particles);
ros_particles.header = msg->header;
pub_particles_.publish(ros_particles);
}
else {
ParticleCloud::Ptr particles = tracker_->getParticles();
Eigen::Vector4f center;
pcl::compute3DCentroid(*particles, center);
if (center.norm() > fast_cloud_threshold_) {
estimate(msg);
}
}
}
double operator()(const std::vector<int>& c) const {
int outerLoopIterationsCount;
int innerLoop1IterationsCount;
int innerLoop2IterationsCount;
int logEvaluationCount;
return estimate(c, outerLoopIterationsCount, innerLoop1IterationsCount, innerLoop2IterationsCount, logEvaluationCount);
}
void estimatePolynomial(const size_t order, const Histogram &histo,
const size_t i_min, const size_t i_max, double &out_bg0,
double &out_bg1, double &out_bg2,
double &out_chisq_red) {
const auto &X = histo.points();
const auto &Y = histo.y();
estimate(order, X, Y, i_min, i_max, 0, 0, false, out_bg0, out_bg1, out_bg2,
out_chisq_red);
}
bool VertexPlane3d::write(std::ostream &os) const
{
Eigen::Vector3d lv = estimate();
for (int i = 0; i < estimateDimension(); i++)
os << lv[i] << " ";
return os.good();
}
//==============================================================================
// returns a sign-accurate result
// (zero if and only if the true answer is zero or underflows too far)
static double
accurate_orientation2d(const double* x0,
const double* x1,
const double* x2)
{
expansion result;
expansion_orientation2d(x0, x1, x2, result);
return estimate(result);
}
bool G2oVertexPointXYZ
::write (std::ostream & os) const
{
const Vector3d & lv = estimate();
for (int i=0; i<3; i++)
{
os << lv[i] << " ";
}
return true;
}
affinity::affinity(float u1, float v1, float up1, float vp1,
float u2, float v2, float up2, float vp2,
float u3, float v3, float up3, float vp3)
{
initialize();
estimate(u1, v1, up1, vp1,
u2, v2, up2, vp2,
u3, v3, up3, vp3);
}
Vector3f twostep_bias_only(const Vector3f samples[],
size_t n_samples,
const Vector3f & referenceField,
const float noise)
{
// \tilde{H}
Vector3f *centeredSamples = new Vector3f[n_samples];
// z_k
float sampleDeltaMag[n_samples];
// eq 7 and 8 applied to samples
Vector3f avg = center(samples, n_samples);
float refSquaredNorm = referenceField.squaredNorm();
float sampleDeltaMagCenter = 0;
for (size_t i = 0; i < n_samples; ++i) {
// eq 9 applied to samples
centeredSamples[i] = samples[i] - avg;
// eqn 2a
sampleDeltaMag[i] = samples[i].squaredNorm() - refSquaredNorm;
sampleDeltaMagCenter += sampleDeltaMag[i];
}
sampleDeltaMagCenter /= n_samples;
Matrix3f P_bb;
Matrix3f P_bb_inv;
// Due to eq 12b
inv_fisher_information_matrix(P_bb, P_bb_inv, centeredSamples, n_samples, noise);
// Compute centered magnitudes
float sampleDeltaMagCentered[n_samples];
for (size_t i = 0; i < n_samples; ++i) {
sampleDeltaMagCentered[i] = sampleDeltaMag[i] - sampleDeltaMagCenter;
}
// From eq 12a
Vector3f estimate(Vector3f::Zero());
for (size_t i = 0; i < n_samples; ++i) {
estimate += sampleDeltaMagCentered[i] * centeredSamples[i];
}
estimate = P_bb * ((2 / noise) * estimate);
// Newton-Raphson gradient descent to the optimal solution
// eq 14a and 14b
float mu = -3 * noise;
for (int i = 0; i < 6; ++i) {
Vector3f neg_gradiant = neg_dJdb(samples, sampleDeltaMag, n_samples,
estimate, mu, noise);
Matrix3f scale = P_bb_inv + 4 / noise * (avg - estimate) * (avg - estimate).transpose();
Vector3f neg_increment;
scale.ldlt().solve(neg_gradiant, &neg_increment);
// Note that the negative has been done twice
estimate += neg_increment;
}
delete[] centeredSamples;
return estimate;
}
int main(int argc, char **argv) {
if (argc != 4) {
printf("Usage: %s input data output\n", argv[0]);
return 0;
}
//printf("load\n");
load(argv[1]);
estimate(argv[2], argv[3]);
return 0;
}
double cal_rmse() {
rmse = 0.;
auto rmse_lambda = [&] (const std::string & uid,
const std::string & iid,
double rating) {
double e = rating - estimate(uid, iid);
rmse += e * e;
};
rating_graph.traverse(rmse_lambda);
return sqrt(rmse / rating_sz);
}
/// Extend terms by adding view for result
FloatView
extend(Home home, Region& r, Term*& t, int& n) {
FloatNum min, max;
estimate(t,n,0.0,min,max);
FloatVar x(home,min,max);
Term* et = r.alloc<Term>(n+1);
for (int i=n; i--; )
et[i] = t[i];
et[n].a=-1.0; et[n].x=x;
t=et; n++;
return x;
}
void arow::train(const common::sfv_t& fv, double value) {
double predict = estimate(fv);
double error = value - predict;
double sign_error = error > 0.0 ? 1.0 : -1.0;
double loss = sign_error * error - config_.sensitivity;
if (loss > 0.0) {
double variance = calc_variance(fv);
double beta = 1.0 / (variance + 1.0 / config_.regularization_weight);
double alpha = sign_error * loss * beta;
update(fv, alpha, beta);
}
}
void estimateBackground(const size_t order, const Histogram &histo,
const size_t i_min, const size_t i_max,
const size_t p_min, const size_t p_max, double &out_bg0,
double &out_bg1, double &out_bg2,
double &out_chisq_red) {
const auto &X = histo.points();
const auto &Y = histo.y();
// fit with a hole in the middle
estimate(order, X, Y, i_min, i_max, p_min, p_max, true, out_bg0, out_bg1,
out_bg2, out_chisq_red);
}
//==============================================================================
// returns a sign-accurate result
// (zero if and only if the true answer is zero or underflows too far)
static double
accurate_orientation_time3d(const double* x0, int time0,
const double* x1, int time1,
const double* x2, int time2,
const double* x3, int time3,
const double* x4, int time4)
{
expansion result;
expansion_orientation_time3d(x0, time0, x1, time1, x2, time2,
x3, time3, x4, time4, result);
return estimate(result);
}
bool VertexSE3Expmap::write(std::ostream& os) const
{
SE3Quat cam2world(estimate().inverse());
for (int i=0; i<7; i++)
os << cam2world[i] << " ";
for (int i=0; i<2; i++)
os << _focal_length[i] << " ";
for (int i=0; i<2; i++)
os << _principle_point[i] << " ";
os << _baseline << " ";
return os.good();
}
int find(state_t *s)
{
state_t t;
int i, k;
k = s->g + estimate(s);
if (k > flimit) {
if (k < fnext) fnext = k;
return 0;
}
if (s->n == m) {
fnext = k;
return 1;
}
for (k = s->s; k >= 0; k--) {
t.n = s->n + k;
if (t.n > m) continue;
t.g = s->g + 1;
t.s = s->s - k;
for (i = 0; i < s->n; i++) {
t.a[i] = s->a[i] + 1;
if (t.a[i] == hp1 || t.a[i] == hp2) t.s++;
}
for (i = 0; i < k; i++)
t.a[s->n + i] = 1;
if (t.n == 0)
continue;
canonic(&t);
for (i = t.n - 1; i >= 0; i--) {
if (i > 0 && t.a[i - 1] == t.a[i])
continue;
t.a[i]++;
if (t.a[i] == hp1 || t.a[i] == hp2) t.s++;
if (find(&t)) return 1;
if (t.a[i] == hp1 || t.a[i] == hp2) t.s--;
t.a[i]--;
}
}
return 0;
}
// Estimation of information dimension for two variables
double estimateCov(int xnum, double *yall, int width, _Bool cov, double minent) {
double res;
do {
if (width > 1) {
res = estimate(xnum, yall, width--, cov, minent);
} else {
res = 0;
break;
}
} while (res <= 0);
return(res);
}
bool VertexSE3Expmap::read(std::istream& is)
{
SE3Quat cam2world;
for (int i=0; i<7; i++)
is >> cam2world[i];
estimate() = cam2world.inverse();
for (int i=0; i<2; i++)
is >> _focal_length[i];
for (int i=0; i<2; i++)
is >> _principle_point[i];
is >> _baseline;
return true;
}
bool VertexSim3Expmap::write(std::ostream &os) const {
Sim3 cam2world(estimate().inverse());
Vector7d lv = cam2world.log();
for (int i = 0; i < 7; i++) {
os << lv[i] << " ";
}
for (int i = 0; i < 2; i++) {
os << _focal_length1[i] << " ";
}
for (int i = 0; i < 2; i++) {
os << _principle_point1[i] << " ";
}
return os.good();
}
pKind_t runGameH2C(state_t st) { //人vsCOM
st->mode = H2C;
runState_t rs;
while(1) {
rs = Hturn(st, A);
if(rs == finish) break;
rs = Cturn(st, B);
if(rs == finish) break;
}
estimate(st);
if(st->value > 0) return A;
else if(st->value < 0) return B;
else return NONE;//引き分け
}
//------------------------------------------------------------------------------
Vector2ui Font::estimate_multiline(const std::string& text) const
{
std::vector<std::string> lines = explode(text, "\n");
Vector2ui result(0,0);
for (unsigned int i = 0; i < lines.size(); i++)
{
Vector2ui line_size = estimate(lines[i]);
result(0) = std::max(result(0), line_size(0));
result(1) += line_size(1);
}
return result;
}
void PlaneSupportedCuboidEstimator::fastCloudCallback(
const sensor_msgs::PointCloud2::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!tracker_) {
return;
}
ParticleCloud::Ptr particles = tracker_->getParticles();
Eigen::Vector4f center;
pcl::compute3DCentroid(*particles, center);
if (center.norm() < fast_cloud_threshold_) {
estimate(msg);
}
}
sq::sq(region &r) {
#define MAX_LEN 25000
struct vect list[MAX_LEN];
int x, y, min_x, min_y, max_x, max_y, no;
struct point p;
int sing;
no = 0;
min_x = r.minx;
min_y = r.miny;
max_x = r.maxx;
max_y = r.maxy;
for(x = min_x; x <= max_x; x++)
for(y = min_y; y <= max_y; y++)
if (r.included(x, y))
if (no < MAX_LEN)
{ p = r.get_point(x,y);
list[no].x = p.x;
list[no].y = p.y;
list[no++].z = p.z;
}
else
std::cout << "Over the limit of SQ points";
double t[4][4];
estimate(list, no, t);
convert(list, no, t, &a1, &a2, &a3, &e1, &e2, &px, &py, &pz, &phi, &theta, &psi);
//std::cout << "Estimate: \n";
//print();
//std::cout << "------------\n";
printf("\nSQ recovering from scratch %d points", no); fflush(stdout);
if (!recover_params(this, list, no))
{ printf("\nSQ recover procedure returned error!"); fflush(stdout);
}
printf("\nSQ recover ended"); fflush(stdout);
g_from_l.translate_g(px, py, pz);
g_from_l.rotate_zr(phi);
g_from_l.rotate_yr(theta);
g_from_l.rotate_zr(psi);
l_from_g = g_from_l.inverse(sing);
}
