C++ UDEBUG函数代码示例

/*
 * union_mkdir(struct vnode *a_dvp, struct vnode **a_vpp,
 *		struct componentname *a_cnp, struct vattr *a_vap)
 */
static int
union_mkdir(struct vop_old_mkdir_args *ap)
{
	struct union_node *dun = VTOUNION(ap->a_dvp);
	struct componentname *cnp = ap->a_cnp;
	struct thread *td = cnp->cn_td;
	struct vnode *upperdvp;
	int error = EROFS;

	if ((upperdvp = union_lock_upper(dun, td)) != NULLVP) {
		struct vnode *vp;

		error = VOP_MKDIR(upperdvp, &vp, cnp, ap->a_vap);
		union_unlock_upper(upperdvp, td);

		if (error == 0) {
			vn_unlock(vp);
			UDEBUG(("ALLOCVP-2 FROM %p REFS %d\n", vp, vp->v_sysref.refcnt));
			error = union_allocvp(ap->a_vpp, ap->a_dvp->v_mount,
				ap->a_dvp, NULLVP, cnp, vp, NULLVP, 1);
			UDEBUG(("ALLOCVP-2B FROM %p REFS %d\n", *ap->a_vpp, vp->v_sysref.refcnt));
		}
	}
	return (error);
}

/*
 * 	union_create:
 *
 * a_dvp is locked on entry and remains locked on return.  a_vpp is returned
 * locked if no error occurs, otherwise it is garbage.
 *
 * union_create(struct vnode *a_dvp, struct vnode **a_vpp,
 *		struct componentname *a_cnp, struct vattr *a_vap)
 */
static int
union_create(struct vop_old_create_args *ap)
{
	struct union_node *dun = VTOUNION(ap->a_dvp);
	struct componentname *cnp = ap->a_cnp;
	struct thread *td = cnp->cn_td;
	struct vnode *dvp;
	int error = EROFS;

	if ((dvp = union_lock_upper(dun, td)) != NULL) {
		struct vnode *vp;
		struct mount *mp;

		error = VOP_CREATE(dvp, &vp, cnp, ap->a_vap);
		if (error == 0) {
			mp = ap->a_dvp->v_mount;
			vn_unlock(vp);
			UDEBUG(("ALLOCVP-1 FROM %p REFS %d\n", vp, vp->v_sysref.refcnt));
			error = union_allocvp(ap->a_vpp, mp, NULLVP, NULLVP,
				cnp, vp, NULLVP, 1);
			UDEBUG(("ALLOCVP-2B FROM %p REFS %d\n", *ap->a_vpp, vp->v_sysref.refcnt));
		}
		union_unlock_upper(dvp, td);
	}
	return (error);
}

void DBDriver::closeConnection()
{
	UDEBUG("isRunning=%d", this->isRunning());
	this->join(true);
	UDEBUG("");
	this->emptyTrashes();
	_dbSafeAccessMutex.lock();
	this->disconnectDatabaseQuery();
	_dbSafeAccessMutex.unlock();
	UDEBUG("");
}

