C++ UTimer::restart方法代码示例

//.........这里部分代码省略.........
				msg.header.stamp = time;

				msg.angle_min = scanAngleMin;
				msg.angle_max = scanAngleMax;
				msg.angle_increment = scanAngleIncrement;
				msg.time_increment = 0.0;
				msg.scan_time = 0;
				msg.range_min = scanRangeMin;
				msg.range_max = scanRangeMax;
				if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
				{
					msg.angle_min = odom.data().laserScanRaw().angleMin();
					msg.angle_max = odom.data().laserScanRaw().angleMax();
					msg.angle_increment = odom.data().laserScanRaw().angleIncrement();
					msg.range_min = odom.data().laserScanRaw().rangeMin();
					msg.range_max = odom.data().laserScanRaw().rangeMax();
				}

				uint32_t rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment);
				msg.ranges.assign(rangesSize, 0.0);

				const cv::Mat & scan = odom.data().laserScanRaw().data();
				for (int i=0; i<scan.cols; ++i)
				{
					const float * ptr = scan.ptr<float>(0,i);
					double range = hypot(ptr[0], ptr[1]);
					if (range >= msg.range_min && range <=msg.range_max)
					{
						double angle = atan2(ptr[1], ptr[0]);
						if (angle >= msg.angle_min && angle <= msg.angle_max)
						{
							int index = (angle - msg.angle_min) / msg.angle_increment;
							if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0))
							{
								msg.ranges[index] = range;
							}
						}
					}
				}

				scanPub.publish(msg);
			}
			else if(scanCloudPub.getNumSubscribers())
			{
				sensor_msgs::PointCloud2 msg;
				pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(odom.data().laserScanRaw()), msg);
				msg.header.frame_id = scanFrameId;
				msg.header.stamp = time;
				scanCloudPub.publish(msg);
			}
		}

		if(odom.data().userDataRaw().type() == CV_8SC1 &&
		   odom.data().userDataRaw().cols >= 7 && // including null str ending
		   odom.data().userDataRaw().rows == 1 &&
		   memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)
		{
			//GOAL format detected, remove it from the user data and send it as goal event
			std::string goalStr = (const char *)odom.data().userDataRaw().data;
			if(!goalStr.empty())
			{
				std::list<std::string> strs = uSplit(goalStr, ':');
				if(strs.size() == 2)
				{
					int goalId = atoi(strs.rbegin()->c_str());

					if(goalId > 0)
					{
						ROS_WARN("Goal %d detected, calling rtabmap's set_goal service!", goalId);
						rtabmap_ros::SetGoal setGoalSrv;
						setGoalSrv.request.node_id = goalId;
						setGoalSrv.request.node_label = "";
						if(!ros::service::call("set_goal", setGoalSrv))
						{
							ROS_ERROR("Can't call \"set_goal\" service");
						}
					}
				}
			}
		}

		ros::spinOnce();

		while(ros::ok() && paused)
		{
			uSleep(100);
			ros::spinOnce();
		}

		timer.restart();
		cameraInfo = rtabmap::CameraInfo();
		data = reader.takeImage(&cameraInfo);
		odomInfo.reg.covariance = cameraInfo.odomCovariance;
		odom = rtabmap::OdometryEvent(data, cameraInfo.odomPose, odomInfo);
		acquisitionTime = timer.ticks();
	}


	return 0;
}

cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
		const Memory * memory,
		const std::vector<int> & oldIds,
		const std::vector<int> & newIds) const
{
	UTimer timer;
	UDEBUG("");

	UASSERT(memory &&
		oldIds.size() &&
		newIds.size() &&
		oldIds.size() == (unsigned int)oldPrediction.cols &&
		oldIds.size() == (unsigned int)oldPrediction.rows);

	cv::Mat prediction = cv::Mat::zeros(newIds.size(), newIds.size(), CV_32FC1);

	// Create id to index maps
	std::map<int, int> oldIdToIndexMap;
	std::map<int, int> newIdToIndexMap;
	for(unsigned int i=0; i<oldIds.size() || i<newIds.size(); ++i)
	{
		if(i<oldIds.size())
		{
			UASSERT(oldIds[i]);
			oldIdToIndexMap.insert(oldIdToIndexMap.end(), std::make_pair(oldIds[i], i));
			//UDEBUG("oldIdToIndexMap[%d] = %d", oldIds[i], i);
		}
		if(i<newIds.size())
		{
			UASSERT(newIds[i]);
			newIdToIndexMap.insert(newIdToIndexMap.end(), std::make_pair(newIds[i], i));
			//UDEBUG("newIdToIndexMap[%d] = %d", newIds[i], i);
		}
	}
	UDEBUG("time creating id-index maps = %fs", timer.restart());

	//Get removed ids
	std::set<int> removedIds;
	for(unsigned int i=0; i<oldIds.size(); ++i)
	{
		if(!uContains(newIdToIndexMap, oldIds[i]))
		{
			removedIds.insert(removedIds.end(), oldIds[i]);
			UDEBUG("removed id=%d at oldIndex=%d", oldIds[i], i);
		}
	}
	UDEBUG("time getting removed ids = %fs", timer.restart());

	int added = 0;
	// get ids to update
	std::set<int> idsToUpdate;
	for(unsigned int i=0; i<oldIds.size() || i<newIds.size(); ++i)
	{
		if(i<oldIds.size())
		{
			if(removedIds.find(oldIds[i]) != removedIds.end())
			{
				unsigned int cols = oldPrediction.cols;
				for(unsigned int j=0; j<cols; ++j)
				{
					if(((const float *)oldPrediction.data)[i + j*cols] != 0.0f &&
					   j!=i &&
					   removedIds.find(oldIds[j]) == removedIds.end())
					{
						//UDEBUG("to update id=%d from id=%d removed (value=%f)", oldIds[j], oldIds[i], ((const float *)oldPrediction.data)[i + j*cols]);
						idsToUpdate.insert(oldIds[j]);
					}
				}
			}
		}
		if(i<newIds.size() && !uContains(oldIdToIndexMap,newIds[i]))
		{
			std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0);
			float sum = this->addNeighborProb(prediction, i, neighbors, newIdToIndexMap);
			this->normalize(prediction, i, sum, newIds[0]<0);
			++added;
			for(std::map<int,int>::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
			{
				if(uContains(oldIdToIndexMap, iter->first) &&
				   removedIds.find(iter->first) == removedIds.end())
				{
					idsToUpdate.insert(iter->first);
				}
			}
		}
	}
	UDEBUG("time getting ids to update = %fs", timer.restart());

	// update modified/added ids
	int modified = 0;
	for(std::set<int>::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter)
	{
		std::map<int, int> neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0);
		int index = newIdToIndexMap.at(*iter);
		float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap);
		this->normalize(prediction, index, sum, newIds[0]<0);
		++modified;
	}
	UDEBUG("time updating modified/added ids = %fs", timer.restart());

//.........这里部分代码省略.........

//.........这里部分代码省略.........
			cv_bridge::CvImage img;
			img.encoding = sensor_msgs::image_encodings::MONO8;
			img.image = odom.data().rightRaw();
			sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
			imageRosMsg->header.frame_id = cameraFrameId;
			imageRosMsg->header.stamp = time;

			rightPub.publish(imageRosMsg);
			rightCamInfoPub.publish(camInfoB);
		}

		if(scanPub.getNumSubscribers() && !odom.data().laserScanRaw().empty())
		{
			//inspired from pointcloud_to_laserscan package
			sensor_msgs::LaserScan msg;
			msg.header.frame_id = scanFrameId;
			msg.header.stamp = time;

			msg.angle_min = scanAngleMin;
			msg.angle_max = scanAngleMax;
			msg.angle_increment = scanAngleIncrement;
			msg.time_increment = 0.0;
			msg.scan_time = scanTime;
			msg.range_min = scanRangeMin;
			msg.range_max = scanRangeMax;

			uint32_t rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment);
			msg.ranges.assign(rangesSize, 0.0);

			const cv::Mat & scan = odom.data().laserScanRaw();
			UASSERT(scan.type() == CV_32FC2 || scan.type() == CV_32FC3);
			UASSERT(scan.rows == 1);
			for (int i=0; i<scan.cols; ++i)
			{
				cv::Vec2f pos = scan.at<cv::Vec2f>(i);
				double range = hypot(pos[0], pos[1]);
				if (range >= scanRangeMin && range <=scanRangeMax)
				{
					double angle = atan2(pos[1], pos[0]);
					if (angle >= msg.angle_min && angle <= msg.angle_max)
					{
						int index = (angle - msg.angle_min) / msg.angle_increment;
						if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0))
						{
							msg.ranges[index] = range;
						}
					}
				}
			}

			scanPub.publish(msg);
		}

		if(odom.data().userDataRaw().type() == CV_8SC1 &&
		   odom.data().userDataRaw().cols >= 7 && // including null str ending
		   odom.data().userDataRaw().rows == 1 &&
		   memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)
		{
			//GOAL format detected, remove it from the user data and send it as goal event
			std::string goalStr = (const char *)odom.data().userDataRaw().data;
			if(!goalStr.empty())
			{
				std::list<std::string> strs = uSplit(goalStr, ':');
				if(strs.size() == 2)
				{
					int goalId = atoi(strs.rbegin()->c_str());

					if(goalId > 0)
					{
						ROS_WARN("Goal %d detected, calling rtabmap's set_goal service!", goalId);
						rtabmap_ros::SetGoal setGoalSrv;
						setGoalSrv.request.node_id = goalId;
						setGoalSrv.request.node_label = "";
						if(!ros::service::call("set_goal", setGoalSrv))
						{
							ROS_ERROR("Can't call \"set_goal\" service");
						}
					}
				}
			}
		}

		ros::spinOnce();

		while(ros::ok() && paused)
		{
			uSleep(100);
			ros::spinOnce();
		}

		timer.restart();
		info = rtabmap::CameraInfo();
		data = reader.takeImage(&info);
		odom = rtabmap::OdometryEvent(data, info.odomPose, info.odomCovariance);
		acquisitionTime = timer.ticks();
	}


	return 0;
}