vector<Eigen::Matrix4f> CollarLinesRegistrationPipeline::runRegistrationEffective(
const PolarGridOfClouds::Ptr &target_grid_cloud) {
assert(!history.empty());
stopwatch.restart();
Eigen::Matrix4f transformation = getPrediction();
vector<Eigen::Matrix4f> results;
int iterations = 0;
for(int i = 0; i < history.size(); i++) {
int current_iterations;
float error;
transformation = registerTwoGrids(*(history[i].getGridCloud()), *target_grid_cloud,
transformation, current_iterations, error);
iterations += current_iterations;
printInfo(stopwatch.elapsed(), iterations, transformation, error);
results.push_back(transformation);
Eigen::Matrix4f edge_trasform = history[i].getTransformation() * transformation;
PoseGraphEdge edge(history[i].getIndex(), pose_index, edge_trasform);
graph_file << edge << endl;
}
return results;
}
// serial implementation of accuracy test
// computes true positive rate, false positive rate and error rate, then adds
// these values to variables passed through pointers
double computeErrorRate(
DataSet data_set,
double* cumulative_true_positive_rate,
double* cumulative_false_positive_rate,
double* cumulative_error){
size_t true_positives = 0, true_negatives = 0, false_positives = 0, false_negatives = 0;
for (size_t i = 0; i < data_set.num_data_points; i++) {
LabelType prediction = getPrediction(data_set.parameter_vector, &data_set.data_points[i * data_set.num_features], data_set.num_features);
if (prediction != data_set.labels[i]){
if (prediction == POSITIVE_LABEL)
false_positives++;
else
false_negatives++;
} else {
if (prediction == POSITIVE_LABEL)
true_positives++;
else
true_negatives++;
}
}
double error_rate = (double)(false_positives + false_negatives) / data_set.num_data_points;
if (cumulative_true_positive_rate != NULL) *cumulative_true_positive_rate += (double)true_positives / (true_positives + false_negatives);
if (cumulative_false_positive_rate != NULL) *cumulative_false_positive_rate += (double)false_positives / (false_positives + true_negatives);
if (cumulative_error != NULL) *cumulative_error += error_rate;
return error_rate;
}
void ComplementaryFilter::updateImpl(
double gx, double gy, double gz,
double ax, double ay, double az,
double dt)
{
if (!initialized_)
{
// First time - ignore prediction:
getMeasurement(ax, ay, az,
q0_, q1_, q2_, q3_);
initialized_ = true;
return;
}
if(dt <= 0.0)
{
UERROR("dt=%f <=0.0, orientation will not be updated!", dt);
return;
}
// Bias estimation.
if (do_bias_estimation_)
updateBiases(ax, ay, az, gx, gy, gz);
// Prediction.
double q0_pred, q1_pred, q2_pred, q3_pred;
getPrediction(gx, gy, gz, dt,
q0_pred, q1_pred, q2_pred, q3_pred);
// Correction (from acc):
// q_ = q_pred * [(1-gain) * qI + gain * dq_acc]
// where qI = identity quaternion
double dq0_acc, dq1_acc, dq2_acc, dq3_acc;
getAccCorrection(ax, ay, az,
q0_pred, q1_pred, q2_pred, q3_pred,
dq0_acc, dq1_acc, dq2_acc, dq3_acc);
double gain;
if (do_adaptive_gain_)
{
gain = getAdaptiveGain(gain_acc_, ax, ay, az);
}
else
{
gain = gain_acc_;
}
scaleQuaternion(gain, dq0_acc, dq1_acc, dq2_acc, dq3_acc);
quaternionMultiplication(q0_pred, q1_pred, q2_pred, q3_pred,
dq0_acc, dq1_acc, dq2_acc, dq3_acc,
q0_, q1_, q2_, q3_);
normalizeQuaternion(q0_, q1_, q2_, q3_);
}
/* ************************************************************************
* Recursive function to grow trees. Compute a splitting rule and make the partition
* for the current node. finally make a call to itself with every child node created.
* param :
* node : the current node to be split
* sortedInd : a 2D array of sorted index of instances.
*/
int RndTree::growSubTree(Node * node, u_int ** sortedInd)
{
DataHandler * data = node->getDataSet();
Rule * rule;
// launch the random feature selection process and create the splitting rule with the chosen feature.
//if(rndFeatSel)
rule = randomFeatSelection(node,sortedInd);
//else
// rule = featSelection(node,sortedInd);
// if no rule has been created, the branch induction is stopped.
// (It means that no feature has managed to produce a split)
if (rule == NULL)
{
// the node is transform into a leaf
int predic = getPrediction(data);
node->makeLeaf(predic);
//delete data;
}
else
{
u_int *** subsets = NULL;
node->setRule(rule);
subsets = partitionNode(node,rule,sortedInd);
//system("pause");
for(u_int j=0; j<node->getNbChildren(); j++) {
Node * n = node->getChild(j);
if(!(n->is_leaf())) {
growSubTree(n,subsets[j]);
}
for(u_int k=0; k<data->dim(); k++)
if(k != data->getClassInd()) delete[] subsets[j][k];
delete[] subsets[j];
}
delete[] subsets;
}
return 0;
}
void GraphLearner::sendTreeTransforms(KinematicGraphType graph) {
if (graph.size() == 0)
return;
double latestTimestamp = stampedMarker.rbegin()->first;
PoseMap observation = getObservation(latestTimestamp);
PoseMap prediction = getPrediction(intersectionOfStamps().back(), graph,
observation);
// send transform for root node
static tf::TransformBroadcaster br;
const PoseStamped &latestPose =
stampedMarker[latestTimestamp].begin()->second->pose;
for (auto it = prediction.begin(); it != prediction.end(); it++) {
br.sendTransform(tf::StampedTransform(poseToTransform(it->second),
latestPose.header.stamp, "/camera", boost::str(boost::format(
"/pred/%1%") % it->first)));
}
}
/* ************************************************************************
* launch the tree induction process.
*/
DTree * Cart::growTree(DataHandler * set)
{
trainSet = set;
if(trainSet == NULL)
{
if(disp)
{
string tmp = "\n\nERREUR : RndTree::growTree()";
tmp += "\n\t Vous n'avez désigné aucune base de données pour l'apprentissage";
Utils::print(tmp);
}
return NULL;
}
// grow an empty tree, i.e. with a single node (the root node)
DTree * p_tree = new DTree(trainSet);
// If the trainSet contains only data from one class, the root is transform into a leaf
if(is_leaf(trainSet,0))
{
// the root node is transform into a leaf
int predic = getPrediction(trainSet);
p_tree->getRoot()->rootToLeaf(predic);
p_tree->stat_setTimeTrain(0.0);
return p_tree;
}
// initialization :
// Have a 2D array to sort data indices according to each attribute
u_int ** sortedInd = new u_int*[trainSet->dim()];
for(u_int i=0; i<trainSet->dim(); i++)
{
if(i != trainSet->getClassInd())
{
vector<double> vals;
for(v_inst_it it=trainSet->begin(); it!=trainSet->end(); it++)
vals.push_back((*it)->at(i));
sortedInd[i] = Utils::sort(vals);
}
}
double _time = ((double) clock());
// launch tree growing from the tree root.
int ret = growSubTree(p_tree->getRoot(),sortedInd);
if(ret == -1)
{
string tmp = "\n\nERREUR : RndTree::growSubTree()";
Utils::print(tmp);
}
_time = (((double) clock()) - _time) /CLOCKS_PER_SEC;
if(_time>=0) p_tree->stat_setTimeTrain(_time);
else p_tree->stat_setTimeTrain(0.0);
for(u_int i=0; i<trainSet->dim(); i++)
if(i != trainSet->getClassInd()) delete[] sortedInd[i];
delete[] sortedInd;
return p_tree;
}
double Sample::getError(const Feature &feature) const {
return (getPrediction(feature) != isMale()) * getWeight();
}
void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time)
{
ros::WallTime start = ros::WallTime::now();
// CSM is used in the following way:
// The scans are always in the laser frame
// The reference scan (prevLDPcan_) has a pose of [0, 0, 0]
// The new scan (currLDPScan) has a pose equal to the movement
// of the laser in the laser frame since the last scan
// The computed correction is then propagated using the tf machinery
prev_ldp_scan_->odometry[0] = 0.0;
prev_ldp_scan_->odometry[1] = 0.0;
prev_ldp_scan_->odometry[2] = 0.0;
prev_ldp_scan_->estimate[0] = 0.0;
prev_ldp_scan_->estimate[1] = 0.0;
prev_ldp_scan_->estimate[2] = 0.0;
prev_ldp_scan_->true_pose[0] = 0.0;
prev_ldp_scan_->true_pose[1] = 0.0;
prev_ldp_scan_->true_pose[2] = 0.0;
input_.laser_ref = prev_ldp_scan_;
input_.laser_sens = curr_ldp_scan;
// **** estimated change since last scan
double dt = (time - last_icp_time_).toSec();
double pr_ch_x, pr_ch_y, pr_ch_a;
getPrediction(pr_ch_x, pr_ch_y, pr_ch_a, dt);
// the predicted change of the laser's position, in the fixed frame
tf::Transform pr_ch;
createTfFromXYTheta(pr_ch_x, pr_ch_y, pr_ch_a, pr_ch);
// account for the change since the last kf, in the fixed frame
pr_ch = pr_ch * (f2b_ * f2b_kf_.inverse());
// the predicted change of the laser's position, in the laser frame
tf::Transform pr_ch_l;
pr_ch_l = laser_to_base_ * f2b_.inverse() * pr_ch * f2b_ * base_to_laser_ ;
input_.first_guess[0] = pr_ch_l.getOrigin().getX();
input_.first_guess[1] = pr_ch_l.getOrigin().getY();
input_.first_guess[2] = tf::getYaw(pr_ch_l.getRotation());
// *** scan match - using point to line icp from CSM
sm_icp(&input_, &output_);
tf::Transform corr_ch;
if (output_.valid)
{
// the correction of the laser's position, in the laser frame
tf::Transform corr_ch_l;
createTfFromXYTheta(output_.x[0], output_.x[1], output_.x[2], corr_ch_l);
// the correction of the base's position, in the base frame
corr_ch = base_to_laser_ * corr_ch_l * laser_to_base_;
// update the pose in the world frame
f2b_ = f2b_kf_ * corr_ch;
// **** publish
if (publish_pose_)
{
// unstamped Pose2D message
geometry_msgs::Pose2D::Ptr pose_msg;
pose_msg = boost::make_shared<geometry_msgs::Pose2D>();
pose_msg->x = f2b_.getOrigin().getX();
pose_msg->y = f2b_.getOrigin().getY();
pose_msg->theta = tf::getYaw(f2b_.getRotation());
pose_publisher_.publish(pose_msg);
}
if (publish_pose_stamped_)
{
// stamped Pose message
geometry_msgs::PoseStamped::Ptr pose_stamped_msg;
pose_stamped_msg = boost::make_shared<geometry_msgs::PoseStamped>();
pose_stamped_msg->header.stamp = time;
pose_stamped_msg->header.frame_id = fixed_frame_;
tf::poseTFToMsg(f2b_, pose_stamped_msg->pose);
pose_stamped_publisher_.publish(pose_stamped_msg);
}
if (publish_tf_)
{
tf::StampedTransform transform_msg (f2b_, time, fixed_frame_, base_frame_);
tf_broadcaster_.sendTransform (transform_msg);
}
}
else
{
corr_ch.setIdentity();
ROS_WARN("Error in scan matching");
}
// **** swap old and new
if (newKeyframeNeeded(corr_ch))
{
// generate a keyframe
ld_free(prev_ldp_scan_);
prev_ldp_scan_ = curr_ldp_scan;
f2b_kf_ = f2b_;
}
else
{
ld_free(curr_ldp_scan);
}
last_icp_time_ = time;
// **** statistics
double dur = (ros::WallTime::now() - start).toSec() * 1e3;
ROS_DEBUG("Scan matcher total duration: %.1f ms", dur);
}
main()
{
int weeks,i,c,j,w=0,g=0,t1,t2,num1,num2,team_index1,team_index2,score1,score2,week_counter,a,hold;
int games_played[]={0,0,0,0,0,0};
int total_points[]={0,0,0,0,0,0};
int points_against[]={0,0,0,0,0,0};
int team_index[]={0,1,2,3,4,5};
int team_order[]={0,0,0,0,0,0};
double win_percentage[]={0,0,0,0,0,0};
printf("How many weeks total? (up to 10) ");
scanf("%i", &weeks);
int team_number[]={0,0,0,0,0,0};
int points_ordered[]={0,0,0,0,0,0};
int points[]={0,0,0,0,0,0};
int team_win[]={0,0,0,0,0,0};
int team_tied[]={0,0,0,0,0,0};
int team_loss[]={0,0,0,0,0,0};
// Note: That if is equal to else then pi is equal to life
if(weeks > 10)
printf("Error: Only up to 10 weeks can be entered.");
else
{
for(i=0;i<weeks;i++) //runs for each week
{
++g;
++w;
if(g>=4){
w=1;
g=1;
}
for(j=0;j<3;j++){ //runs for each game
week_counter= j+1;
printf("\n---Week %i Game %i---\n", w, week_counter);
printf("Enter the two teams and their scores: ");
scanf("%i%i%i%i",&num1,&num2,&score1,&score2);
games_played[num1]+=1;
games_played[num2]+=1;
total_points[num1]+=score1;
total_points[num2]+=score2;
points_against[num1]+=score2;
points_against[num2]+=score1;
if(score1<score2){
team_win[num2]+=1;
team_loss[num1]+=1;
}
else if(score2<score1){
team_win[num1]+=1;
team_loss[num2]+=1;
}
else if(score1==score2){
team_tied[num1]+=1;
team_tied[num2]+=1;
}
}
}
for(i=0;i<6;i++){
team_index[i]=points[i];
}
//Points
for(j=0;j<6;j++){
points[j]= (team_win[j]*2) + (team_tied[j]*1);
}
printf("\n----------------------------------------\n\nLeague Standings after %i weeks\n\n",weeks);
printf("\t W T L\n");
for(j=0;j<6;j++){
printf("\tTeam %i: %i %i %i\n",j,team_win[j],team_tied[j],team_loss[j]);
}
for(i=0;i<6;i++){
win_percentage[i]=(float)team_win[i]/(float)games_played[i];
printf("\nTeam %i winning percentage: %.2f\n",i,win_percentage[i]);
}
printf("\n----------------------------------------\nTotal Points For:\n");
//'Modify to compute and print for each team the total points scored for and against'
for(i=0;i<6;i++){
printf("Team %i: %i\n",i,total_points[i]);
}
printf("\n\nTotal Points Against\n");
for(i=0;i<6;i++){
printf("Team %i: %i\n",i,points_against[i]);
}
printf("\n\n----------------------------------------\n");
int newOrder[6];
for(i=0;i<6;i++){
newOrder[i] = points[i];
}
int order = getOrder(newOrder);
//Next block of code calls a test for a new prediction algorithm function written below
int prediction = getPrediction(total_points,games_played);
}
getch();
}