static void
add_myself(void)
{
	static const char myself[] = "/proc/self/exe";
	static const char moddir[] = PKGLIBDIR;
	static const char eprefix[] = "${exec_prefix}";
	static const char prefix[] = "${prefix}";
	const char *relmoddir;
	char wd[PATH_MAX], *dp;
	size_t sz;

	sz = readlink(myself, wd, sizeof(wd));
	wd[sz] = '\0';
	if ((dp = strrchr(wd, '/')) == NULL) {
		return;
	}
	/* add the path where the binary resides */
	*dp = '\0';
	UDEBUG("adding %s\n", wd);
	lt_dladdsearchdir(wd);

#define MEMCMPLIT(a, b)	memcmp((a), (b), sizeof(b) - 1)
	if (moddir[0] == '/') {
		/* absolute libdir, add him */
		lt_dladdsearchdir(moddir);
	} else if (moddir[0] == '.') {
		/* relative libdir? relative to what? */
		return;
	} else if (memcmp(moddir, eprefix, sizeof(eprefix) - 1) == 0) {
		/* take the bit after EPREFIX for catting later on */
		relmoddir = moddir + sizeof(eprefix) - 1;
	} else if (memcmp(moddir, prefix, sizeof(prefix) - 1) == 0) {
		/* take the bit after PREFIX for catting later on */
		relmoddir = moddir + sizeof(prefix) - 1;
	} else {
		/* don't know, i guess i'll leave ya to it */
		return;
	}

	/* go back one level in dp */
	if ((dp = strrchr(wd, '/')) == NULL) {
		return;
	} else if (strcmp(dp, "/bin") && strcmp(dp, "/sbin")) {
		/* dp doesn't end in /bin nor /sbin */
		return;
	}

	/* good, now we're ready to cat relmoddir to dp */
	strncpy(dp, relmoddir, sizeof(wd) - (dp - wd));
	UDEBUG("adding %s\n", wd);
	lt_dladdsearchdir(wd);
	return;
}

    void DiscreteDepthDistortionModel::deserialize(std::istream& in, bool ascii)
    {
      UDEBUG("");
      string buf;
      getline(in, buf);
      UDEBUG("buf=%s", buf.c_str());
      assert(buf == "DiscreteDepthDistortionModel v01");
      if(ascii)
      {
    	  eigen_extensions::deserializeScalarASCII(in, &width_);
		  eigen_extensions::deserializeScalarASCII(in, &height_);
		  eigen_extensions::deserializeScalarASCII(in, &bin_width_);
		  eigen_extensions::deserializeScalarASCII(in, &bin_height_);
		  eigen_extensions::deserializeScalarASCII(in, &bin_depth_);
		  eigen_extensions::deserializeScalarASCII(in, &num_bins_x_);
		  eigen_extensions::deserializeScalarASCII(in, &num_bins_y_);
		  eigen_extensions::deserializeScalarASCII(in, &training_samples_);
      }
      else
      {
		  eigen_extensions::deserializeScalar(in, &width_);
		  eigen_extensions::deserializeScalar(in, &height_);
		  eigen_extensions::deserializeScalar(in, &bin_width_);
		  eigen_extensions::deserializeScalar(in, &bin_height_);
		  eigen_extensions::deserializeScalar(in, &bin_depth_);
		  eigen_extensions::deserializeScalar(in, &num_bins_x_);
		  eigen_extensions::deserializeScalar(in, &num_bins_y_);
		  eigen_extensions::deserializeScalar(in, &training_samples_);
      }
      UINFO("Distortion Model: width=%d", width_);
      UINFO("Distortion Model: height=%d", height_);
      UINFO("Distortion Model: bin_width=%d", bin_width_);
      UINFO("Distortion Model: bin_height=%d", bin_height_);
      UINFO("Distortion Model: bin_depth=%f", bin_depth_);
      UINFO("Distortion Model: num_bins_x=%d", num_bins_x_);
      UINFO("Distortion Model: num_bins_y=%d", num_bins_y_);
      UINFO("Distortion Model: training_samples=%d", training_samples_);
      deleteFrustums();
      frustums_.resize(num_bins_y_);
      for(size_t y = 0; y < frustums_.size(); ++y) {
        frustums_[y].resize(num_bins_x_, NULL);
        for(size_t x = 0; x < frustums_[y].size(); ++x) {
        	UDEBUG("Distortion Model: Frustum[%d][%d]", y, x);
          frustums_[y][x] = new DiscreteFrustum;
          frustums_[y][x]->deserialize(in, ascii);
        }
      }
      UDEBUG("");
    }

std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
		const cv::Mat & leftImage,
		const cv::Mat & rightImage,
		const std::vector<cv::Point2f> & leftCorners,
		std::vector<unsigned char> & status) const
{
	std::vector<cv::Point2f> rightCorners;
	UDEBUG("util2d::calcOpticalFlowPyrLKStereo() begin");
	std::vector<float> err;
	util2d::calcOpticalFlowPyrLKStereo(
			leftImage,
			rightImage,
			leftCorners,
			rightCorners,
			status,
			err,
			this->winSize(),
			this->maxLevel(),
			cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, this->iterations(), epsilon_),
			cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
	UDEBUG("util2d::calcOpticalFlowPyrLKStereo() end");
	UASSERT(leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size());
	int countFlowRejected = 0;
	int countDisparityRejected = 0;
	for(unsigned int i=0; i<status.size(); ++i)
	{
		if(status[i]!=0)
		{
			float disparity = leftCorners[i].x - rightCorners[i].x;
			if(disparity < float(this->minDisparity()) || disparity > float(this->maxDisparity()))
			{
				status[i] = 0;
				++countDisparityRejected;
			}
		}
		else
		{
			++countFlowRejected;
		}
	}
	UDEBUG("total=%d countFlowRejected=%d countDisparityRejected=%d", (int)status.size(), countFlowRejected, countDisparityRejected);

	if(countFlowRejected + countDisparityRejected > (int)status.size()/2)
	{
		UWARN("A large number (%d/%d) of stereo correspondences are rejected! Optical flow may have failed, images are not calibrated or the background is too far (no disparity between the images).", countFlowRejected+countDisparityRejected, (int)status.size());
	}

	return rightCorners;
}

