void cloudIndicesModelCallback(const PointCloud2::ConstPtr& cloud_msg) {
boost::mutex::scoped_lock lock(callback_mutex); // Lock
PointCloudT::Ptr pointCloud1_BB(new PointCloudT), pointCloud1_A(new PointCloudT);
// Downsample filtering
// float pointsRatio = (float)(cloud_msg->points.size())/2000;
// float leafSize = 0.001 * pointsRatio;
sensor_msgs::PointCloud2::Ptr inputFiltered (new sensor_msgs::PointCloud2 ());
pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
sor.setInputCloud (cloud_msg);
sor.setDownsampleAllData (true);
sor.setLeafSize (0.04f, 0.04f, 0.04f);
sor.filter (*inputFiltered);
// std::cout << "Removed with <VoxelGrid>: " << sor.getRemovedIndices()->size();
// std::cout << "with ratio = " << pointsRatio << " and leafSize = " << leafSize << ", got " << inputFiltered->points.size() << " of " << clud_msg->points.size() << std::endl << std::endl;
// std::stringstream ss;
// ss << "n/a";
ROS_INFO("Got cloud with timestamp %s", "n/a");
pcl::fromROSMsg(*inputFiltered, *pointCloud1_A); // ros PCL2 to pcl PCL
// Create the filtering object
pcl::PassThrough<PointT> pass;
pass.setInputCloud (pointCloud1_A);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.6, 3.2);
pass.filter (*pointCloud1_BB);
std::vector<int> tempIntVec;
pcl::removeNaNFromPointCloud(*pointCloud1_BB, *pointCloud1_A, tempIntVec); // Removing NaN values
// std::cout << "Removed with <removeNaN>: " << tempIntVec.size();
if (pointCloud1_A->points.size() < 1)
return; // Nothing can be done without points
if (previous != 0 && previous->points.size() > 0)
{
Eigen::Matrix4f tf_result = processCloud(pointCloud1_A, previous);
// for (int i=0; i<4; i++)
// for (int j=0; j<4; j++)
// tf_result(i, j) = float(int(tf_result(i, j)*100))/100; // MICHI: Brutal rounding
std::cout << tf_result << std::endl;
GlobalTransform = tf_result * GlobalTransform;
broadcastTransform(GlobalTransform);
// tf::Transform t = ;
// publishOdom(odom_pub, t.getOrigin().x(), t.getOrigin().y(), t.getOrigin().z(),
// t.getRotation().getX(), t.getRotation().getY(), t.getRotation().getZ(), t.getRotation().getW());
}
previous = pointCloud1_A;
}
void updateCallback(const ros::TimerEvent& event)
{
// Determine the motion since the last update
tf::StampedTransform transform;
try
{
ros::Time now = ros::Time::now();
tf_listener_.waitForTransform("base_link", "odom", now, ros::Duration(0.1));
tf_listener_.lookupTransform("base_link", "odom", now, transform);
}
catch (tf::TransformException& e)
{
ROS_ERROR("Unable to compute the motion since the last update...");
return;
}
tf::Transform delta_transform = previous_pose_ * transform.inverse();
previous_pose_ = transform;
// In theory the obtained transform should be imprecise (because it is
// based on the wheel odometry). In our system, however, we in fact get the
// ground truth transform. It is therefore impossible to simulate the
// "kidnapped robot" problem, because even if the robot is teleported in a
// random spot in the world, the transform will be exact, and the particle
// filter will not lose the track.
// We test the length of the transform vector and if it exceeds some "sane"
// limit we assume that the odometry system "failed" and feed identity
// transform to the particle filter.
if (delta_transform.getOrigin().length() > 2.0)
delta_transform.setIdentity();
// Perform particle filter update, note that the transform is in robot's
// coordinate frame
double forward = delta_transform.getOrigin().getX();
double lateral = delta_transform.getOrigin().getY();
double yaw = tf::getYaw(delta_transform.getRotation());
particle_filter_->update(forward, lateral, yaw);
// Visualize the particle set, broadcast transform, and print some information
const auto& p = particle_filter_->getParticles();
double avg_weight = std::accumulate(p.begin(), p.end(), 0.0, [](double sum, Particle p) { return sum + p.weight; }) / p.size();
particle_visualizer_->publish(p);
broadcastTransform();
ROS_INFO("Motion: [%.3f, %.3f, %.3f] Average partile weight: %3f", forward, lateral, yaw, avg_weight);
}
// returns true, iff node could be added to the cloud
bool GraphManager::addNode(Node new_node) {
std::clock_t starttime=std::clock();
if(reset_request_) {
int numLevels = 3;
int nodeDistance = 2;
marker_id =0;
time_of_last_transform_= ros::Time();
last_batch_update_=std::clock();
delete optimizer_;
optimizer_ = new AIS::HCholOptimizer3D(numLevels, nodeDistance);
graph_.clear();
matches_.clear();
freshlyOptimized_= false;
reset_request_ = false;
}
if (new_node.feature_locations_2d_.size() <= 5) {
ROS_DEBUG("found only %i features on image, node is not included",(int)new_node.feature_locations_2d_.size());
return false;
}
//set the node id only if the node is actually added to the graph
//needs to be done here as the graph size can change inside this function
new_node.id_ = graph_.size();
//First Node, so only build its index, insert into storage and add a
//vertex at the origin, of which the position is very certain
if (graph_.size()==0) {
new_node.buildFlannIndex(); // create index so that next nodes can use it
// graph_.insert(make_pair(new_node.id_, new_node)); //store
graph_[new_node.id_] = new_node;
optimizer_->addVertex(0, Transformation3(), 1e9*Matrix6::eye(1.0)); //fix at origin
return true;
}
unsigned int num_edges_before = optimizer_->edges().size();
std::vector<cv::DMatch> initial_matches;
ROS_DEBUG("Graphsize: %d", (int) graph_.size());
marker_id = 0; //overdraw old markers
Eigen::Matrix4f ransac_trafo, final_trafo;
bool edge_added_to_base;
std::vector<int> vertices_to_comp = getPotentialEdgeTargets(new_node, -1); //vernetzungsgrad
int id_of_id = vertices_to_comp.size() -1;
for (; id_of_id >=0; id_of_id--) { //go from the back, so the last comparison is with the first node. The last comparison is visualized.
initial_matches = processNodePair(new_node, graph_[vertices_to_comp[id_of_id]],edge_added_to_base,ransac_trafo, final_trafo);
//initial_matches = processNodePair(new_node, graph_rit->second);
//What if the node has not been added? visualizeFeatureFlow3D(graph_rit->second, new_node, initial_matches, matches_, marker_id++);
}
id_of_id++;//set back to last valid id
if (optimizer_->edges().size() > num_edges_before) {
new_node.buildFlannIndex();
graph_[new_node.id_] = new_node;
ROS_DEBUG("Added Node, new Graphsize: %i", (int) graph_.size());
optimizeGraph();
//make the transform of the last node known
ros::TimerEvent unused;
broadcastTransform(unused);
visualizeGraphEdges();
visualizeGraphNodes();
visualizeFeatureFlow3D(graph_[vertices_to_comp[id_of_id]], new_node, initial_matches, matches_, marker_id++);
} else {
ROS_WARN("##### could not find link for PointCloud!");
matches_.clear();
}
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "function runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec");
return (optimizer_->edges().size() > num_edges_before);
}
