Node::Node(ros::NodeHandle* nh,
const cv::Mat& visual, const cv::Mat& depth,
image_geometry::PinholeCameraModel cam_model,
cv::Ptr<cv::FeatureDetector> detector,
cv::Ptr<cv::DescriptorExtractor> extractor,
cv::Ptr<cv::DescriptorMatcher> matcher,
const sensor_msgs::PointCloud2ConstPtr& point_cloud,
unsigned int msg_id,
unsigned int id,
const cv::Mat& detection_mask):
nh_(nh),
msg_id_(msg_id),
id_(id),
cloudMessage_(*point_cloud),
cam_model_(cam_model),
matcher_(matcher)
{
std::clock_t starttime=std::clock();
ROS_FATAL_COND(detector.empty(), "No valid detector!");
detector->detect( visual, feature_locations_2d_, detection_mask);// fill 2d locations
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "Feature detection runtime: " << ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC );
ROS_INFO("Found %d Keypoints", (int)feature_locations_2d_.size());
cloud_pub = nh_->advertise<sensor_msgs::PointCloud2>("/rgbdslam/batch_clouds",20);
cloud_pub2 = nh_->advertise<sensor_msgs::PointCloud2>("/rgbdslam/my_clouds",20);
// get pcl::Pointcloud to extract depthValues a pixel positions
std::clock_t starttime5=std::clock();
// TODO: This takes 0.1 seconds and is not strictly necessary
//pcl::fromROSMsg(*point_cloud,pc);
pcl::fromROSMsg(*point_cloud,pc_col);
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime5) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "projection runtime: " << ( std::clock() - starttime5 ) / (double)CLOCKS_PER_SEC );
// project pixels to 3dPositions and create search structures for the gicp
projectTo3D(depth, feature_locations_2d_, feature_locations_3d_,pc_col); //takes less than 0.01 sec
std::clock_t starttime4=std::clock();
// projectTo3d need a dense cloud to use the points.at(px.x,px.y)-Call
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime4) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "projection runtime: " << ( std::clock() - starttime4 ) / (double)CLOCKS_PER_SEC );
std::clock_t starttime2=std::clock();
extractor->compute(visual, feature_locations_2d_, feature_descriptors_); //fill feature_descriptors_ with information
assert(feature_locations_2d_.size() == feature_locations_3d_.size());
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime2) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "Feature extraction runtime: " << ( std::clock() - starttime2 ) / (double)CLOCKS_PER_SEC );
flannIndex = NULL;
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "constructor runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec");
}
void GraphManager::sendAllClouds() {
std::clock_t starttime=std::clock();
ROS_WARN("Sending out all clouds");
batch_processing_runs_ = true;
tf::Transform world2cam;
//fill message
rgbdslam::CloudTransforms msg;
for (unsigned int i = 0; i < optimizer_->vertices().size(); ++i) {
AIS::PoseGraph3D::Vertex* v = optimizer_->vertex(i);
if(!v) {
ROS_FATAL("Nullpointer im Graph an Position %i!", i);
continue;
}
tf::Transform transform = hogman2TF(v->transformation);
tf::Transform cam2rgb;
cam2rgb.setRotation(tf::createQuaternionFromRPY(-1.57,0,-1.57));
cam2rgb.setOrigin(tf::Point(0,-0.04,0));
world2cam = cam2rgb*transform;
ros::Time time_of_transform = ros::Time::now();
ROS_WARN("Sending out transform %i", i);
br_.sendTransform(tf::StampedTransform(world2cam, time_of_transform,
"/openni_camera", "/batch_transform"));
ROS_WARN("Sending out cloud %i", i);
graph_[i].publish("/batch_transform", time_of_transform);
graph_[i].publish2("/my_transform", time_of_transform,world2cam);
}
batch_processing_runs_ = false;
emit sendFinished();
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");
}
void Node::computeInliersAndError(const std::vector<cv::DMatch>& matches,
const Eigen::Matrix4f& transformation,
const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& origins,
const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& earlier,
std::vector<cv::DMatch>& inliers, //output var
double& mean_error,
vector<double>& errors,
double squaredMaxInlierDistInM) const{ //output var
std::clock_t starttime=std::clock();
inliers.clear();
errors.clear();
vector<pair<float,int> > dists;
std::vector<cv::DMatch> inliers_temp;
assert(matches.size() > 0);
mean_error = 0.0;
for (unsigned int j = 0; j < matches.size(); j++){ //compute new error and inliers
unsigned int this_id = matches[j].queryIdx;
unsigned int earlier_id = matches[j].trainIdx;
Eigen::Vector4f vec = (transformation * origins[this_id]) - earlier[earlier_id];
double error = vec.dot(vec);
if(error > squaredMaxInlierDistInM)
continue; //ignore outliers
if(!(error >= 0.0)){
ROS_ERROR_STREAM("Transformation for error !> 0: " << transformation);
ROS_ERROR_STREAM(error << " " << matches.size());
}
error = sqrt(error);
dists.push_back(pair<float,int>(error,j));
inliers_temp.push_back(matches[j]); //include inlier
mean_error += error;
errors.push_back(error);
}
if (inliers_temp.size()<3){ //at least the samples should be inliers
ROS_WARN_COND(inliers_temp.size() > 3, "No inliers at all in %d matches!", matches.size()); // only warn if this checks for all initial matches
mean_error = 1e9;
} else {
mean_error /= inliers_temp.size();
// sort inlier ascending according to their error
sort(dists.begin(),dists.end());
inliers.resize(inliers_temp.size());
for (unsigned int i=0; i<inliers_temp.size(); i++){
inliers[i] = matches[dists[i].second];
}
}
if(!(mean_error>0)) ROS_ERROR_STREAM("Transformation for mean error !> 0: " << transformation);
if(!(mean_error>0)) ROS_ERROR_STREAM(mean_error << " " << inliers_temp.size());
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", __FUNCTION__ << " runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec");
}
bool GraphManager::addEdgeToHogman(AIS::LoadedEdge3D edge, bool largeEdge) {
std::clock_t starttime=std::clock();
freshlyOptimized_ = false;
AIS::PoseGraph3D::Vertex* v1 = optimizer_->vertex(edge.id1);
AIS::PoseGraph3D::Vertex* v2 = optimizer_->vertex(edge.id2);
// at least one vertex has to be created, assert that the transformation
// is large enough to avoid to many vertices on the same spot
if (!v1 || !v2) {
if (!largeEdge) {
ROS_INFO("edge to new vertex is to short, vertex will not be inserted");
//return false;
}
}
if (!v1) {
v1 = optimizer_->addVertex(edge.id1, Transformation3(), Matrix6::eye(1.0));
assert(v1);
}
if (!v2) {
v2 = optimizer_->addVertex(edge.id2, Transformation3(), Matrix6::eye(1.0));
assert(v2);
}
optimizer_->addEdge(v1, v2, edge.mean, edge.informationMatrix);
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 true;
}
std::vector<cv::DMatch> GraphManager::processNodePair(Node& new_node, Node& old_node, bool& edge_added,Eigen::Matrix4f& ransac_transform, Eigen::Matrix4f& final_trafo) {
std::clock_t starttime=std::clock();
const unsigned int hit_threshold = 50; // minimal number of hits to be a valid candidate for a link
std::vector<cv::DMatch> initial_matches;
Eigen::Matrix4f icp_trafo;
Eigen::Matrix4f icp_trafo2;
AIS::LoadedEdge3D edge;
float rmse;
new_node.findPairsFlann(old_node, &initial_matches);
ROS_DEBUG("found %i inital matches",(int) initial_matches.size());
if (initial_matches.size() >= hit_threshold) {
bool valid_trafo = new_node.getRelativeTransformationTo(old_node,&initial_matches, ransac_transform, rmse, matches_);
if (!valid_trafo) {
ROS_INFO("GraphManager: Found no valid trafo, but had initially %d hits",(int) initial_matches.size());
edge_added = false;
}
else {
// improve transformation by using the generalized ICP
// std::clock_t starttime_gicp = std::clock();
// new_node.getRelativeTransformationTo_ICP(old_node,icp_trafo, &ransac_transform);
// cout << "complete time for ICP: " << ((std::clock()-starttime_gicp*1.0) / (double)CLOCKS_PER_SEC) << endl;
// cout << "old icp " << icp_trafo << endl;
// starttime_gicp = std::clock();
//new_node.getRelativeTransformationTo_ICP2(old_node,icp_trafo2, &ransac_transform);
// cout << "complete time for ICP2: " << ((std::clock()-starttime_gicp*1.0) / (double)CLOCKS_PER_SEC) << endl;
//cout << "new icp " << icp_trafo2 << endl;
// ROS_INFO_STREAM("zzz icp trafo: " << endl << icp_trafo << endl);
// ROS_INFO_STREAM("zzz icp trafo2: " << endl << icp_trafo2 << endl);
final_trafo = ransac_transform ; //*icp_trafo2;
edge.id1 = old_node.id_;//and we have a valid transformation
edge.id2 = new_node.id_; //since there are enough matching features,
edge.mean = eigen2Hogman(final_trafo);//we insert an edge between the frames
edge.informationMatrix = Matrix6::eye(1.0); //TODO: What do we do about the information matrix? Scale with rmse? inlier_count)
edge_added = addEdgeToHogman(edge, isBigTrafo(final_trafo));
if (edge_added) {
ROS_DEBUG("Adding Edge between %i and %i. Inlier: %i, error: %f",edge.id1,edge.id2,(int) matches_.size(),rmse);
}
}
}
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 initial_matches;
}
// build search structure for descriptor matching
void Node::buildFlannIndex() {
std::clock_t starttime=std::clock();
// use same type as in http://opencv-cocoa.googlecode.com/svn/trunk/samples/c/find_obj.cpp
flannIndex = new cv_flannIndex(feature_descriptors_,
cv::flann::KDTreeIndexParams(4));
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");
}
void Node::computeInliersAndError(const std::vector<cv::DMatch>& matches,
const Eigen::Matrix4f& transformation,
const std::vector<Eigen::Vector4f>& origins,
const std::vector<Eigen::Vector4f>& earlier,
std::vector<cv::DMatch>& inliers, //output var
double& mean_error,vector<double>& errors,
double squaredMaxInlierDistInM) const{ //output var
std::clock_t starttime=std::clock();
inliers.clear();
vector<pair<float,int> > dists;
std::vector<cv::DMatch> inliers_temp;
mean_error = 0.0;
for (unsigned int j = 0; j < matches.size(); j++){ //compute new error and inliers
unsigned int this_id = matches[j].queryIdx;
unsigned int earlier_id = matches[j].trainIdx;
Eigen::Vector4f vec = (transformation * origins[this_id]) - earlier[earlier_id];
double error = vec.dot(vec);
if(error > squaredMaxInlierDistInM)
continue; //ignore outliers
error = sqrt(error);
dists.push_back(pair<float,int>(error,j));
inliers_temp.push_back(matches[j]); //include inlier
mean_error += error;
errors.push_back(error);
}
if (inliers_temp.size()==0){
mean_error = -1;
inliers = inliers_temp;
}
else
{
mean_error /= inliers_temp.size();
// sort inlier ascending according to their error
sort(dists.begin(),dists.end());
inliers.resize(inliers_temp.size());
for (unsigned int i=0; i<inliers_temp.size(); i++){
inliers[i] = matches[dists[i].second];
}
}
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");
}
void Node::projectTo3DSiftGPU(std::vector<cv::KeyPoint>& feature_locations_2d,
std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& feature_locations_3d,
const pointcloud_type& point_cloud, float* descriptors_in, cv::Mat& descriptors_out){
std::clock_t starttime=std::clock();
cv::Point2f p2d;
if(feature_locations_3d.size()){
ROS_INFO("There is already 3D Information in the FrameInfo, clearing it");
feature_locations_3d.clear();
}
std::list<int> featuresUsed;
int index = -1;
for(unsigned int i = 0; i < feature_locations_2d.size(); /*increment at end of loop*/){
++index;
p2d = feature_locations_2d[i].pt;
if (p2d.x >= point_cloud.width || p2d.x < 0 ||
p2d.y >= point_cloud.height || p2d.y < 0 ||
std::isnan(p2d.x) || std::isnan(p2d.y)){ //TODO: Unclear why points should be outside the image or be NaN
ROS_WARN_STREAM("Ignoring invalid keypoint: " << p2d); //Does it happen at all? If not, remove this code block
feature_locations_2d.erase(feature_locations_2d.begin()+i);
continue;
}
point_type p3d = point_cloud.at((int) p2d.x,(int) p2d.y);
if ( isnan(p3d.x) || isnan(p3d.y) || isnan(p3d.z)){
feature_locations_2d.erase(feature_locations_2d.begin()+i);
continue;
}
featuresUsed.push_back(index); //save id for constructing the descriptor matrix
feature_locations_3d.push_back(Eigen::Vector4f(p3d.x, p3d.y, p3d.z, 1.0));
i++; //Only increment if no element is removed from vector
}
//create descriptor matrix
int size = feature_locations_3d.size();
descriptors_out = cv::Mat(size, 128, CV_32F);
for (int y = 0; y < size && featuresUsed.size() > 0; ++y) {
int id = featuresUsed.front();
featuresUsed.pop_front();
for (int x = 0; x < 128; ++x) {
descriptors_out.at<float>(y, x) = descriptors_in[id * 128 + x];
}
}
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", __FUNCTION__ << " runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec");
}
void GraphManager::visualizeGraphEdges() const {
std::clock_t starttime=std::clock();
if (marker_pub_.getNumSubscribers() > 0) { //no visualization for nobody
visualization_msgs::Marker edges_marker;
edges_marker.header.frame_id = "/openni_rgb_optical_frame"; //TODO: Should be a meaningfull fixed frame with known relative pose to the camera
edges_marker.header.stamp = ros::Time::now();
edges_marker.ns = "camera_pose_graph"; // Set the namespace and id for this marker. This serves to create a unique ID
edges_marker.id = 0; // Any marker sent with the same namespace and id will overwrite the old one
edges_marker.type = visualization_msgs::Marker::LINE_LIST;
edges_marker.action = visualization_msgs::Marker::ADD; // Set the marker action. Options are ADD and DELETE
edges_marker.frame_locked = true; //rviz automatically retransforms the markers into the frame every update cycle
// Set the scale of the marker -- 1x1x1 here means 1m on a side
edges_marker.scale.x = 0.005; //line width
//Global pose (used to transform all points)
edges_marker.pose.position.x = 0;
edges_marker.pose.position.y = 0;
edges_marker.pose.position.z = 0;
edges_marker.pose.orientation.x = 0.0;
edges_marker.pose.orientation.y = 0.0;
edges_marker.pose.orientation.z = 0.0;
edges_marker.pose.orientation.w = 1.0;
// Set the color -- be sure to set alpha to something non-zero!
edges_marker.color.r = 1.0f;
edges_marker.color.g = 1.0f;
edges_marker.color.b = 1.0f;
edges_marker.color.a = 0.5f;//looks smoother
geometry_msgs::Point point; //start and endpoint for each line segment
AISNavigation::PoseGraph3D::Vertex* v; //used in loop
AISNavigation::PoseGraph3D::EdgeSet::iterator edge_iter = optimizer_->edges().begin();
int counter = 0;
for(; edge_iter != optimizer_->edges().end(); edge_iter++, counter++) {
v = static_cast<AISNavigation::PoseGraph3D::Vertex*>((*edge_iter)->from());
point.x = v->transformation.translation().x();
point.y = v->transformation.translation().y();
point.z = v->transformation.translation().z();
edges_marker.points.push_back(point);
v = static_cast<AISNavigation::PoseGraph3D::Vertex*>((*edge_iter)->to());
point.x = v->transformation.translation().x();
point.y = v->transformation.translation().y();
point.z = v->transformation.translation().z();
edges_marker.points.push_back(point);
}
marker_pub_.publish (edges_marker);
ROS_INFO("published %d graph edges", counter);
}
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");
}
Transformation3 eigen2Hogman(const Eigen::Matrix4f eigen_mat) {
std::clock_t starttime=std::clock();
Eigen::Affine3f eigen_transform(eigen_mat);
Eigen::Quaternionf eigen_quat(eigen_transform.rotation());
Vector3 translation(eigen_mat(0, 3), eigen_mat(1, 3), eigen_mat(2, 3));
Quaternion rotation(eigen_quat.x(), eigen_quat.y(), eigen_quat.z(),
eigen_quat.w());
Transformation3 result(translation, rotation);
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", __FUNCTION__ << " runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec");
return result;
}
/** Assumptions: No 3d Info available yet */
void Node::projectTo3D(const cv::Mat& depth,
std::vector<cv::KeyPoint>& feature_locations_2d,
std::vector<Eigen::Vector4f>& feature_locations_3d,
const pcl::PointCloud<pcl::PointXYZRGB>& point_cloud){ //TODO: Use this instead of member "pc" or remove argument
std::clock_t starttime=std::clock();
cv::Point2f p2d;
if(feature_locations_3d.size()){
ROS_INFO("There is already 3D Information in the FrameInfo, clearing it");
feature_locations_3d.clear();
}
for(unsigned int i = 0; i < feature_locations_2d.size(); /*increment at end of loop*/){
p2d = feature_locations_2d[i].pt;
if (p2d.x >= depth.cols || p2d.x < 0 || p2d.y >= depth.rows || p2d.y < 0 ||
std::isnan(p2d.x) || std::isnan(p2d.y)){
ROS_WARN_STREAM("Ignoring invalid keypoint: " << p2d); //Does it happen at all? If not, remove this code block
feature_locations_2d.erase(feature_locations_2d.begin()+i);
continue;
}
float depth_val = depth.at<float>((int)p2d.y, (int)p2d.x);
if(std::isnan(depth_val) ) {//No 3d pose would be available
ROS_DEBUG_STREAM("No depth for: " << p2d);
feature_locations_2d.erase(feature_locations_2d.begin()+i);
continue;
}
pcl::PointXYZRGB p3d = point_cloud.at((int) p2d.x,(int) p2d.y);
// Todo: should not happen
if ( isnan(p3d.x) || isnan(p3d.y) || isnan(p3d.z)){
// ROS_INFO("nan in pointcloud! %f, %f",p2d.x, p2d.y);
feature_locations_2d.erase(feature_locations_2d.begin()+i);
continue;
}
feature_locations_3d.push_back(Eigen::Vector4f(p3d.x, p3d.y, p3d.z, 1.0));
i++; //Only increment if no element is removed from vector
}
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");
}
void Node::createGICPStructures(unsigned int max_count){
gicp_point_set = new dgc::gicp::GICPPointSet();
dgc::gicp::GICPPoint g_p;
g_p.range = -1;
for(int k = 0; k < 3; k++) {
for(int l = 0; l < 3; l++) {
g_p.C[k][l] = (k == l)?1:0;
}
}
int step = 1;
if (pc_col.points.size()>max_count)
step = ceil(pc_col.points.size()*1.0/max_count);
int cnt = 0;
for (unsigned int i=0; i<pc_col.points.size(); i++ ){
point_type p = pc_col.points.at(i);
if (!(isnan(p.x) || isnan(p.y) || isnan(p.z))) {
// add points to pointset for icp
if (cnt++%step == 0){
g_p.x=p.x;
g_p.y=p.y;
g_p.z=p.z;
gicp_point_set->AppendPoint(g_p);
}
}
}
ROS_WARN("gicp_point_set.Size() %i", gicp_point_set->Size() );
std::clock_t starttime_gicp = std::clock();
// build search structure for gicp:
gicp_point_set->SetDebug(false);
gicp_point_set->SetGICPEpsilon(gicp_epsilon);
gicp_point_set->BuildKDTree();
gicp_point_set->ComputeMatrices();
gicp_point_set->SetMaxIterationInner(8); // as in test_gicp->cpp
gicp_point_set->SetMaxIteration(gicp_max_iterations);
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime_gicp) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", __FUNCTION__ << " runtime to create gicp-Structures: "<< ( std::clock() - starttime_gicp ) / (double)CLOCKS_PER_SEC <<"sec");
ROS_INFO_STREAM("time for creating the structure: " << ((std::clock()-starttime_gicp*1.0) / (double)CLOCKS_PER_SEC));
ROS_INFO_STREAM("current: " << std::clock() << " " << "start_time: " << starttime_gicp);
gicp_initialized = true;
}
//TODO: This function seems to be resistant to paralellization probably due to knnSearch
int Node::findPairsFlann(const Node* other, vector<cv::DMatch>* matches) const {
std::clock_t starttime=std::clock();
assert(matches->size()==0);
if (other->flannIndex == NULL) {
ROS_FATAL("Node %i in findPairsFlann: flann Index of Node %i was not initialized", this->id_, other->id_);
return -1;
}
// number of neighbours found (has to be two, see l. 57)
const int k = 2;
// compare
// http://opencv-cocoa.googlecode.com/svn/trunk/samples/c/find_obj.cpp
cv::Mat indices(feature_descriptors_.rows, k, CV_32S);
cv::Mat dists(feature_descriptors_.rows, k, CV_32F);
//ROS_INFO("find flann pairs: feature_descriptor (rows): %i", feature_descriptors_.rows);
// get the best two neighbours
other->flannIndex->knnSearch(feature_descriptors_, indices, dists, k,
cv::flann::SearchParams(64));
int* indices_ptr = indices.ptr<int> (0);
float* dists_ptr = dists.ptr<float> (0);
cv::DMatch match;
for (int i = 0; i < indices.rows; ++i) {
if (dists_ptr[2 * i] < 0.6 * dists_ptr[2 * i + 1]) {
match.queryIdx = i;
match.trainIdx = indices_ptr[2 * i];
match.distance = dists_ptr[2 * i];
assert(match.trainIdx < other->feature_descriptors_.rows);
assert(match.queryIdx < feature_descriptors_.rows);
matches->push_back(match);
}
}
//ROS_INFO("matches size: %i, rows: %i", (int) matches->size(), feature_descriptors_.rows);
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", "findPairsFlann runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec");
return matches->size();
}
int Node::findPairsFlann(Node& other, vector<cv::DMatch>* matches) {
std::clock_t starttime=std::clock();
assert(matches->size()==0);
if (other.flannIndex == NULL) {
ROS_FATAL("Node::findpairs: flann Index was not initialized");
return -1;
}
// number of neighbours found (has to be two, see l. 57)
int k = 2;
// compare
// http://opencv-cocoa.googlecode.com/svn/trunk/samples/c/find_obj.cpp
cv::Mat indices(feature_descriptors_.rows, k, CV_32S);
cv::Mat dists(feature_descriptors_.rows, k, CV_32F);
// get the best two neighbours
other.flannIndex->knnSearch(feature_descriptors_, indices, dists, k,
cv::flann::SearchParams(64));
int* indices_ptr = indices.ptr<int> (0);
float* dists_ptr = dists.ptr<float> (0);
cv::DMatch match;
for (int i = 0; i < indices.rows; ++i) {
if (dists_ptr[2 * i] < 0.6 * dists_ptr[2 * i + 1]) {
// && abs(feature_locations_2d_[i].pt.y-other.feature_locations_2d_[indices_ptr[2 * i]].pt.y)<10000000) {
// std::cerr<<feature_locations_2d_[i].pt.y<<","<<other.feature_locations_2d_[indices_ptr[2 * i]].pt.y <<endl;
match.queryIdx = i;
match.trainIdx = indices_ptr[2 * i];
match.distance = dists_ptr[2 * i];
matches->push_back(match);
}
}
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 matches->size();
}
/**
* Publish the updated transforms for the graph node resp. clouds
*/
void GraphManager::publishCorrectedTransforms() {
std::clock_t starttime=std::clock();
//fill message
rgbdslam::CloudTransforms msg;
for (unsigned int i = 0; i < optimizer_->vertices().size(); ++i) {
AIS::PoseGraph3D::Vertex* v = optimizer_->vertex(i);
tf::Transform trans = hogman2TF(v->transformation);
geometry_msgs::Transform trans_msg;
tf::transformTFToMsg(trans,trans_msg);
msg.transforms.push_back(trans_msg);
msg.ids.push_back(graph_[i].msg_id_);
}
msg.header.stamp = ros::Time::now();
if (transform_pub_.getNumSubscribers() > 0)
transform_pub_.publish(msg);
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");
}
void GraphManager::drawFeatureFlow(cv::Mat& canvas,
cv::Scalar line_color,
cv::Scalar circle_color) {
std::clock_t starttime=std::clock();
ROS_DEBUG("Number of features to draw: %d", (int)matches_.size());
const double pi_fourth = 3.14159265358979323846 / 4.0;
const int line_thickness = 1;
const int circle_radius = 6;
if(graph_.size() < 2) return; //feature flow is only available between at least two nodes
Node& earliernode = graph_[0];//graph_.size()-2; //compare current to previous
Node& newernode = graph_[graph_.size()-1];
for(unsigned int mtch = 0; mtch < matches_.size(); mtch++) {
cv::Point2f p,q; //TODO: Use sub-pixel-accuracy
unsigned int newer_idx = matches_[mtch].queryIdx;
unsigned int earlier_idx = matches_[mtch].trainIdx;
q = newernode.feature_locations_2d_[newer_idx].pt;
p = earliernode.feature_locations_2d_[earlier_idx].pt;
double angle;
angle = atan2( (double) p.y - q.y, (double) p.x - q.x );
double hypotenuse;
hypotenuse = cv::norm(p-q);
if(hypotenuse < 1.5) { //encircle near-stationary points
cv::line(canvas, p, q, line_color, line_thickness, 8);
cv::circle(canvas, p, circle_radius, circle_color, line_thickness, 8);
} else { /* Connect the points */
cv::line( canvas, p, q, line_color, line_thickness, 8 );
/* Now draw the tips of the arrow. */
p.x = (q.x + 4 * cos(angle + pi_fourth));
p.y = (q.y + 4 * sin(angle + pi_fourth));
cv::line( canvas, p, q, line_color, line_thickness, 8 );
p.x = (q.x + 4 * cos(angle - pi_fourth));
p.y = (q.y + 4 * sin(angle - pi_fourth));
cv::line( canvas, p, q, line_color, line_thickness, 8 );
}
}
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");
}
void Node::projectTo3D(std::vector<cv::KeyPoint>& feature_locations_2d,
std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& feature_locations_3d,
const pointcloud_type& point_cloud){
std::clock_t starttime=std::clock();
cv::Point2f p2d;
if(feature_locations_3d.size()){
ROS_INFO("There is already 3D Information in the FrameInfo, clearing it");
feature_locations_3d.clear();
}
for(unsigned int i = 0; i < feature_locations_2d.size(); /*increment at end of loop*/){
p2d = feature_locations_2d[i].pt;
if (p2d.x >= point_cloud.width || p2d.x < 0 ||
p2d.y >= point_cloud.height || p2d.y < 0 ||
std::isnan(p2d.x) || std::isnan(p2d.y)){ //TODO: Unclear why points should be outside the image or be NaN
ROS_WARN_STREAM("Ignoring invalid keypoint: " << p2d); //Does it happen at all? If not, remove this code block
feature_locations_2d.erase(feature_locations_2d.begin()+i);
continue;
}
point_type p3d = point_cloud.at((int) p2d.x,(int) p2d.y);
//ROS_INFO("3d: %f, %f, %f, 2d: %f, %f", p3d.x, p3d.y, p3d.z, p2d.x, p2d.y);
if ( isnan(p3d.x) || isnan(p3d.y) || isnan(p3d.z)){
feature_locations_2d.erase(feature_locations_2d.begin()+i);
continue;
}
feature_locations_3d.push_back(Eigen::Vector4f(p3d.x, p3d.y, p3d.z, 1.0));
i++; //Only increment if no element is removed from vector
}
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", __FUNCTION__ << " runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec");
}
tf::Transform hogman2TF(const Transformation3 hogman_trans) {
std::clock_t starttime=std::clock();
tf::Transform result;
tf::Vector3 translation;
translation.setX(hogman_trans.translation().x());
translation.setY(hogman_trans.translation().y());
translation.setZ(hogman_trans.translation().z());
tf::Quaternion rotation;
rotation.setX(hogman_trans.rotation().x());
rotation.setY(hogman_trans.rotation().y());
rotation.setZ(hogman_trans.rotation().z());
rotation.setW(hogman_trans.rotation().w());
result.setOrigin(translation);
result.setRotation(rotation);
return result;
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");
}
GraphManager::GraphManager(ros::NodeHandle nh) :
freshlyOptimized_(false),
nh_(nh), time_of_last_transform_(ros::Time()),
optimizer_(0), reset_request_(false),
last_batch_update_(std::clock()),
marker_id(0),
batch_processing_runs_(false)
{
std::clock_t starttime=std::clock();
int numLevels = 3;
int nodeDistance = 2;
optimizer_ = new AIS::HCholOptimizer3D(numLevels, nodeDistance);
marker_pub_ = nh.advertise<visualization_msgs::Marker>("camera_pose_graph", 1);
kinect_transform_.setRotation(tf::Quaternion::getIdentity());
timer_ = nh.createTimer(ros::Duration(0.1), &GraphManager::broadcastTransform, this);
transform_pub_ = nh.advertise<rgbdslam::CloudTransforms>("/rgbdslam/transforms", 1);
ransac_marker_pub_ = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10);
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");
}
void GraphManager::optimizeGraph() {
std::clock_t starttime=std::clock();
const int iterations = 10;
int currentIt = optimizer_->optimize(iterations, true);
double chi2 = optimizer_->chi2();
ROS_INFO_STREAM("nodes= " << optimizer_->vertices().size() << "\t edges= "
<< optimizer_->edges().size() << "\t chi2= " << chi2
<< "\t iterations= " << currentIt);
freshlyOptimized_ = true;
AISNavigation::PoseGraph3D::Vertex* v = optimizer_->vertex(optimizer_->vertices().size()-1);
kinect_transform_ = hogman2TF(v->transformation);
//publish the corrected transforms to the visualization module every five seconds
if( ((std::clock()-last_batch_update_) / (double)CLOCKS_PER_SEC) > 2) {
publishCorrectedTransforms();
last_batch_update_ = std::clock();
}
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");
}
// 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);
}
///Find transformation with largest support, RANSAC style.
///Return false if no transformation can be found
bool Node::getRelativeTransformationTo(const Node& earlier_node,
std::vector<cv::DMatch>* initial_matches,
Eigen::Matrix4f& resulting_transformation,
float& rmse,
std::vector<cv::DMatch>& matches, //for visualization?
unsigned int min_inlier_threshold,
unsigned int ransac_iterations) const{
std::clock_t starttime=std::clock();
assert(initial_matches != NULL);
matches.clear();
std::vector<cv::DMatch> inlier; //holds those feature correspondences that support the transformation
if(initial_matches->size() <= min_inlier_threshold){
ROS_DEBUG("Only %d feature matches between frames %d and %d",(int)initial_matches->size() , this->id_, earlier_node.id_);
return false;
}
double inlier_error; //all squared errors
srand((long)std::clock());
pcl::TransformationFromCorrespondences tfc;
// a point is an inlier if it's no more than max_dist_m m from its partner apart
float max_dist_m = 0.02;
unsigned int n_iter = 0;
vector<double> errors;
vector<double> temp_errorsA;
double best_error = 1e6;
int valid_iterations = 0;
unsigned int good_inlier_cnt = min(20,int(initial_matches->size()*0.2));
for (; n_iter < ransac_iterations; n_iter++) {
tfc.reset();
// chose randomly from the correspondences:
int sample_size = 3;
// TODO: assert that no point is drawn twice
for (int k = 0; k < sample_size; k++) {
int id = rand() % initial_matches->size();
int this_id = initial_matches->at(id).queryIdx;
int earlier_id = initial_matches->at(id).trainIdx;
Eigen::Vector3f from(this->feature_locations_3d_[this_id][0],
this->feature_locations_3d_[this_id][1],
this->feature_locations_3d_[this_id][2]);
Eigen::Vector3f to (earlier_node.feature_locations_3d_[earlier_id][0],
earlier_node.feature_locations_3d_[earlier_id][1],
earlier_node.feature_locations_3d_[earlier_id][2]);
tfc.add(from, to);
}
// get relative movement from samples
Eigen::Matrix4f transformation = tfc.getTransformation().matrix();
computeInliersAndError(*initial_matches, transformation,
this->feature_locations_3d_,
earlier_node.feature_locations_3d_,
inlier, inlier_error, temp_errorsA, max_dist_m*max_dist_m);
if (inlier.size() < good_inlier_cnt || inlier_error > max_dist_m)
continue;
///Iterations with more than half of the initial_matches inlying, count twice
if (inlier.size() > initial_matches->size()*0.5)
n_iter++;
///Iterations with more than 80% of the initial_matches inlying, count threefold
if (inlier.size() > initial_matches->size()*0.8)
n_iter++;
valid_iterations++;
assert(inlier_error>0);
if (inlier_error < best_error )
{
resulting_transformation = transformation;
matches = inlier;
assert(matches.size()>= good_inlier_cnt);
rmse = inlier_error;
errors = temp_errorsA;
best_error = inlier_error;
}
int max_ndx = min((int) good_inlier_cnt,30);
double new_inlier_error;
// compute new trafo from inliers:
for (int k = 0; k < sample_size; k++) {
int id = rand() % max_ndx;
int this_id = inlier[id].queryIdx;
int earlier_id = inlier[id].trainIdx;
Eigen::Vector3f from(this->feature_locations_3d_[this_id][0],
this->feature_locations_3d_[this_id][1],
this->feature_locations_3d_[this_id][2]);
Eigen::Vector3f to (earlier_node.feature_locations_3d_[earlier_id][0],
earlier_node.feature_locations_3d_[earlier_id][1],
earlier_node.feature_locations_3d_[earlier_id][2]);
tfc.add(from, to);
}
// get relative movement from samples
transformation = tfc.getTransformation().matrix();
computeInliersAndError(*initial_matches, transformation,
this->feature_locations_3d_,
earlier_node.feature_locations_3d_,
inlier, new_inlier_error, temp_errorsA, max_dist_m*max_dist_m);
if (inlier.size() < good_inlier_cnt || new_inlier_error > max_dist_m)
continue;
assert(new_inlier_error>0);
if (new_inlier_error < best_error )
{
resulting_transformation = transformation;
matches = inlier;
assert(matches.size()>= good_inlier_cnt);
rmse = new_inlier_error;
errors = temp_errorsA;
best_error = new_inlier_error;
}
} //iterations
ROS_DEBUG("asd %i good iterations, inlier percentage %i, inlier cnt: %i ",valid_iterations, (int) (matches.size()*1.0/initial_matches->size()*100),(int) matches.size());
ROS_DEBUG("asd time: %.2f, error: %.3f cm",( std::clock() - starttime ) / (double)CLOCKS_PER_SEC, rmse*100 );
for (unsigned int i=0; i < errors.size(); i++){
// ROS_INFO("asd error(%i) = %.4f cm",i,errors.at(i)*100);
}
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 (matches.size() > 0);
}
void GraphManager::visualizeFeatureFlow3D(const Node& earlier_node,
const Node& newer_node,
const std::vector<cv::DMatch>& all_matches,
const std::vector<cv::DMatch>& inlier_matches,
unsigned int marker_id,
bool draw_outlier) const {
std::clock_t starttime=std::clock();
if (ransac_marker_pub_.getNumSubscribers() > 0) { //don't visualize, if nobody's looking
visualization_msgs::Marker marker_lines;
marker_lines.header.frame_id = "/openni_rgb_optical_frame";
marker_lines.ns = "ransac_markers";
marker_lines.header.stamp = ros::Time::now();
marker_lines.action = visualization_msgs::Marker::ADD;
marker_lines.pose.orientation.w = 1.0;
marker_lines.id = marker_id;
marker_lines.type = visualization_msgs::Marker::LINE_LIST;
marker_lines.scale.x = 0.002;
std_msgs::ColorRGBA color_red ; //red outlier
color_red.r = 1.0;
color_red.a = 1.0;
std_msgs::ColorRGBA color_green; //green inlier, newer endpoint
color_green.g = 1.0;
color_green.a = 1.0;
std_msgs::ColorRGBA color_yellow; //yellow inlier, earlier endpoint
color_yellow.r = 1.0;
color_yellow.g = 1.0;
color_yellow.a = 1.0;
std_msgs::ColorRGBA color_blue ; //red-blue outlier
color_blue.b = 1.0;
color_blue.a = 1.0;
marker_lines.color = color_green; //just to set the alpha channel to non-zero
AISNavigation::PoseGraph3D::Vertex* earlier_v; //used to get the transform
AISNavigation::PoseGraph3D::Vertex* newer_v; //used to get the transform
AISNavigation::PoseGraph3D::VertexIDMap v_idmap = optimizer_->vertices();
// end of initialization
ROS_DEBUG("Matches Visualization start");
// write all inital matches to the line_list
marker_lines.points.clear();//necessary?
if (draw_outlier)
{
for (unsigned int i=0; i<all_matches.size(); i++) {
int newer_id = all_matches.at(i).queryIdx; //feature id in newer node
int earlier_id = all_matches.at(i).trainIdx; //feature id in earlier node
earlier_v = static_cast<AISNavigation::PoseGraph3D::Vertex*>(v_idmap[earlier_node.id_]);
newer_v = static_cast<AISNavigation::PoseGraph3D::Vertex*>(v_idmap[newer_node.id_]);
//Outliers are red (newer) to blue (older)
marker_lines.colors.push_back(color_red);
marker_lines.colors.push_back(color_blue);
marker_lines.points.push_back(
pointInWorldFrame(newer_node.feature_locations_3d_[newer_id],
newer_v->transformation));
marker_lines.points.push_back(
pointInWorldFrame(earlier_node.feature_locations_3d_[earlier_id],
earlier_v->transformation));
}
}
for (unsigned int i=0; i<inlier_matches.size(); i++) {
int newer_id = inlier_matches.at(i).queryIdx; //feature id in newer node
int earlier_id = inlier_matches.at(i).trainIdx; //feature id in earlier node
earlier_v = static_cast<AISNavigation::PoseGraph3D::Vertex*>(v_idmap[earlier_node.id_]);
newer_v = static_cast<AISNavigation::PoseGraph3D::Vertex*>(v_idmap[newer_node.id_]);
//inliers are green (newer) to blue (older)
marker_lines.colors.push_back(color_green);
marker_lines.colors.push_back(color_blue);
marker_lines.points.push_back(
pointInWorldFrame(newer_node.feature_locations_3d_[newer_id],
newer_v->transformation));
marker_lines.points.push_back(
pointInWorldFrame(earlier_node.feature_locations_3d_[earlier_id],
earlier_v->transformation));
}
ransac_marker_pub_.publish(marker_lines);
ROS_DEBUG_STREAM("Published " << marker_lines.points.size()/2 << " lines");
}
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");
}
void GraphManager::visualizeGraphNodes() const {
std::clock_t starttime=std::clock();
if (marker_pub_.getNumSubscribers() > 0) { //don't visualize, if nobody's looking
visualization_msgs::Marker nodes_marker;
nodes_marker.header.frame_id = "/openni_rgb_optical_frame"; //TODO: Should be a meaningfull fixed frame with known relative pose to the camera
nodes_marker.header.stamp = ros::Time::now();
nodes_marker.ns = "camera_pose_graph"; // Set the namespace and id for this marker. This serves to create a unique ID
nodes_marker.id = 1; // Any marker sent with the same namespace and id will overwrite the old one
nodes_marker.type = visualization_msgs::Marker::LINE_LIST;
nodes_marker.action = visualization_msgs::Marker::ADD; // Set the marker action. Options are ADD and DELETE
nodes_marker.frame_locked = true; //rviz automatically retransforms the markers into the frame every update cycle
// Set the scale of the marker -- 1x1x1 here means 1m on a side
nodes_marker.scale.x = 0.002;
//Global pose (used to transform all points) //TODO: is this the default pose anyway?
nodes_marker.pose.position.x = 0;
nodes_marker.pose.position.y = 0;
nodes_marker.pose.position.z = 0;
nodes_marker.pose.orientation.x = 0.0;
nodes_marker.pose.orientation.y = 0.0;
nodes_marker.pose.orientation.z = 0.0;
nodes_marker.pose.orientation.w = 1.0;
// Set the color -- be sure to set alpha to something non-zero!
nodes_marker.color.r = 1.0f;
nodes_marker.color.g = 0.0f;
nodes_marker.color.b = 0.0f;
nodes_marker.color.a = 1.0f;
geometry_msgs::Point tail; //same startpoint for each line segment
geometry_msgs::Point tip; //different endpoint for each line segment
std_msgs::ColorRGBA arrow_color_red ; //red x axis
arrow_color_red.r = 1.0;
arrow_color_red.a = 1.0;
std_msgs::ColorRGBA arrow_color_green; //green y axis
arrow_color_green.g = 1.0;
arrow_color_green.a = 1.0;
std_msgs::ColorRGBA arrow_color_blue ; //blue z axis
arrow_color_blue.b = 1.0;
arrow_color_blue.a = 1.0;
Vector3f origin(0.0,0.0,0.0);
Vector3f x_axis(0.2,0.0,0.0); //20cm long axis for the first (almost fixed) node
Vector3f y_axis(0.0,0.2,0.0);
Vector3f z_axis(0.0,0.0,0.2);
Vector3f tmp; //the transformed endpoints
int counter = 0;
AISNavigation::PoseGraph3D::Vertex* v; //used in loop
AISNavigation::PoseGraph3D::VertexIDMap::iterator vertex_iter = optimizer_->vertices().begin();
for(/*see above*/; vertex_iter != optimizer_->vertices().end(); vertex_iter++, counter++) {
v = static_cast<AISNavigation::PoseGraph3D::Vertex*>((*vertex_iter).second);
//v->transformation.rotation().x()+ v->transformation.rotation().y()+ v->transformation.rotation().z()+ v->transformation.rotation().w();
tmp = v->transformation * origin;
tail.x = tmp.x();
tail.y = tmp.y();
tail.z = tmp.z();
//Endpoints X-Axis
nodes_marker.points.push_back(tail);
nodes_marker.colors.push_back(arrow_color_red);
tmp = v->transformation * x_axis;
tip.x = tmp.x();
tip.y = tmp.y();
tip.z = tmp.z();
nodes_marker.points.push_back(tip);
nodes_marker.colors.push_back(arrow_color_red);
//Endpoints Y-Axis
nodes_marker.points.push_back(tail);
nodes_marker.colors.push_back(arrow_color_green);
tmp = v->transformation * y_axis;
tip.x = tmp.x();
tip.y = tmp.y();
tip.z = tmp.z();
nodes_marker.points.push_back(tip);
nodes_marker.colors.push_back(arrow_color_green);
//Endpoints Z-Axis
nodes_marker.points.push_back(tail);
nodes_marker.colors.push_back(arrow_color_blue);
tmp = v->transformation * z_axis;
tip.x = tmp.x();
tip.y = tmp.y();
tip.z = tmp.z();
nodes_marker.points.push_back(tip);
nodes_marker.colors.push_back(arrow_color_blue);
//shorten all nodes after the first one
x_axis.x() = 0.1;
y_axis.y() = 0.1;
z_axis.z() = 0.1;
}
marker_pub_.publish (nodes_marker);
ROS_INFO("published %d graph nodes", counter);
}
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");
}
Node::Node(ros::NodeHandle& nh, const cv::Mat& visual,
cv::Ptr<cv::FeatureDetector> detector,
cv::Ptr<cv::DescriptorExtractor> extractor,
cv::Ptr<cv::DescriptorMatcher> matcher,
const sensor_msgs::PointCloud2ConstPtr point_cloud,
const cv::Mat& detection_mask)
: id_(0),
flannIndex(NULL),
matcher_(matcher)
{
#ifdef USE_ICP_CODE
gicp_initialized = false;
#endif
std::clock_t starttime=std::clock();
#ifdef USE_SIFT_GPU
SiftGPUFeatureDetector* siftgpu = SiftGPUFeatureDetector::GetInstance();
float* descriptors = siftgpu->detect(visual, feature_locations_2d_);
if (descriptors == NULL) {
ROS_FATAL("Can't run SiftGPU");
}
#else
ROS_FATAL_COND(detector.empty(), "No valid detector!");
detector->detect( visual, feature_locations_2d_, detection_mask);// fill 2d locations
#endif
ROS_INFO("Feature detection and descriptor extraction runtime: %f", ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC);
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", "Feature detection runtime: " << ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC );
/*
if (id_ == 0)
cloud_pub_ = nh_->advertise<sensor_msgs::PointCloud2>("clouds_from_node_base",10);
else{
*/
cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/rgbdslam/batch_clouds",20);
// cloud_pub_ransac = nh_->advertise<sensor_msgs::PointCloud2>("clouds_from_node_current_ransac",10);
//} */
// get pcl::Pointcloud to extract depthValues a pixel positions
std::clock_t starttime5=std::clock();
// TODO: If batch sending/saving of clouds would be removed, the pointcloud wouldn't have to be saved
// which would slim down the Memory requirements
pcl::fromROSMsg(*point_cloud,pc_col);
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime5) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", "pc2->pcl conversion runtime: " << ( std::clock() - starttime5 ) / (double)CLOCKS_PER_SEC );
// project pixels to 3dPositions and create search structures for the gicp
#ifdef USE_SIFT_GPU
// removes also unused descriptors from the descriptors matrix
// build descriptor matrix
projectTo3DSiftGPU(feature_locations_2d_, feature_locations_3d_, pc_col, descriptors, feature_descriptors_); //takes less than 0.01 sec
if (descriptors != NULL) delete descriptors;
#else
projectTo3D(feature_locations_2d_, feature_locations_3d_, pc_col); //takes less than 0.01 sec
#endif
// projectTo3d need a dense cloud to use the points.at(px.x,px.y)-Call
#ifdef USE_ICP_CODE
std::clock_t starttime4=std::clock();
createGICPStructures();
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime4) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", "gicp runtime: " << ( std::clock() - starttime4 ) / (double)CLOCKS_PER_SEC );
#endif
std::clock_t starttime2=std::clock();
#ifndef USE_SIFT_GPU
// ROS_INFO("Use extractor");
//cv::Mat topleft, topright;
//topleft = visual.colRange(0,visual.cols/2+50);
//topright= visual.colRange(visual.cols/2+50, visual.cols-1);
//std::vector<cv::KeyPoint> kp1, kp2;
//extractor->compute(topleft, kp1, feature_descriptors_); //fill feature_descriptors_ with information
extractor->compute(visual, feature_locations_2d_, feature_descriptors_); //fill feature_descriptors_ with information
#endif
assert(feature_locations_2d_.size() == feature_locations_3d_.size());
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime2) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", "Feature extraction runtime: " << ( std::clock() - starttime2 ) / (double)CLOCKS_PER_SEC );
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", "constructor runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec");
}
///Find transformation with largest support, RANSAC style.
///Return false if no transformation can be found
bool Node::getRelativeTransformationTo(const Node* earlier_node,
std::vector<cv::DMatch>* initial_matches,
Eigen::Matrix4f& resulting_transformation,
float& rmse,
std::vector<cv::DMatch>& matches,
unsigned int ransac_iterations) const
{
std::clock_t starttime=std::clock();
assert(initial_matches != NULL);
matches.clear();
if(initial_matches->size() <= global_min_feature_count){
ROS_INFO("Only %d feature matches between %d and %d (minimal: %i)",(int)initial_matches->size() , this->id_, earlier_node->id_, global_min_feature_count);
return false;
}
unsigned int min_inlier_threshold = int(initial_matches->size()*global_min_inlier_pct);
std::vector<cv::DMatch> inlier; //holds those feature correspondences that support the transformation
double inlier_error; //all squared errors
srand((long)std::clock());
// a point is an inlier if it's no more than max_dist_m m from its partner apart
const float max_dist_m = global_max_dist_for_inliers;
vector<double> dummy;
// best values of all iterations (including invalids)
double best_error = 1e6, best_error_invalid = 1e6;
unsigned int best_inlier_invalid = 0, best_inlier_cnt = 0, valid_iterations = 0;
Eigen::Matrix4f transformation;
const unsigned int sample_size = 3;// chose this many randomly from the correspondences:
for (uint n_iter = 0; n_iter < ransac_iterations; n_iter++) {
//generate a map of samples. Using a map solves the problem of drawing a sample more than once
std::set<cv::DMatch> sample_matches;
std::vector<cv::DMatch> sample_matches_vector;
while(sample_matches.size() < sample_size){
int id = rand() % initial_matches->size();
sample_matches.insert(initial_matches->at(id));
sample_matches_vector.push_back(initial_matches->at(id));
}
bool valid; // valid is false iff the sampled points clearly aren't inliers themself
transformation = getTransformFromMatches(earlier_node, sample_matches.begin(), sample_matches.end(),valid,max_dist_m);
if (!valid) continue; // valid is false iff the sampled points aren't inliers themself
if(transformation!=transformation) continue; //Contains NaN
//test whether samples are inliers (more strict than before)
computeInliersAndError(sample_matches_vector, transformation, this->feature_locations_3d_,
earlier_node->feature_locations_3d_, inlier, inlier_error, /*output*/
dummy, max_dist_m*max_dist_m);
if(inlier_error > 1000) continue; //most possibly a false match in the samples
computeInliersAndError(*initial_matches, transformation, this->feature_locations_3d_,
earlier_node->feature_locations_3d_, inlier, inlier_error, /*output*/
dummy, max_dist_m*max_dist_m);
// check also invalid iterations
if (inlier.size() > best_inlier_invalid) {
best_inlier_invalid = inlier.size();
best_error_invalid = inlier_error;
}
// ROS_INFO("iteration %d cnt: %d, best: %d, error: %.2f",n_iter, inlier.size(), best_inlier_cnt, inlier_error*100);
if(inlier.size() < min_inlier_threshold || inlier_error > max_dist_m){
//inlier.size() < ((float)initial_matches->size())*min_inlier_ratio ||
// ROS_INFO("Skipped iteration: inliers: %i (min %i), inlier_error: %.2f (max %.2f)", (int)inlier.size(), (int) min_inlier_threshold, inlier_error*100, max_dist_m*100);
continue;
}
// ROS_INFO("Refining iteration from %i samples: all matches: %i, inliers: %i, inlier_error: %f", (int)sample_size, (int)initial_matches->size(), (int)inlier.size(), inlier_error);
valid_iterations++;
//if (inlier_error > 0) ROS_ERROR("size: %i", (int)dummy.size());
assert(inlier_error>0);
//Performance hacks:
///Iterations with more than half of the initial_matches inlying, count twice
if (inlier.size() > initial_matches->size()*0.5) n_iter++;
///Iterations with more than 80% of the initial_matches inlying, count threefold
if (inlier.size() > initial_matches->size()*0.8) n_iter++;
if (inlier_error < best_error) { //copy this to the result
resulting_transformation = transformation;
matches = inlier;
assert(matches.size()>= min_inlier_threshold);
best_inlier_cnt = inlier.size();
//assert(matches.size()>= ((float)initial_matches->size())*min_inlier_ratio);
rmse = inlier_error;
best_error = inlier_error;
// ROS_INFO(" new best iteration %d cnt: %d, best_inlier: %d, error: %.4f, bestError: %.4f",n_iter, inlier.size(), best_inlier_cnt, inlier_error, best_error);
}else
{
// ROS_INFO("NO new best iteration %d cnt: %d, best_inlier: %d, error: %.4f, bestError: %.4f",n_iter, inlier.size(), best_inlier_cnt, inlier_error, best_error);
}
//int max_ndx = min((int) min_inlier_threshold,30); //? What is this 30?
double new_inlier_error;
transformation = getTransformFromMatches(earlier_node, matches.begin(), matches.end(), valid); // compute new trafo from all inliers:
if(transformation!=transformation) continue; //Contains NaN
computeInliersAndError(*initial_matches, transformation,
this->feature_locations_3d_, earlier_node->feature_locations_3d_,
inlier, new_inlier_error, dummy, max_dist_m*max_dist_m);
// ROS_INFO("asd recomputed: inliersize: %i, inlier error: %f", (int) inlier.size(),100*new_inlier_error);
// check also invalid iterations
if (inlier.size() > best_inlier_invalid)
{
best_inlier_invalid = inlier.size();
best_error_invalid = inlier_error;
}
if(inlier.size() < min_inlier_threshold || new_inlier_error > max_dist_m){
//inlier.size() < ((float)initial_matches->size())*min_inlier_ratio ||
// ROS_INFO("Skipped iteration: inliers: %i (min %i), inlier_error: %.2f (max %.2f)", (int)inlier.size(), (int) min_inlier_threshold, inlier_error*100, max_dist_m*100);
continue;
}
// ROS_INFO("Refined iteration from %i samples: all matches %i, inliers: %i, new_inlier_error: %f", (int)sample_size, (int)initial_matches->size(), (int)inlier.size(), new_inlier_error);
assert(new_inlier_error>0);
if (new_inlier_error < best_error)
{
resulting_transformation = transformation;
matches = inlier;
assert(matches.size()>= min_inlier_threshold);
//assert(matches.size()>= ((float)initial_matches->size())*min_inlier_ratio);
rmse = new_inlier_error;
best_error = new_inlier_error;
// ROS_INFO(" improved: new best iteration %d cnt: %d, best_inlier: %d, error: %.2f, bestError: %.2f",n_iter, inlier.size(), best_inlier_cnt, inlier_error*100, best_error*100);
}else
{
// ROS_INFO("improved: NO new best iteration %d cnt: %d, best_inlier: %d, error: %.2f, bestError: %.2f",n_iter, inlier.size(), best_inlier_cnt, inlier_error*100, best_error*100);
}
} //iterations
ROS_INFO("%i good iterations (from %i), inlier pct %i, inlier cnt: %i, error: %.2f cm",valid_iterations, (int) ransac_iterations, (int) (matches.size()*1.0/initial_matches->size()*100),(int) matches.size(),rmse*100);
// ROS_INFO("best overall: inlier: %i, error: %.2f",best_inlier_invalid, best_error_invalid*100);
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", "getRelativeTransformationTo runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec");
return matches.size() >= min_inlier_threshold;
}
int main( int argc, char **argv )
{
ros::init( argc, argv, "example2" );
ros::NodeHandle n;
const double val = 3.14;
// Basic messages:
ROS_INFO( "My INFO message." );
ROS_INFO( "My INFO message with argument: %f", val );
ROS_INFO_STREAM(
"My INFO stream message with argument: " << val
);
// Named messages:
ROS_INFO_STREAM_NAMED(
"named_msg",
"My named INFO stream message; val = " << val
);
// Conditional messages:
ROS_INFO_STREAM_COND(
val < 0.,
"My conditional INFO stream message; val (" << val << ") < 0"
);
ROS_INFO_STREAM_COND(
val >= 0.,
"My conditional INFO stream message; val (" << val << ") >= 0"
);
// Conditional Named messages:
ROS_INFO_STREAM_COND_NAMED(
val < 0., "cond_named_msg",
"My conditional INFO stream message; val (" << val << ") < 0"
);
ROS_INFO_STREAM_COND_NAMED(
val >= 0., "cond_named_msg",
"My conditional INFO stream message; val (" << val << ") >= 0"
);
// Filtered messages:
struct MyLowerFilter : public ros::console::FilterBase {
MyLowerFilter( const double& val ) : value( val ) {}
inline virtual bool isEnabled()
{
return value < 0.;
}
double value;
};
struct MyGreaterEqualFilter : public ros::console::FilterBase {
MyGreaterEqualFilter( const double& val ) : value( val ) {}
inline virtual bool isEnabled()
{
return value >= 0.;
}
double value;
};
MyLowerFilter filter_lower( val );
MyGreaterEqualFilter filter_greater_equal( val );
ROS_INFO_STREAM_FILTER(
&filter_lower,
"My filter INFO stream message; val (" << val << ") < 0"
);
ROS_INFO_STREAM_FILTER(
&filter_greater_equal,
"My filter INFO stream message; val (" << val << ") >= 0"
);
// Once messages:
for( int i = 0; i < 10; ++i ) {
ROS_INFO_STREAM_ONCE(
"My once INFO stream message; i = " << i
);
}
// Throttle messages:
for( int i = 0; i < 10; ++i ) {
ROS_INFO_STREAM_THROTTLE(
2,
"My throttle INFO stream message; i = " << i
);
ros::Duration( 1 ).sleep();
}
ros::spinOnce();
return EXIT_SUCCESS;
}