SensorData Camera::takeImage(CameraInfo * info)
{
	bool warnFrameRateTooHigh = false;
	float actualFrameRate = 0;
	if(_imageRate>0)
	{
		int sleepTime = (1000.0f/_imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
		if(sleepTime > 2)
		{
			uSleep(sleepTime-2);
		}
		else if(sleepTime < 0)
		{
			warnFrameRateTooHigh = true;
			actualFrameRate = 1.0/(_frameRateTimer->getElapsedTime());
		}

		// Add precision at the cost of a small overhead
		while(_frameRateTimer->getElapsedTime() < 1.0/double(_imageRate)-0.000001)
		{
			//
		}

		double slept = _frameRateTimer->getElapsedTime();
		_frameRateTimer->start();
		UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_imageRate));
	}

	UTimer timer;
	SensorData data  = this->captureImage(info);
	double captureTime = timer.ticks();
	if(warnFrameRateTooHigh)
	{
		UWARN("Camera: Cannot reach target image rate %f Hz, current rate is %f Hz and capture time = %f s.",
				_imageRate, actualFrameRate, captureTime);
	}
	else
	{
		UDEBUG("Time capturing image = %fs", captureTime);
	}
	if(info)
	{
		info->id = data.id();
		info->stamp = data.stamp();
		info->timeCapture = captureTime;
	}
	return data;
}

void Odometry::updateKalmanFilter(float & vx, float & vy, float & vz, float & vroll, float & vpitch, float & vyaw)
{
	// Set measurement to predict
	cv::Mat measurements;
	if(!_force3DoF)
	{
		measurements = cv::Mat(6,1,CV_32FC1);
		measurements.at<float>(0) = vx;     // x'
		measurements.at<float>(1) = vy;     // y'
		measurements.at<float>(2) = vz;     // z'
		measurements.at<float>(3) = vroll;  // roll'
		measurements.at<float>(4) = vpitch; // pitch'
		measurements.at<float>(5) = vyaw;   // yaw'
	}
	else
	{
		measurements = cv::Mat(3,1,CV_32FC1);
		measurements.at<float>(0) = vx;     // x'
		measurements.at<float>(1) = vy;     // y'
		measurements.at<float>(2) = vyaw;   // yaw',
	}

	// The "correct" phase that is going to use the predicted value and our measurement
	UDEBUG("Correct");
	const cv::Mat & estimated = kalmanFilter_.correct(measurements);


	vx = estimated.at<float>(3);                      // x'
	vy = estimated.at<float>(4);                      // y'
	vz = _force3DoF?0.0f:estimated.at<float>(5);      // z'
	vroll = _force3DoF?0.0f:estimated.at<float>(12);  // roll'
	vpitch = _force3DoF?0.0f:estimated.at<float>(13); // pitch'
	vyaw = estimated.at<float>(_force3DoF?7:14);      // yaw'
}

