Skip to content

Commit

Permalink
Hiding superfluous printouts behind the debug flag
Browse files Browse the repository at this point in the history
  • Loading branch information
@ViktorWalter
ViktorWalter committed Mar 20, 2024
1 parent 257caad commit 7585e34
Showing 1 changed file with 31 additions and 16 deletions.
47 changes: 31 additions & 16 deletions src/pose_calculator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1161,11 +1161,13 @@ namespace uvdar {
else{

removeExtraHypotheses(index, latest_local.time);
ROS_INFO("[%s]: Culling. Curr hypothesis count: %d", ros::this_node::getName().c_str(), (int)(hypothesis_buffer_.at(index).hypotheses.size()));
if (_debug_)
ROS_INFO("[%s]: Culling. Curr hypothesis count: %d", ros::this_node::getName().c_str(), (int)(hypothesis_buffer_.at(index).hypotheses.size()));
/* hypothesis_buffer_.at(index).hypotheses.insert(hypothesis_buffer_.at(index).hypotheses.end(), new_hypotheses.begin(), new_hypotheses.end()); */
hypothesis_buffer_.at(index).addHypotheses(new_hypotheses);
/* hypothesis_buffer_.at(index).verified_count+=(int)(new_hypotheses.size()); */
ROS_INFO("[%s]: Inserting new. Curr hypothesis count: %d", ros::this_node::getName().c_str(), (int)(hypothesis_buffer_.at(index).hypotheses.size()));
if (_debug_)
ROS_INFO("[%s]: Inserting new. Curr hypothesis count: %d", ros::this_node::getName().c_str(), (int)(hypothesis_buffer_.at(index).hypotheses.size()));
}

}
Expand All @@ -1183,15 +1185,17 @@ namespace uvdar {
}

}
ROS_INFO("[%s]: Hypothesis count: ", ros::this_node::getName().c_str());
for (auto &hb : hypothesis_buffer_){
ROS_INFO("[%s]: target: %d: %d", ros::this_node::getName().c_str(), hb.target,(int)(hb.hypotheses.size()));
if (_debug_){
ROS_INFO("[%s]: Hypothesis count: ", ros::this_node::getName().c_str());
for (auto &hb : hypothesis_buffer_){
ROS_INFO("[%s]: target: %d: %d", ros::this_node::getName().c_str(), hb.target,(int)(hb.hypotheses.size()));
}
}
batch_processsed_ = true;

/* r.sleep(); */
/* } */
ROS_INFO("[UVDARPoseCalculator]:IN: I");
/* ROS_INFO("[UVDARPoseCalculator]:IN: I"); */
}
//}
//
Expand All @@ -1214,14 +1218,17 @@ namespace uvdar {
for (int index = 0; index < (int)(hypothesis_buffer_.size()); index++){
std::scoped_lock lock(hypothesis_mutex);
int verified_count = hypothesis_buffer_.at(index).verified_count;
ROS_INFO("[%s]: Prev. hypothesis count: %d, of which verified: %d", ros::this_node::getName().c_str(), (int)(hypothesis_buffer_.at(index).hypotheses.size()), verified_count);
if (_debug_)
ROS_INFO("[%s]: Prev. hypothesis count: %d, of which verified: %d", ros::this_node::getName().c_str(), (int)(hypothesis_buffer_.at(index).hypotheses.size()), verified_count);
profiler_main_.indent();
auto mutations = mutateHypotheses(hypothesis_buffer_.at(index).hypotheses, std::min(std::max(0,MAX_HYPOTHESIS_COUNT - (int)(hypothesis_buffer_.at(index).hypotheses.size())),verified_count), now_time);
/* auto mutations = mutateHypotheses(hypothesis_buffer_.at(index).hypotheses, MUTATION_COUNT); */
ROS_INFO("[%s]: Made: %d mutations", ros::this_node::getName().c_str(), (int)(mutations.size()));
if (_debug_)
ROS_INFO("[%s]: Made: %d mutations", ros::this_node::getName().c_str(), (int)(mutations.size()));
profiler_main_.unindent();
hypothesis_buffer_.at(index).hypotheses.insert(hypothesis_buffer_.at(index).hypotheses.end(), mutations.begin(), mutations.end());
ROS_INFO("[%s]: Adding mutations to set. Curr hypothesis count: %d", ros::this_node::getName().c_str(), (int)(hypothesis_buffer_.at(index).hypotheses.size()));
if (_debug_)
ROS_INFO("[%s]: Adding mutations to set. Curr hypothesis count: %d", ros::this_node::getName().c_str(), (int)(hypothesis_buffer_.at(index).hypotheses.size()));
}

/* ROS_INFO("[UVDARPoseCalculator]:PF: B"); */
Expand Down Expand Up @@ -1255,6 +1262,7 @@ namespace uvdar {
{
std::scoped_lock lock(hypothesis_mutex);

if (_debug_)
ROS_INFO("[%s]: C:%d, I:%d Refining. Prev. hypothesis count: %d", ros::this_node::getName().c_str(), image_index, index, (int)(hypothesis_buffer_.at(index).hypotheses.size()));

/* auto start = profiler.getTime(); */
Expand All @@ -1281,7 +1289,8 @@ namespace uvdar {
checkHypothesisFitness(hypothesis_buffer_.at(index), image_index, threshold_reproject_unfit, threshold_reproject_verified, separated_points, tocam_tf, latest_local.time);
removeUnfitHypotheses(hypothesis_buffer_.at(index));
profiler_main_.unindent();
ROS_INFO("[%s]: C:%d, I:%d Curr. hypothesis count: %d", ros::this_node::getName().c_str(), image_index, index, (int)(hypothesis_buffer_.at(index).hypotheses.size()));
if (_debug_)
ROS_INFO("[%s]: C:%d, I:%d Curr. hypothesis count: %d", ros::this_node::getName().c_str(), image_index, index, (int)(hypothesis_buffer_.at(index).hypotheses.size()));
/* ROS_INFO("[%s]: C:%d Removing extras, left: %d", ros::this_node::getName().c_str(), image_index, (int)(hypothesis_buffer_.at(index).hypotheses.size())); */
}

Expand Down Expand Up @@ -1417,15 +1426,17 @@ namespace uvdar {
/* ROS_INFO("[%s]: error: %f vs threshold_scaled: %f", ros::this_node::getName().c_str(), error_total, threshold_scaled); */

if (error_total > threshold_scaled_unfit){
ROS_INFO_STREAM("[UVDARPoseCalculator]: setting hypo as unfit, err:" << error_total << " vs " << threshold_scaled_unfit);
if (_debug_)
ROS_INFO_STREAM("[UVDARPoseCalculator]: setting hypo as unfit, err:" << error_total << " vs " << threshold_scaled_unfit);
/* if (h.flag == verified) */
/* hypotheses.verified_count--; */
/* h.flag = unfit; */
hypotheses.setUnfit(i);
/* h.updated = time; */
}
else if (error_total < threshold_scaled_verified) {
ROS_INFO_STREAM("[UVDARPoseCalculator]: setting hypo as verified, err:" << error_total << " vs " << threshold_scaled_verified);
if (_debug_)
ROS_INFO_STREAM("[UVDARPoseCalculator]: setting hypo as verified, err:" << error_total << " vs " << threshold_scaled_verified);
/* if (h.flag != verified) */
/* hypotheses.verified_count++; */
/* h.flag = verified; */
Expand Down Expand Up @@ -2183,14 +2194,17 @@ namespace uvdar {
Hypothesis h_t;
std::tie(h_t, error_t) = iterFitFull(model_, points, h, target, image_index, tocam_tf, profiler);

ROS_INFO_STREAM("[UVDARPoseCalculator]: Fitted position: Pre. error: " << errors_init[i] << ", New error: " << error_t);
if (_debug_)
ROS_INFO_STREAM("[UVDARPoseCalculator]: Fitted position: Pre. error: " << errors_init[i] << ", New error: " << error_t);
const double threshold = (double)(points.size())*ERROR_THRESHOLD_FITTED(image_index);
if (error_t <= threshold){
hypotheses_fitted.push_back(h_t);
ROS_INFO_STREAM("[UVDARPoseCalculator]: Adding");
if (_debug_)
ROS_INFO_STREAM("[UVDARPoseCalculator]: Adding");
}
else {
ROS_INFO_STREAM("[UVDARPoseCalculator]: Rejecting!");
if (_debug_)
ROS_INFO_STREAM("[UVDARPoseCalculator]: Rejecting!");
}

i++;
Expand Down Expand Up @@ -2566,7 +2580,8 @@ namespace uvdar {
}

}
ROS_INFO_STREAM("[UVDARPoseCalculator]: Iterations for initialization : "<< iter );
if (_debug_)
ROS_INFO_STREAM("[UVDARPoseCalculator]: Iterations for initialization : "<< iter );
return {initial_hypotheses, errors};
}

Expand Down

0 comments on commit 7585e34

Please sign in to comment.