void
WmMapServer::step()
{
if(!do_mapping_)
{
tf::StampedTransform World2Map;
tf::Quaternion q;
q.setEuler(0.0f, 0.0f, 0.0);
World2Map.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
World2Map.setRotation(q);
World2Map.frame_id_ = "/world";
World2Map.stamp_ = ros::Time::now();
World2Map.child_frame_id_ = worldFrameId_;
try
{
tfBroadcaster_.sendTransform(World2Map);
}catch(tf::TransformException & ex)
{
ROS_WARN("WmMapServer::step %s",ex.what());
}
}
publishMap(ros::Time::now());
}
void HectorMappingRos::publishMapLoop(double map_pub_period)
{
ros::Rate r(1.0 / map_pub_period);
while(ros::ok())
{
//ros::WallTime t1 = ros::WallTime::now();
ros::Time mapTime (ros::Time::now());
//publishMap(mapPubContainer[2],slamProcessor->getGridMap(2), mapTime);
//publishMap(mapPubContainer[1],slamProcessor->getGridMap(1), mapTime);
//if(p_update_given_map_ || first_Time){
publishMap(mapPubContainer[0],slamProcessor->getGridMap(0), mapTime, slamProcessor->getMapMutex(0));
//first_Time = false;
//}
//ros::WallDuration t2 = ros::WallTime::now() - t1;
//std::cout << "time s: " << t2.toSec();
//ROS_INFO("HectorSM ms: %4.2f", t2.toSec()*1000.0f);
r.sleep();
}
}
void Node::mapPublishThread(ros::Rate rate)
{
while(ros::ok()) {
{
#ifdef USE_HECTOR_TIMING
hector_diagnostics::TimingSection section("map publish");
#endif
publishMap();
}
rate.sleep();
}
}
void Node::reset()
{
MapBase::UniqueLock lock(map_->getWriteLock());
last_map_update_pose_.setIdentity();
last_map_update_time_ = ros::Time();
map_odom_transform_.setIdentity();
if (map_) map_->reset();
if (matcher_) matcher_->reset();
lock.unlock();
publishMap();
publishPose();
}
void sptam::sptam_node::onImages(
const sensor_msgs::ImageConstPtr& l_image_msg, const sensor_msgs::CameraInfoConstPtr& left_info,
const sensor_msgs::ImageConstPtr& r_image_msg, const sensor_msgs::CameraInfoConstPtr& right_info
)
{
ROS_INFO_STREAM("dt: " << (l_image_msg->header.stamp - r_image_msg->header.stamp).toSec());
// Save current Time
ros::Time currentTime = l_image_msg->header.stamp;
// If using odometry, try to get a new estimate for the current pose.
// if not using odometry, camera_to_odom is left blank.
tf::StampedTransform camera_to_odom;
if ( use_odometry_ )
{
if ( not getCameraInOdom(camera_to_odom, currentTime) )
return;
TFPose2CameraPose(odom_to_map_ * camera_to_odom, cameraPose_);
}
else
{
cv::Point3d currentCameraPosition;
cv::Vec4d currentCameraOrientation;
motionModel_->PredictNextCameraPose(currentCameraPosition, currentCameraOrientation);
cameraPose_ = CameraPose( currentCameraPosition, currentCameraOrientation );
}
// If SPTAM has not been initialized yet, do it
if ( sptam_ == nullptr )
{
ROS_INFO_STREAM("init calib");
loadCameraCalibration(left_info, right_info);
}
// convert image to OpenCv cv::Mat format
cv_bridge::CvImageConstPtr bridgeLeft_ptr = cv_bridge::toCvShare(l_image_msg, "rgb8");
cv_bridge::CvImageConstPtr bridgeRight_ptr = cv_bridge::toCvShare(r_image_msg, "rgb8");
// save images
cv::Mat imageLeft = bridgeLeft_ptr->image;
cv::Mat imageRight = bridgeRight_ptr->image;
#ifdef SHOW_PROFILING
double start, end;
start = GetSeg();
#endif // SHOW_PROFILING
#ifdef SHOW_PROFILING
double startStep, endStep;
startStep = GetSeg();
#endif
FeatureExtractorThread featureExtractorThreadLeft(imageLeft, *featureDetector_, *descriptorExtractor_);
FeatureExtractorThread featureExtractorThreadRight(imageRight, *featureDetector_, *descriptorExtractor_);
featureExtractorThreadLeft.WaitUntilFinished();
const std::vector<cv::KeyPoint>& keyPointsLeft = featureExtractorThreadLeft.GetKeyPoints();
const cv::Mat& descriptorsLeft = featureExtractorThreadLeft.GetDescriptors();
featureExtractorThreadRight.WaitUntilFinished();
const std::vector<cv::KeyPoint>& keyPointsRight = featureExtractorThreadRight.GetKeyPoints();
const cv::Mat& descriptorsRight = featureExtractorThreadRight.GetDescriptors();
ImageFeatures imageFeaturesLeft = ImageFeatures(imageLeft, keyPointsLeft, descriptorsLeft, mapper_params_.matchingCellSize);
ImageFeatures imageFeaturesRight = ImageFeatures(imageRight, keyPointsRight, descriptorsRight, mapper_params_.matchingCellSize);
#ifdef SHOW_PROFILING
endStep = GetSeg();
WriteToLog(" tk Extract: ", startStep, endStep);
#endif
// if the map was not initialized, try to build it
if( not map_.nMapPoints() )
{
ROS_INFO("Trying to intialize map...");
// Create keyFrame
StereoFrame* frame = new StereoFrame(
cameraPose_, cameraParametersLeft_,
stereo_baseline_, cameraParametersRight_,
imageFeaturesLeft, imageFeaturesRight, true
);
frame->SetId( 0 ); // Set Keyframe ID
// Initialize MapMaker
// TODO: this bool must be a local variable to do not check not map_.nMapPoints() in the "if"
/*bool isMapInitialized = */InitFromStereo( map_, *frame, imageLeft, *rowMatcher_);
}
// if the map is already initialized, do tracking
else
{
cameraPose_ = sptam_->track(cameraPose_, imageFeaturesLeft, imageFeaturesRight, imageLeft, imageRight);
#ifdef SHOW_PROFILING
end = GetSeg();
std::cout << GetSeg() << " tk trackingtotal: " << (end - start) << std::endl;
std::stringstream message;
message << std::fixed << GetSeg() << " tk trackingtotal: " << (end - start) << std::endl;
Logger::Write( message.str() );
#endif
// fix the error between map and odom to correct the current pose.
if ( use_odometry_ )
fixOdomFrame( cameraPose_, camera_to_odom, currentTime );
else
// if odometry is disabled, odom_to_map_ actually contains the map->cam transform because I'm too lazy
{
motionModel_->UpdateCameraPose(cameraPose_.GetPosition(), cameraPose_.GetOrientationQuaternion());
tf::Transform cam_to_map;
CameraPose2TFPose(cameraPose_, cam_to_map);
tf::StampedTransform base_to_cam;
transform_listener_.lookupTransform(camera_frame_, base_frame_, currentTime, base_to_cam);
std::lock_guard<std::mutex> lock( odom_to_map_mutex_ );
odom_to_map_ = cam_to_map * base_to_cam;
}
}
// Publish Map To be drawn by rviz visualizer
publishMap();
// Publish the camera Pose
publishPose( l_image_msg->header.seq, currentTime, cameraPose_ );
}
void
WmMapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_in)
{
ros::WallTime startTime = ros::WallTime::now();
if(!do_mapping_)
return;
pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ());
pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
pcl_conversions::toPCL(*cloud_in, *cloud);
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloud);
sor.setLeafSize (res_, res_, res_);
sor.filter (*cloud_filtered);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc(new pcl::PointCloud<pcl::PointXYZRGB>); // input cloud for filtering and ground-detection
pcl::fromPCLPointCloud2 (*cloud_filtered, *pc);
/*
* Filter in Robot space
*/
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_filtered(new pcl::PointCloud<pcl::PointXYZRGB>); // input cloud for filtering and ground-detection
tf::StampedTransform sensorToRobotTf;
Eigen::Matrix4f sensorToRobot;
try {
tfListener_.lookupTransform(baseFrameId_, cloud_in->header.frame_id, cloud_in->header.stamp, sensorToRobotTf);
} catch(tf::TransformException& ex){
ROS_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback");
return;
}
pcl_ros::transformAsMatrix(sensorToRobotTf, sensorToRobot);
pcl::transformPointCloud(*pc, *pc, sensorToRobot);
for(pcl::PointCloud<pcl::PointXYZRGB>::iterator it=pc->begin(); it!=pc->end(); ++it)
{
if(!std::isnan(it->x))
{
double thetaxy, thetaxz;
thetaxy = atan2(it->y, it->x);
thetaxz = atan2(it->z, it->x);
if(fabs(thetaxy)<pointcloudMaxTxy_ && fabs(thetaxz)<pointcloudMaxTxz_ &&
it->x < pointcloudMaxX_ &&
it->z > pointcloudMinZ_ && it->z < pointcloudMaxZ_)
pc_filtered->push_back(*it);
}
}
/*
* Add to Map
*/
tf::StampedTransform RobotToWorldTf;
try {
tfListener_.lookupTransform(worldFrameId_, baseFrameId_, cloud_in->header.stamp, RobotToWorldTf);
} catch(tf::TransformException& ex){
ROS_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback");
return;
}
Eigen::Matrix4f RobotToWorld;
pcl_ros::transformAsMatrix(RobotToWorldTf, RobotToWorld);
pcl::transformPointCloud(*pc_filtered, *pc_filtered, RobotToWorld);
int c=0;
bool newPoint = false;
for(pcl::PointCloud<pcl::PointXYZRGB>::iterator it=pc_filtered->begin(); it!=pc_filtered->end(); ++it)
if(validNewPoint(*it))
{
map_->push_back(*it);
octree_->addPointToCloud (*it, map_);
c++;
}
double total_elapsed = (ros::WallTime::now() - startTime).toSec();
ROS_DEBUG("Pointcloud insertion in map done (%d new points, %zu pts total, %f sec)", c, map_->size(), total_elapsed);
publishMap(cloud_in->header.stamp);
return;
}