void CameraThread::mainLoopKill()
{
	UDEBUG("");
	if(dynamic_cast<CameraFreenect2*>(_camera) != 0)
	{
		int i=20;
		while(i-->0)
		{
			uSleep(100);
			if(!this->isKilled())
			{
				break;
			}
		}
		if(this->isKilled())
		{
			//still in killed state, maybe a deadlock
			UERROR("CameraFreenect2: Failed to kill normally the Freenect2 driver! The thread is locked "
				   "on waitForNewFrame() method of libfreenect2. This maybe caused by not linking on the right libusb. "
				   "Note that rtabmap should link on libusb of libfreenect2. "
				   "Tip before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
		}

	}
}

int Settings::getHomographyMethod()
{
	int method = cv::RANSAC;
	QString str = getHomography_method();
	QStringList split = str.split(':');
	if(split.size()==2)
	{
		bool ok = false;
		int index = split.first().toInt(&ok);
		if(ok)
		{
			QStringList strategies = split.last().split(';');
			if(strategies.size() == 2 && index>=0 && index<2)
			{
				switch(method)
				{
				case 0:
					method = cv::LMEDS;
					break;
				default:
					method = cv::RANSAC;
					break;
				}
			}
		}
	}
	UDEBUG("method=%d", method);
	return method;
}

void DataRecorder::addData(const rtabmap::SensorData & data, const Transform & pose, const cv::Mat & covariance)
{
	memoryMutex_.lock();
	if(memory_)
	{
		if(memory_->getStMem().size() == 0 && data.id() > 0)
		{
			ParametersMap customParameters;
			customParameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false")); // use id from data
			memory_->parseParameters(customParameters);
		}

		//save to database
		UTimer time;
		memory_->update(data, pose, covariance);
		const Signature * s = memory_->getLastWorkingSignature();
		totalSizeKB_ += (int)s->sensorData().imageCompressed().total()/1000;
		totalSizeKB_ += (int)s->sensorData().depthOrRightCompressed().total()/1000;
		totalSizeKB_ += (int)s->sensorData().laserScanCompressed().total()/1000;
		memory_->cleanup();

		if(++count_ % 30)
		{
			memory_->emptyTrash();
		}
		UDEBUG("Time to process a message = %f s", time.ticks());
	}
	memoryMutex_.unlock();
}

bool CameraImages::init()
{
	UDEBUG("");
	if(_dir)
	{
		_dir->setPath(_path, "jpg ppm png bmp pnm");
	}
	else
	{
		_dir = new UDirectory(_path, "jpg ppm png bmp pnm");
	}
	_count = 0;
	if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/')
	{
		_path.append("/");
	}
	if(!_dir->isValid())
	{
		ULOGGER_ERROR("Directory path is not valid \"%s\"", _path.c_str());
	}
	else if(_dir->getFileNames().size() == 0)
	{
		UWARN("Directory is empty \"%s\"", _path.c_str());
	}
	return _dir->isValid();
}

static int
__nfulnl_send(struct nfulnl_instance *inst)
{
	int status;

	if (timer_pending(&inst->timer))
		del_timer(&inst->timer);

	if (!inst->skb)
		return 0;

	if (inst->qlen > 1)
		inst->lastnlh->nlmsg_type = NLMSG_DONE;

	status = nfnetlink_unicast(inst->skb, inst->peer_pid, MSG_DONTWAIT);
	if (status < 0) {
		UDEBUG("netlink_unicast() failed\n");
		/* FIXME: statistics */
	}

	inst->qlen = 0;
	inst->skb = NULL;
	inst->lastnlh = NULL;

	return status;
}

static void
_instance_destroy2(struct nfulnl_instance *inst, int lock)
{
	/* first pull it out of the global list */
	if (lock)
		write_lock_bh(&instances_lock);

	UDEBUG("removing instance %p (queuenum=%u) from hash\n",
		inst, inst->group_num);

	hlist_del(&inst->hlist);

	if (lock)
		write_unlock_bh(&instances_lock);

	/* then flush all pending packets from skb */

	spin_lock_bh(&inst->lock);
	if (inst->skb) {
		if (inst->qlen)
			__nfulnl_send(inst);
		if (inst->skb) {
			kfree_skb(inst->skb);
			inst->skb = NULL;
		}
	}
	spin_unlock_bh(&inst->lock);

	/* and finally put the refcount */
	instance_put(inst);

	module_put(THIS_MODULE);
}

static struct sk_buff *nfulnl_alloc_skb(unsigned int inst_size, 
					unsigned int pkt_size)
{
	struct sk_buff *skb;
	unsigned int n;

	UDEBUG("entered (%u, %u)\n", inst_size, pkt_size);

	/* alloc skb which should be big enough for a whole multipart
	 * message.  WARNING: has to be <= 128k due to slab restrictions */

	n = max(inst_size, pkt_size);
	skb = alloc_skb(n, GFP_ATOMIC);
	if (!skb) {
		PRINTR("nfnetlink_log: can't alloc whole buffer (%u bytes)\n",
			inst_size);

		if (n > pkt_size) {
			/* try to allocate only as much as we need for current
			 * packet */

			skb = alloc_skb(pkt_size, GFP_ATOMIC);
			if (!skb)
				PRINTR("nfnetlink_log: can't even alloc %u "
				       "bytes\n", pkt_size);
		}
	}

	return skb;
}