template <typename PointNT> void
pcl::MarchingCubesHoppe<PointNT>::voxelizeData ()
{
const bool is_far_ignored = dist_ignore_ > 0.0f;
for (int x = 0; x < res_x_; ++x)
{
const int y_start = x * res_y_ * res_z_;
for (int y = 0; y < res_y_; ++y)
{
const int z_start = y_start + y * res_z_;
for (int z = 0; z < res_z_; ++z)
{
std::vector<int> nn_indices (1, 0);
std::vector<float> nn_sqr_dists (1, 0.0f);
const Eigen::Vector3f point = (lower_boundary_ + size_voxel_ * Eigen::Array3f (x, y, z)).matrix ();
PointNT p;
p.getVector3fMap () = point;
tree_->nearestKSearch (p, 1, nn_indices, nn_sqr_dists);
if (!is_far_ignored || nn_sqr_dists[0] < dist_ignore_)
{
const Eigen::Vector3f normal = input_->points[nn_indices[0]].getNormalVector3fMap ();
if (!std::isnan (normal (0)) && normal.norm () > 0.5f)
grid_[z_start + z] = normal.dot (
point - input_->points[nn_indices[0]].getVector3fMap ());
}
}
}
}
}
void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in)
{
if (!we_have_a_map)
{
ROS_INFO("SnapMapICP waiting for map to be published");
return;
}
ros::Time scan_in_time = scan_in->header.stamp;
ros::Time time_received = ros::Time::now();
if ( scan_in_time - last_processed_scan < ros::Duration(1.0f / SCAN_RATE) )
{
ROS_DEBUG("rejected scan, last %f , this %f", last_processed_scan.toSec() ,scan_in_time.toSec());
return;
}
//projector_.transformLaserScanToPointCloud("base_link",*scan_in,cloud,listener_);
if (!scan_callback_mutex.try_lock())
return;
ros::Duration scan_age = ros::Time::now() - scan_in_time;
//check if we want to accept this scan, if its older than 1 sec, drop it
if (!use_sim_time)
if (scan_age.toSec() > AGE_THRESHOLD)
{
//ROS_WARN("SCAN SEEMS TOO OLD (%f seconds, %f threshold)", scan_age.toSec(), AGE_THRESHOLD);
ROS_WARN("SCAN SEEMS TOO OLD (%f seconds, %f threshold) scan time: %f , now %f", scan_age.toSec(), AGE_THRESHOLD, scan_in_time.toSec(),ros::Time::now().toSec() );
scan_callback_mutex.unlock();
return;
}
count_sc_++;
//ROS_DEBUG("count_sc %i MUTEX LOCKED", count_sc_);
//if (count_sc_ > 10)
//if (count_sc_ > 10)
{
count_sc_ = 0;
tf::StampedTransform base_at_laser;
if (!getTransform(base_at_laser, ODOM_FRAME, "base_link", scan_in_time))
{
ROS_WARN("Did not get base pose at laser scan time");
scan_callback_mutex.unlock();
return;
}
sensor_msgs::PointCloud cloud;
sensor_msgs::PointCloud cloudInMap;
projector_->projectLaser(*scan_in,cloud);
we_have_a_scan = false;
bool gotTransform = false;
if (!listener_->waitForTransform("/map", cloud.header.frame_id, cloud.header.stamp, ros::Duration(0.05)))
{
scan_callback_mutex.unlock();
ROS_WARN("SnapMapICP no map to cloud transform found MUTEX UNLOCKED");
return;
}
if (!listener_->waitForTransform("/map", "/base_link", cloud.header.stamp, ros::Duration(0.05)))
{
scan_callback_mutex.unlock();
ROS_WARN("SnapMapICP no map to base transform found MUTEX UNLOCKED");
return;
}
while (!gotTransform && (ros::ok()))
{
try
{
gotTransform = true;
listener_->transformPointCloud ("/map",cloud,cloudInMap);
}
catch (...)
{
gotTransform = false;
ROS_WARN("DIDNT GET TRANSFORM IN A");
}
}
for (size_t k =0; k < cloudInMap.points.size(); k++)
{
cloudInMap.points[k].z = 0;
}
gotTransform = false;
tf::StampedTransform oldPose;
while (!gotTransform && (ros::ok()))
{
try
{
gotTransform = true;
listener_->lookupTransform("/map", "/base_link",
cloud.header.stamp , oldPose);
}
catch (tf::TransformException ex)
{
gotTransform = false;
ROS_WARN("DIDNT GET TRANSFORM IN B");
}
}
if (we_have_a_map && gotTransform)
{
sensor_msgs::convertPointCloudToPointCloud2(cloudInMap,cloud2);
we_have_a_scan = true;
actScan++;
//pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> reg;
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> reg;
reg.setTransformationEpsilon (1e-6);
// Set the maximum distance between two correspondences (src<->tgt) to 10cm
// Note: adjust this based on the size of your datasets
reg.setMaxCorrespondenceDistance(0.5);
reg.setMaximumIterations (ICP_NUM_ITER);
// Set the point representation
//ros::Time bef = ros::Time::now();
PointCloudT::Ptr myMapCloud (new PointCloudT());
PointCloudT::Ptr myScanCloud (new PointCloudT());
pcl::fromROSMsg(*output_cloud,*myMapCloud);
pcl::fromROSMsg(cloud2,*myScanCloud);
reg.setInputCloud(myScanCloud);
reg.setInputTarget(myMapCloud);
PointCloudT unused;
int i = 0;
reg.align (unused);
const Eigen::Matrix4f &transf = reg.getFinalTransformation();
tf::Transform t;
matrixAsTransfrom(transf,t);
//ROS_ERROR("proc time %f", (ros::Time::now() - bef).toSec());
we_have_a_scan_transformed = false;
PointCloudT transformedCloud;
pcl::transformPointCloud (*myScanCloud, transformedCloud, reg.getFinalTransformation());
double inlier_perc = 0;
{
// count inliers
std::vector<int> nn_indices (1);
std::vector<float> nn_sqr_dists (1);
size_t numinliers = 0;
for (size_t k = 0; k < transformedCloud.points.size(); ++k )
{
if (mapTree->radiusSearch (transformedCloud.points[k], ICP_INLIER_DIST, nn_indices,nn_sqr_dists, 1) != 0)
numinliers += 1;
}
if (transformedCloud.points.size() > 0)
{
//ROS_INFO("Inliers in dist %f: %zu of %zu percentage %f (%f)", ICP_INLIER_DIST, numinliers, transformedCloud.points.size(), (double) numinliers / (double) transformedCloud.points.size(), ICP_INLIER_THRESHOLD);
inlier_perc = (double) numinliers / (double) transformedCloud.points.size();
}
}
last_processed_scan = scan_in_time;
pcl::toROSMsg (transformedCloud, cloud2transformed);
we_have_a_scan_transformed = true;
double dist = sqrt((t.getOrigin().x() * t.getOrigin().x()) + (t.getOrigin().y() * t.getOrigin().y()));
double angleDist = t.getRotation().getAngle();
tf::Vector3 rotAxis = t.getRotation().getAxis();
t = t * oldPose;
tf::StampedTransform base_after_icp;
if (!getTransform(base_after_icp, ODOM_FRAME, "base_link", ros::Time(0)))
{
ROS_WARN("Did not get base pose at now");
scan_callback_mutex.unlock();
return;
}
else
{
tf::Transform rel = base_at_laser.inverseTimes(base_after_icp);
ROS_DEBUG("relative motion of robot while doing icp: %fcm %fdeg", rel.getOrigin().length(), rel.getRotation().getAngle() * 180 / M_PI);
t= t * rel;
}
//ROS_DEBUG("dist %f angleDist %f",dist, angleDist);
//ROS_DEBUG("SCAN_AGE seems to be %f", scan_age.toSec());
char msg_c_str[2048];
sprintf(msg_c_str,"INLIERS %f (%f) scan_age %f (%f age_threshold) dist %f angleDist %f axis(%f %f %f) fitting %f (icp_fitness_threshold %f)",inlier_perc, ICP_INLIER_THRESHOLD, scan_age.toSec(), AGE_THRESHOLD ,dist, angleDist, rotAxis.x(), rotAxis.y(), rotAxis.z(),reg.getFitnessScore(), ICP_FITNESS_THRESHOLD );
std_msgs::String strmsg;
strmsg.data = msg_c_str;
//ROS_DEBUG("%s", msg_c_str);
double cov = POSE_COVARIANCE_TRANS;
//if ((actScan - lastTimeSent > UPDATE_AGE_THRESHOLD) && ((dist > DIST_THRESHOLD) || (angleDist > ANGLE_THRESHOLD)) && (angleDist < ANGLE_UPPER_THRESHOLD))
// if ( reg.getFitnessScore() <= ICP_FITNESS_THRESHOLD )
if ((actScan - lastTimeSent > UPDATE_AGE_THRESHOLD) && ((dist > DIST_THRESHOLD) || (angleDist > ANGLE_THRESHOLD)) && (inlier_perc > ICP_INLIER_THRESHOLD) && (angleDist < ANGLE_UPPER_THRESHOLD))
{
lastTimeSent = actScan;
geometry_msgs::PoseWithCovarianceStamped pose;
pose.header.frame_id = "map";
pose.pose.pose.position.x = t.getOrigin().x();
pose.pose.pose.position.y = t.getOrigin().y();
tf::Quaternion quat = t.getRotation();
//quat.setRPY(0.0, 0.0, theta);
tf::quaternionTFToMsg(quat,pose.pose.pose.orientation);
float factorPos = 0.03;
float factorRot = 0.1;
pose.pose.covariance[6*0+0] = (cov * cov) * factorPos;
pose.pose.covariance[6*1+1] = (cov * cov) * factorPos;
pose.pose.covariance[6*3+3] = (M_PI/12.0 * M_PI/12.0) * factorRot;
ROS_DEBUG("i %i converged %i SCORE: %f", i, reg.hasConverged (), reg.getFitnessScore() );
ROS_DEBUG("PUBLISHING A NEW INITIAL POSE dist %f angleDist %f Setting pose: %.3f %.3f [frame=%s]",dist, angleDist , pose.pose.pose.position.x , pose.pose.pose.position.y , pose.header.frame_id.c_str());
pub_pose.publish(pose);
strmsg.data += " << SENT";
}
//ROS_INFO("processing time : %f", (ros::Time::now() - time_received).toSec());
pub_info_.publish(strmsg);
//ROS_DEBUG("map width %i height %i size %i, %s", myMapCloud.width, myMapCloud.height, (int)myMapCloud.points.size(), myMapCloud.header.frame_id.c_str());
//ROS_DEBUG("scan width %i height %i size %i, %s", myScanCloud.width, myScanCloud.height, (int)myScanCloud.points.size(), myScanCloud.header.frame_id.c_str());
}
}
scan_callback_mutex.unlock();
}