/* * 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); }
static struct nfulnl_instance * instance_create(u_int16_t group_num, int pid) { struct nfulnl_instance *inst; UDEBUG("entering (group_num=%u, pid=%d)\n", group_num, pid); write_lock_bh(&instances_lock); if (__instance_lookup(group_num)) { inst = NULL; UDEBUG("aborting, instance already exists\n"); goto out_unlock; } inst = kmalloc(sizeof(*inst), GFP_ATOMIC); if (!inst) goto out_unlock; memset(inst, 0, sizeof(*inst)); INIT_HLIST_NODE(&inst->hlist); inst->lock = SPIN_LOCK_UNLOCKED; /* needs to be two, since we _put() after creation */ atomic_set(&inst->use, 2); init_timer(&inst->timer); inst->timer.function = nfulnl_timer; inst->timer.data = (unsigned long)inst; /* don't start timer yet. (re)start it with every packet */ inst->peer_pid = pid; inst->group_num = group_num; inst->qthreshold = NFULNL_QTHRESH_DEFAULT; inst->flushtimeout = NFULNL_TIMEOUT_DEFAULT; inst->nlbufsiz = NFULNL_NLBUFSIZ_DEFAULT; inst->copy_mode = NFULNL_COPY_PACKET; inst->copy_range = 0xffff; if (!try_module_get(THIS_MODULE)) goto out_free; hlist_add_head(&inst->hlist, &instance_table[instance_hashfn(group_num)]); UDEBUG("newly added node: %p, next=%p\n", &inst->hlist, inst->hlist.next); write_unlock_bh(&instances_lock); return inst; out_free: instance_put(inst); out_unlock: write_unlock_bh(&instances_lock); return NULL; }
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 DBDriver::loadSignatures(const std::list<int> & signIds, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash) { UDEBUG(""); // look up in the trash before the database std::list<int> ids = signIds; std::list<Signature*>::iterator sIter; bool valueFound = false; _trashesMutex.lock(); { for(std::list<int>::iterator iter = ids.begin(); iter != ids.end();) { valueFound = false; for(std::map<int, Signature*>::iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end();) { if(sIter->first == *iter) { signatures.push_back(sIter->second); _trashSignatures.erase(sIter++); valueFound = true; break; } else { ++sIter; } } if(valueFound) { if(loadedFromTrash) { loadedFromTrash->insert(*iter); } iter = ids.erase(iter); } else { ++iter; } } } _trashesMutex.unlock(); UDEBUG(""); if(ids.size()) { _dbSafeAccessMutex.lock(); this->loadSignaturesQuery(ids, signatures); _dbSafeAccessMutex.unlock(); } }
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_destroy(struct nfulnl_instance *inst) { /* first pull it out of the global list */ UDEBUG("removing instance %p (queuenum=%u) from hash\n", inst, inst->group_num); hlist_del(&inst->hlist); /* then flush all pending packets from skb */ spin_lock_bh(&inst->lock); if (inst->skb) { /* timer "holds" one reference (we have one more) */ if (del_timer(&inst->timer)) instance_put(inst); 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); }
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; }
void Signature::addLink(const Link & link) { UDEBUG("Add link %d to %d (type=%d)", link.to(), this->id(), (int)link.type()); UASSERT(link.from() == this->id()); std::pair<std::map<int, Link>::iterator, bool> pair = _links.insert(std::make_pair(link.to(), link)); UASSERT_MSG(pair.second, uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str()); _linksModified = true; }
static void instance_put(struct nfulnl_instance *inst) { if (inst && atomic_dec_and_test(&inst->use)) { UDEBUG("kfree(inst=%p)\n", inst); kfree(inst); } }
void DiscreteFrustum::deserialize(std::istream& in, bool ascii) { if(ascii) { eigen_extensions::deserializeScalarASCII(in, &max_dist_); eigen_extensions::deserializeScalarASCII(in, &num_bins_); eigen_extensions::deserializeScalarASCII(in, &bin_depth_); eigen_extensions::deserializeASCII(in, &counts_); eigen_extensions::deserializeASCII(in, &total_numerators_); eigen_extensions::deserializeASCII(in, &total_denominators_); eigen_extensions::deserializeASCII(in, &multipliers_); } else { eigen_extensions::deserializeScalar(in, &max_dist_); eigen_extensions::deserializeScalar(in, &num_bins_); eigen_extensions::deserializeScalar(in, &bin_depth_); eigen_extensions::deserialize(in, &counts_); eigen_extensions::deserialize(in, &total_numerators_); eigen_extensions::deserialize(in, &total_denominators_); eigen_extensions::deserialize(in, &multipliers_); } UDEBUG("Frustum: max_dist=%f", max_dist_); UDEBUG("Frustum: num_bins=%d", num_bins_); UDEBUG("Frustum: bin_depth=%f", bin_depth_); UDEBUG("Frustum: counts=%d", counts_.rows()); UDEBUG("Frustum: total_numerators=%d", total_numerators_.rows()); UDEBUG("Frustum: total_denominators=%d", total_denominators_.rows()); UDEBUG("Frustum: multipliers=%d", multipliers_.rows()); }
void Signature::removeLink(int idTo) { int count = (int)_links.erase(idTo); if(count) { UDEBUG("Removed link %d from %d", idTo, this->id()); _linksModified = true; } }
void DiscreteDepthDistortionModel::deleteFrustums() { UDEBUG(""); for(size_t y = 0; y < frustums_.size(); ++y) for(size_t x = 0; x < frustums_[y].size(); ++x) if(frustums_[y][x]) delete frustums_[y][x]; training_samples_ = 0; }
CameraThread::~CameraThread() { UDEBUG(""); join(true); if(_camera) { delete _camera; } delete _stereoDense; }
OdometryThread::~OdometryThread() { this->unregisterFromEventsManager(); this->join(true); if(_odometry) { delete _odometry; } UDEBUG(""); }
void CameraThread::mainLoop() { UTimer timer; UDEBUG(""); SensorData data = _camera->takeImage(); if(!data.imageRaw().empty()) { if(_colorOnly && !data.depthRaw().empty()) { data.setDepthOrRightRaw(cv::Mat()); } if(_mirroring && data.cameraModels().size() == 1) { cv::Mat tmpRgb; cv::flip(data.imageRaw(), tmpRgb, 1); data.setImageRaw(tmpRgb); if(data.cameraModels()[0].cx()) { CameraModel tmpModel( data.cameraModels()[0].fx(), data.cameraModels()[0].fy(), float(data.imageRaw().cols) - data.cameraModels()[0].cx(), data.cameraModels()[0].cy(), data.cameraModels()[0].localTransform()); data.setCameraModel(tmpModel); } if(!data.depthRaw().empty()) { cv::Mat tmpDepth; cv::flip(data.depthRaw(), tmpDepth, 1); data.setDepthOrRightRaw(tmpDepth); } } if(_stereoToDepth && data.stereoCameraModel().isValid() && !data.rightRaw().empty()) { cv::Mat depth = util2d::depthFromDisparity( util2d::disparityFromStereoImages(data.imageRaw(), data.rightRaw()), data.stereoCameraModel().left().fx(), data.stereoCameraModel().baseline()); data.setCameraModel(data.stereoCameraModel().left()); data.setDepthOrRightRaw(depth); data.setStereoCameraModel(StereoCameraModel()); } this->post(new CameraEvent(data, _camera->getSerial())); } else if(!this->isKilled()) { UWARN("no more images..."); this->kill(); this->post(new CameraEvent()); } }
static void nfulnl_timer(unsigned long data) { struct nfulnl_instance *inst = (struct nfulnl_instance *)data; UDEBUG("timer function called, flushing buffer\n"); spin_lock_bh(&inst->lock); __nfulnl_send(inst); instance_put(inst); spin_unlock_bh(&inst->lock); }
void DBDriver::asyncSave(Signature * s) { if(s) { UDEBUG("s=%d", s->id()); _trashesMutex.lock(); { _trashSignatures.insert(std::pair<int, Signature*>(s->id(), s)); } _trashesMutex.unlock(); } }
void Signature::changeLinkIds(int idFrom, int idTo) { std::map<int, Link>::iterator iter = _links.find(idFrom); if(iter != _links.end()) { Link link = iter->second; _links.erase(iter); link.setTo(idTo); _links.insert(std::make_pair(idTo, link)); _linksModified = true; UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo); } }
void UAudioCaptureMic::mainLoopEnd() { UDEBUG(""); FMOD_RESULT result; FMOD_BOOL isRecording; result = UAudioSystem::isRecording(_driver, &isRecording); UASSERT_MSG(result==FMOD_OK, FMOD_ErrorString(result)); if(isRecording) { result = UAudioSystem::recordStop(_driver); UASSERT_MSG(result==FMOD_OK, FMOD_ErrorString(result)); } UAudioCapture::mainLoopEnd(); }