MultiCloudAndImageProducer(std::vector<std::shared_ptr<pcl::Grabber>> grabbers, size_t id) :
grabbers_ (grabbers),
id_ (id) {
provides_images_ = std::make_shared<std::vector<bool>>(grabbers_.size());
cloud_connections_ = std::make_shared<std::vector<boost::signals2::connection>>(grabbers_.size());
image_connections_ = std::make_shared<std::vector<boost::signals2::connection>>(grabbers_.size());
cloud_timers_ = std::make_shared<std::vector<std::shared_ptr<Timer>>>(grabbers_.size());
image_timers_ = std::make_shared<std::vector<std::shared_ptr<Timer>>>(grabbers_.size());
for(size_t idx = 0; idx < grabbers_.size(); ++idx) {
if(grabbers_.at(idx)->providesCallback<void (const typename pcl::PointCloud<PointType>::ConstPtr&)>()) {
boost::function<void (const typename pcl::PointCloud<PointType>::ConstPtr&)> cloud_callback =
boost::bind(&MultiCloudAndImageProducer::cloudCallback, this, _1, idx);
cloud_connections_->at(idx) = grabbers_.at(idx)->registerCallback (cloud_callback);
}
else
Logger::log(Logger::ERROR, "Cloud callback not provided by grabber %d\n", id);
if(grabbers_.at(idx)->providesCallback<void (const pcl::io::Image::Ptr&)>()) {
boost::function<void(const pcl::io::Image::Ptr &)> image_callback =
boost::bind(&MultiCloudAndImageProducer::imageCallback, this, _1, idx);
image_connections_->at(idx) = grabbers_.at(idx)->registerCallback(image_callback);
provides_images_->at(idx) = true;
}
else
Logger::log(Logger::ERROR, "Image callback not provided by grabber %d\n", id);
}
temp_buffer_element_ = std::make_shared<MultiCloudAndImage<PointType>>();
}
// OVERRIDE
void Hand::SortHandHighLow(std::shared_ptr<std::vector<Card>> c)
{
if (c->empty())
{
printf_s("Passing a null/void vector! Cannot sort the hand high to low");
return;
}
// Iterate throught the card's hand
bool swapped;
do
{
swapped = false;
for (unsigned int i = 0; i < c->size() - 1; ++i)
{
if (c->at(i).GetCardValue() < c->at(i + 1).GetCardValue())
{
SwapCards(c, i, i + 1);
swapped = true;
}
else
{
// Do nothing here
}
}
} while (swapped);
}
void mark(
size_t const s,
std::shared_ptr<std::vector<bool> >& marked,
bool const value,
VectorField const& V,
Incidences const& I)
{
std::queue<size_t> queue;
marked->at(s) = value;
queue.push(s);
while (not queue.empty())
{
size_t const a = queue.front();
queue.pop();
for (int i = 0; i < I.count(a); ++i)
{
size_t const b = I(a, i);
if (V.defined(b) and V(b) != a)
{
size_t const c = V(b);
if (c != b and marked->at(c) != value)
{
queue.push(c);
marked->at(b) = marked->at(c) = value;
}
}
}
}
}
void extract(
std::string const inpath,
std::string const outpath,
double const from,
double const to,
double const into)
{
namespace js = anu_am::json;
// Read the data.
NCFileInfo const info = readFileInfo(inpath);
Variable const var = findVolumeVariable(info);
std::string const name = var.name();
std::vector<size_t> dims = readDimensions(info);
size_t const n = dims.at(0) * dims.at(1) * dims.at(2);
std::shared_ptr<std::vector<T> > const data = readVolumeData<T>(inpath);
// Do the processing.
for (size_t k = 0; k < n; ++k)
{
T const val = data->at(k);
if (val >= from and val <= to)
data->at(k) = into;
}
// Generate metadata to include with the output data
std::string const parentID = guessDatasetID(inpath, info.attributes());
std::string const thisID = derivedID(parentID, name, "PM");
std::string const outfile =
outpath.size() > 0 ? outpath : (stripTimestamp(thisID) + ".nc");
js::Object const parameters = js::Object
("from", from)
("to" , to)
("into", into);
js::Object const fullSpec = js::Object
("id" , thisID)
("process" , "Phase Merge")
("sourcefile" , __FILE__)
("revision" , js::Object("id", GIT_REVISION)("date", GIT_TIMESTAMP))
("parent" , parentID)
("predecessors", js::Array(parentID))
("parameters" , parameters);
std::string const description = js::toString(fullSpec, 2);
// Write the results.
writeVolumeData(
data, outfile, name, dims.at(0), dims.at(1), dims.at(2),
VolumeWriteOptions()
.fileAttributes(inheritableAttributes(info.attributes()))
.datasetID(thisID)
.description(description));
}
void
MeshPose::setReferencePose(std::shared_ptr<std::vector<VertexData::Attr>> &vertexData)
{
m_ReferencePose.resize(vertexData->size());
for ( unsigned int i = 0 ; i < vertexData->size(); i++) {
m_ReferencePose[i].set(vertexData->at(i).x, vertexData->at(i).y, vertexData->at(i).z);
}
}
void start() {
for(size_t idx = 0; idx < grabbers_.size(); ++idx) {
std::stringstream ss;
ss << "MultiGrabber id: " << id_ << ", Camera : " << idx;
cloud_timers_->at(idx).reset(new Timer(ss.str() + "Cloud Callback"));
if (provides_images_)
image_timers_->at(idx).reset(new Timer(ss.str() + "Image Callback"));
grabbers_.at(idx)->start();
}
}
void stop() {
for(size_t idx = 0; idx < grabbers_.size(); ++idx) {
grabbers_.at(idx)->stop();
cloud_connections_->at(idx).disconnect();
if (provides_images_->at(idx))
image_connections_->at(idx).disconnect();
std::stringstream ss;
ss << "MultiGrabber id: " << id_ << ", Camera : " << idx << ", Producer done." << std::endl;
Logger::log(Logger::INFO, ss.str());
}
}
RPCMethod::ParameterError::Enum RPCMethod::checkParameters(std::shared_ptr<std::vector<BaseLib::PVariable>> parameters, std::vector<BaseLib::VariableType> types)
{
if(types.size() != parameters->size())
{
return RPCMethod::ParameterError::Enum::wrongCount;
}
for(uint32_t i = 0; i < types.size(); i++)
{
if(types.at(i) == BaseLib::VariableType::tVariant && parameters->at(i)->type != BaseLib::VariableType::tVoid) continue;
if(types.at(i) != parameters->at(i)->type) return RPCMethod::ParameterError::Enum::wrongType;
}
return RPCMethod::ParameterError::Enum::noError;
}
// OVERRIDE
void Hand::SwapCards(std::shared_ptr<std::vector<Card>> c, int pos1, int pos2)
{
// Create temp
std::vector<Card> temp;
// Store value of pos2 in temp vec: temp[POS2], HAND[POS1, POS2]
temp.push_back(c->at(pos2));
// Now take value of pos1 and place into pos2 of vector: temp[POS2], HAND[POS1, POS1]
c->at(pos2) = c->at(pos1);
// Transfer back temp into position 1: temp[POS2], HAND[POS2, POS1]
c->at(pos1) = temp.at(0);
}
void clean()
{
for (size_t i = 0; i < spreadsheet->size(); i++)
{
spreadsheet->at(i).clear();
spreadsheet->at(i).shrink_to_fit();
inputValues->at(i).clear();
inputValues->at(i).shrink_to_fit();
}
spreadsheet->clear();
spreadsheet->shrink_to_fit();
inputValues->clear();
inputValues->shrink_to_fit();
}
static int binary_search(std::shared_ptr< std::vector<T> >& vec, const int start, const int& end, const T& key){
if(start > end)
return -100;
const int middle = start + ((end - start) / 2);
if(vec->at(middle) == key)
return (int)middle;
else if(vec->at(middle) > key){
const int new_middle = middle - 1;
return binary_search(vec, start, new_middle, key);
};
return binary_search(vec, middle + 1, end, key);
};
void Road::initialize(const std::shared_ptr<InnerModel> &inner,
const std::shared_ptr<NavigationState> &state_,
const std::shared_ptr<RoboCompCommonBehavior::ParameterList> ¶ms)
{
innerModel = inner;
state = state_;
try{ robotname = params->at("RobotName").value;}
catch(const std::exception &e){ std::cout << e.what() << " No Robot name defined in config. Using default 'robot' " << std::endl;}
threshold = QString::fromStdString(params->at("ArrivalTolerance").value).toFloat();
MINIMUM_SAFETY_DISTANCE = QString::fromStdString(params->at("MinimumSafetyDistance").value).toFloat();
ROBOT_RADIUS = QString::fromStdString(params->at("RobotRadius").value).toFloat();
reset();
std::cout << __FUNCTION__ << "Road initialized correclty" << std::endl;
}
void SimpleField::apply(std::shared_ptr<Stone> s, int x, int y, int reverse, int angle)
{
Field::apply(s, x, y, reverse, angle);
#ifdef _DEBUG
if(!appliable(s, x, y, reverse, angle)) {
throw std::runtime_error("cannot apply");
}
#endif
for (int i = 0; i < 8; ++i) {
for (int j = 0; j < 8; ++j) {
if ( x + j < 0 || 32 <= x + j || y + i < 0 || 32 <= y + i) continue;
mat[y + i][x + j] |= s -> at(j, i, reverse, angle);
if (!s->at(j, i, reverse, angle)) continue;
for (int k = 0; k < 4; ++k) {
static const int ofs[4][2] = {
{0, 1},
{1, 0},
{-1, 0},
{0, -1}
};
int nx = x + j + ofs[k][0];
int ny = y + i + ofs[k][1];
if (nx < 0 || 32 <= nx || ny < 0 || 32 <= ny) {
continue;
}
ok[ny][nx] |= true;
}
}
}
value = -1;
is_first &= false;
}
void imageCallback(const pcl::io::Image::Ptr& image, size_t index) {
bool overwrite = false;
bool pushed = false;
{
std::unique_lock<std::mutex> lock(mutex_);
if (!temp_buffer_element_->isImageSetAt(index)) {
temp_buffer_element_->setImageAt(image, index);
if (isBufferElementFullySet()) {
overwrite = buffer_->pushBack(temp_buffer_element_);
pushed = true;
temp_buffer_element_ = std::make_shared<MultiCloudAndImage<PointType>>();
}
}
else {
overwrite = buffer_->pushBack(temp_buffer_element_);
pushed = true;
temp_buffer_element_ = std::make_shared<MultiCloudAndImage<PointType>>();
temp_buffer_element_->setImageAt(image, index);
}
}
if(pushed) {
if(overwrite)
Logger::log(Logger::WARN, "Warning! CloudAndImage Buffer was full, overwriting data!\n");
}
if(image_timers_->at(index)->time())
Logger::log(Logger::INFO, "MultiCloudImage Buffer size: %zd.\n", buffer_->getSize());
}
SYMBOL_DECLSPEC char* __stdcall mGetLinkItem(int k, double *top_x, double
*top_y, double *height, double *width, int *topage, int *type)
{
char* retstr = NULL;
if (k < 0 || k > gLinkResults->size())
return false;
sh_link muctx_link = gLinkResults->at(k);
*top_x = muctx_link->upper_left.X;
*top_y = muctx_link->upper_left.Y;
*width = muctx_link->lower_right.X - muctx_link->upper_left.X;
*height = muctx_link->lower_right.Y - muctx_link->upper_left.Y;
*topage = muctx_link->page_num;
*type = (int) muctx_link->type;
if (muctx_link->type == LINK_URI)
{
const char* str = muctx_link->uri.get();
int len = strlen(str);
if (len > 0)
{
/* This allocation ensures that Marshal will release in the managed code */
retstr = (char*)::CoTaskMemAlloc(len + 1);
strcpy(retstr, str);
}
muctx_link->uri.release();
}
return retstr;
}
std::shared_ptr<RPCHeader> RPCDecoder::decodeHeader(std::shared_ptr<std::vector<char>> packet)
{
std::shared_ptr<RPCHeader> header(new RPCHeader());
try
{
if(!(packet->at(3) & 0x40) || packet->size() < 12) return header;
uint32_t position = 4;
uint32_t headerSize = 0;
headerSize = _decoder.decodeInteger(packet, position);
if(headerSize < 4) return header;
uint32_t parameterCount = _decoder.decodeInteger(packet, position);
for(uint32_t i = 0; i < parameterCount; i++)
{
std::string field = _decoder.decodeString(packet, position);
HelperFunctions::toLower(field);
std::string value = _decoder.decodeString(packet, position);
if(field == "authorization") header->authorization = value;
}
}
catch(const std::exception& ex)
{
Output::printEx(__FILE__, __LINE__, __PRETTY_FUNCTION__, ex.what());
}
catch(Exception& ex)
{
Output::printEx(__FILE__, __LINE__, __PRETTY_FUNCTION__, ex.what());
}
catch(...)
{
Output::printEx(__FILE__, __LINE__, __PRETTY_FUNCTION__);
}
return header;
}
// pass by value
void printReversed(std::shared_ptr<StringPtr> v) {
std::cout << "Value reversed: ";
for(int i = v->len() - 1; i >= 0; --i) {
std::cout << v->at(i);
}
std::cout << "\n";
}
virtual void _Send_back_history(std::shared_ptr<protocol::Invoke> invoke, std::shared_ptr<slave::InvokeHistory> $history)
{
std::shared_ptr<PRInvokeHistory> history = std::dynamic_pointer_cast<PRInvokeHistory>($history);
if (history == nullptr)
return;
// REMOVE UID AND FIRST, LAST INDEXES
for (size_t i = invoke->size(); i < invoke->size(); i--)
{
const std::string &name = invoke->at(i)->getName();
if (name == "_History_uid" || name == "_Piece_first" || name == "_Piece_last")
invoke->erase(invoke->begin() + i);
}
// RE-SEND (DISTRIBUTE) THE PIECE TO OTHER SLAVES
std::thread
(
&base::ParallelSystemArrayBase::sendPieceData, (base::ParallelSystemArrayBase*)system_array_,
invoke, history->getFirst(), history->getLast()
).detach();
// ERASE FROM THE PROGRESS LIST
progress_list_.erase(history->getUID());
};
/**
* @brief Construct result, by packing process.
*/
void constructResult()
{
if (result.empty() == false)
return;
// 제품과 포장지 그룹, Product와 WrapperGroup의 1:1 매칭
for (size_t i = 0; i < size(); i++)
{
const std::shared_ptr<Wrapper> &wrapper = at(i);
if (result.count(wrapper->key()) == 0)
{
WrapperGroup *wrapperGroup = new WrapperGroup(wrapper);
result.insert({ wrapper->key(), std::shared_ptr<WrapperGroup>(wrapperGroup) });
}
std::shared_ptr<WrapperGroup> wrapperGroup = result.at(wrapper->key());
std::shared_ptr<Instance> instance = instanceArray->at(i);
if (wrapperGroup->allocate(instance) == false)
{
// 일개 제품 크기가 포장지보다 커서 포장할 수 없는 경우,
// 현재의 염기서열은 유효하지 못하여 폐기됨
valid = false;
return;
}
}
// 유효한 염기서열일 때,
for (auto it = result.begin(); it != result.end(); it++)
{
it->second->optimize(); // 세부적(그룹별)으로 bin-packing을 실시함
price += it->second->getPrice(); // 더불어 가격도 합산해둔다
}
valid = true;
};
void cloudCallback (const typename pcl::PointCloud<PointType>::ConstPtr& cloud, size_t index) {
bool overwrite = false;
bool pushed = false;
{
std::unique_lock<std::mutex> lock(mutex_);
if (!temp_buffer_element_->isCloudSetAt(index)) {
temp_buffer_element_->setCloudAt(cloud, index);
if(isBufferElementFullyEmpty())
first_set_stamp = cloud->header.stamp;
else if (isBufferElementFullySet() || isItTimeToPushElement(cloud->header.stamp)) {
overwrite = buffer_->pushBack(temp_buffer_element_);
pushed = true;
temp_buffer_element_ = std::make_shared<MultiCloudAndImage<PointType>>();
// first_set_stamp = cloud->header.stamp;
}
}
else {
overwrite = buffer_->pushBack(temp_buffer_element_);
pushed = true;
temp_buffer_element_ = std::make_shared<MultiCloudAndImage<PointType>>();
temp_buffer_element_->setCloudAt(cloud, index);
first_set_stamp = cloud->header.stamp;
}
}
if(pushed) {
if(overwrite)
Logger::log(Logger::WARN, "Warning! CloudAndImage Buffer was full, overwriting data!\n");
}
if(cloud_timers_->at(index)->time())
Logger::log(Logger::INFO, "MultiCloudImage Buffer size: %zd.\n", buffer_->getSize());
}
bool Gui::Update(const std::shared_ptr<pb::ImageArray> &frames, const ReferenceFrameId& id, std::vector<std::vector<cv::KeyPoint> >¤t_keypoints)
{
std::unique_lock<std::mutex> lock = lock_gui();
if(!lock)
{
ROS_ERROR("Can't update Gui");
return false;
}
// Reference frame
current_frame_id_ = id;
// Current images
current_frames_.clear();
if(frames)
{
for (int i = 0; i < frames->Size(); ++i) {
current_frames_.push_back(frames->at(i)->Mat().clone());
}
}
SlamFramePtr current_frame = map_->GetFramePtr(current_frame_id_);
if (!current_frame) {
return false;
}
size_t num_meas = current_frame->NumMeasurements();
ROS_INFO("Frame has %d measurements", num_meas);
std::vector<MultiViewMeasurement> cur_meas(
num_meas, MultiViewMeasurement(rig_.cameras.size()));
for (size_t zi = 0; zi < num_meas; ++zi) {
current_frame->GetMeasurement(zi, &cur_meas[zi]);
}
current_measurements_.swap(cur_meas);
current_keypoints_.swap(current_keypoints);
// Update track data structure
TrackInfoPtr pTrackInfo;
if(track_info_.size() < num_visible_time_steps_)
{
pTrackInfo = TrackInfoPtr(new TrackInfo());
}
else
{
pTrackInfo = track_info_.back();
track_info_.pop_back();
}
track_info_.push_front(pTrackInfo);
pTrackInfo->Update(current_measurements_, current_keypoints_, current_frame_id_);
if(!init_)
init_=true;
return true;
}
void js_enter_game_test(Character *actor, Character *self, std::shared_ptr< std::vector<JSTrigger*> > js_scripts)
{
if (!js_scripts || js_scripts->size() == 0)
return;
if( (self && self->IsPurged()) || actor->IsPurged() )
return;
for (int i = 0; i < js_scripts->size(); ++i)
{
if (js_scripts->at(i)->isFlagged(JS::CHARACTER_LOGIN))
{
JSTrigger* trig = js_scripts->at(i);
if( (!self || is_allowed( self, actor, trig )) ) {
JSManager::get()->execute(trig, self, actor);
}
}
}
}
int count(size_t const id) const
{
int n = 0;
for (int i = 0; i < I_.count(id); ++i)
if (good_->at(I_(id, i)))
++n;
return n;
}
size_t operator()(size_t const id, int const n) const
{
int m = -1;
for (int i = 0; i < I_.count(id); ++i)
if (good_->at(I_(id, i)) and ++m == n)
return I_(id, i);
assert(false);
}
void Hand::AddToHand(std::shared_ptr<std::vector<Card>> c)
{
// Resize hand to size of C
SetSize(c->size());
// Check needs to make sure we aren't going over hand size of the game
for (unsigned int i = 0; i < c->size(); ++i)
{
m_vHand.at(i) = c->at(i);
}
}
void Evaluator::evaluate(std::shared_ptr<Population> pop,
std::shared_ptr<const Problem> problem,
std::shared_ptr<Statistics> statistics) const
{
auto size = pop->population_size();
for (decltype(size) idx = 0; idx < size; ++idx) {
problem->evaluate(pop->at(idx));
statistics->increase_number_of_fitness_evaluations();
}
}
bool SimpleField::appliable(std::shared_ptr<Stone> s, int x, int y, int reverse, int angle) const
{
bool adjf = false;
for (int i = 0; i < 8; ++i) {
for (int j = 0; j < 8; ++j) {
if (y + i < 0 || 32 <= y + i || x + j < 0 || 32 <= x + j) {
if (s->at(j, i, reverse, angle)) {
return false;
}
continue;
}
if (mat[y + i][x + j] && s->at(j, i, reverse, angle)) {
return false;
}
adjf |= s->at(j, i, reverse, angle) && (is_first || ok[y + i][x + j]);
}
}
return adjf;
}
void WReconstruction3D::work(std::shared_ptr<std::vector<Datum3D>>& datumsPtr)
{
// User's post-processing (after OpenPose processing & before OpenPose outputs) here
// datum.cvOutputData: rendered frame with pose or heatmaps
// datum.poseKeypoints: Array<float> with the estimated pose
try
{
// Profiling speed
const auto profilerKey = Profiler::timerInit(__LINE__, __FUNCTION__, __FILE__);
if (datumsPtr != nullptr && !datumsPtr->empty())
{
std::vector<Array<float>> poseKeypointVector;
std::vector<Array<float>> faceKeypointVector;
std::vector<Array<float>> leftHandKeypointVector;
std::vector<Array<float>> rightHandKeypointVector;
for (auto& datumsElement : *datumsPtr)
{
poseKeypointVector.emplace_back(datumsElement.poseKeypoints);
faceKeypointVector.emplace_back(datumsElement.faceKeypoints);
leftHandKeypointVector.emplace_back(datumsElement.handKeypoints[0]);
rightHandKeypointVector.emplace_back(datumsElement.handKeypoints[1]);
}
// Pose 3-D reconstruction
reconstructArray(datumsPtr->at(0).poseKeypoints3D, poseKeypointVector);
// Face 3-D reconstruction
reconstructArray(datumsPtr->at(0).faceKeypoints3D, faceKeypointVector);
// Left hand 3-D reconstruction
reconstructArray(datumsPtr->at(0).leftHandKeypoints3D, leftHandKeypointVector);
// Right hand 3-D reconstruction
reconstructArray(datumsPtr->at(0).rightHandKeypoints3D, rightHandKeypointVector);
// Profiling speed
Profiler::timerEnd(profilerKey);
Profiler::printAveragedTimeMsOnIterationX(profilerKey, __LINE__, __FUNCTION__, __FILE__);
}
}
catch (const std::exception& e)
{
log("Some kind of unexpected error happened.");
this->stop();
error(e.what(), __LINE__, __FUNCTION__, __FILE__);
}
}
void js_extraction_test( JSBindable *self, std::shared_ptr< std::vector<JSTrigger*> > js_scripts )
{
JSTrigger *trig;
if( !js_scripts || js_scripts->empty() )
return;
for(int i = 0;i < js_scripts->size();i++)
{
if (js_scripts->at(i)->isFlagged(JS::EXTRACTION))
{
trig = js_scripts->at(i);
if (randomly_triggered(trig->narg))
{
int ret_val = JSManager::get()->execute(trig, self, 0);
if( ret_val == 0 )
return;
}
}
}
}
void set_value(const std::shared_ptr<pj::object> &cfg, const char *part, const char* key, const pj::value &val)
{
if (cfg->find(part) == cfg->end())
{
pj::object temp;
cfg->insert({ part, pj::value(temp) });
}
pj::object o = cfg->at(part).get<pj::object>();
o[key] = val;
(*cfg.get())[part] = pj::value(o);
}
