void LoadEstimator::publishMarkers()
{
setMarker(inertial_parameters_msg_.cog.x, inertial_parameters_msg_.cog.y, inertial_parameters_msg_.cog.z, cog_marker_);
cog_marker_pub_.publish(cog_marker_);
setMarker(inertial_parameters_msg_.hand_cog.x, inertial_parameters_msg_.hand_cog.y, inertial_parameters_msg_.hand_cog.z, cog_hand_marker_);
cog_marker_pub_.publish(cog_hand_marker_);
setMarker(inertial_parameters_msg_.obj_cog.x, inertial_parameters_msg_.obj_cog.y, inertial_parameters_msg_.obj_cog.z, cog_obj_marker_);
cog_marker_pub_.publish(cog_obj_marker_);
}
void TextField::backspace(g_key_info& info) {
if (text.length() > 0) {
g_range selected = getSelectedRange();
int leftcut = selected.getFirst();
if (info.alt) {
leftcut = cursorMoveStrategy->calculateSkip(text, leftcut, CursorDirection::LEFT);
} else if (info.ctrl) {
leftcut = 0;
}
int rightcut = selected.getLast();
if (rightcut - leftcut == 0) {
leftcut--;
}
if (leftcut >= 0 && rightcut <= text.length()) {
std::string beforeCursor = text.substr(0, leftcut);
std::string afterCursor = text.substr(rightcut);
text = beforeCursor + afterCursor;
setCursor(leftcut);
setMarker(leftcut);
markFor(COMPONENT_REQUIREMENT_UPDATE);
}
}
}
void MarkerList::ValueTreeWrapper::readFrom (const MarkerList& markerList, UndoManager* undoManager)
{
state.removeAllChildren (undoManager);
for (int i = 0; i < markerList.getNumMarkers(); ++i)
setMarker (*markerList.getMarker(i), undoManager);
}
modASM_Remove::modASM_Remove(const std::string& newName, const std::string& newChannelName,
const CE_DLL* newDLLPtr):
BaseTrafficHandler(newName, newChannelName, newDLLPtr),
_marker(CEcfg::instance()->getOrAddArray(cfgKey("markerPattern"))),
_expectedUnitLength(256),
_expectedUnitLengthSetting(CEcfg::instance()->getOrAddInt(cfgKey("expectedUnitLength"), _expectedUnitLength)),
_allowedMarkerBitErrors(0),
_allowedMarkerBitErrorsSetting(CEcfg::instance()->getOrAddInt(cfgKey("allowedMarkerBitErrors"), 0)),
_rebuildMarker(true),
_currentBitErrors(0),
_asmCount(0),
_asmValidCount(0),
_asmMissedCount(0),
_asmSearchCount(0),
_asmDiscoveredCount(0),
_asmBitErrorsAllowed(0),
_asmBitErrorsRejected(0),
_asmAllowedWithBitErrorsCount(0),
_asmRejectedWithBitErrorsCount(0),
_asmPartialMismatch(0) {
if ( _marker.getLength() == 0 ) {
uint8_t defaultPattern[] = { 0x1A, 0xCF, 0xFC, 0x1D };
setMarker(defaultPattern, 4);
}
_allowedMarkerBitErrors = _allowedMarkerBitErrorsSetting;
_expectedUnitLength = _expectedUnitLengthSetting;
MOD_DEBUG("Initializing with a %d-octet sync marker.", _marker.getLength());
}
DSPCOMPLEX throb::doDecode (DSPCOMPLEX z) {
static int cnt = 0;
// shift down to 0 +- 32 (64) Hz
// and lowpass
if (--cnt < 0) {
cnt = sampleRate / 10;
setMarker (throbIF);
}
z = z * localOscillator -> nextValue (throbIF);
z = fft_filter -> Pass (z);
// downRate to 250 and push to the receiver */
if (++deccntr >= downRate) {
rxcntr -= 1.0;
ourScope -> addElement (z);
// do symbol sync
throb_sync (z);
// decode
throb_rx (z);
symptr = (symptr + 1) % rxsymlen;
deccntr = 0;
}
return z;
}
QgsCentroidFillSymbolLayerV2Widget::QgsCentroidFillSymbolLayerV2Widget( const QgsVectorLayer* vl, QWidget* parent )
: QgsSymbolLayerV2Widget( parent, vl )
{
mLayer = NULL;
setupUi( this );
connect( btnChangeMarker, SIGNAL( clicked() ), this, SLOT( setMarker() ) );
}
TITANIUM_FUNCTION(ListView, setMarker)
{
if (arguments.size() >= 1) {
const auto _0 = arguments.at(0);
TITANIUM_ASSERT(_0.IsObject());
setMarker(js_to_ListViewMarkerProps(static_cast<JSObject>(_0)));
}
return this_object.get_context().CreateUndefined();
}
//Method to Move Between Players
void TicTacToe::PlayerMovement()
{
do
{
for (int i = 0; i < currentPlayer; i++)
{
cout << endl;
int selectROW;
int selectCOL;
int counter1 = 0;
do
{
cout << "@" << players[i].getHandle() << ": Please enter the row and column "
<< "for the space you want to mark: ";
cin >> selectROW >> selectCOL;
counter1++;
} while (!setMarker(selectROW, selectCOL, players[i].getMarker()));
if((WinnerCheck(players[i].getMarker())))
{
if(i == 0)
{
Player::IncrementWinsP1();
totalWins++;
displayBoard();
WinnerMessage(players[i].getHandle());
NoWinner = false;
break;
}
else if(i == 1)
{
Player::IncrementWinsP2();
totalWins++;
displayBoard();
WinnerMessage(players[i].getHandle());
NoWinner = false;
break;
}
}
else if ((TieCheck(players[i].getMarker())))
{
TieMessage();
ties++;
NoWinner = false;
displayBoard();
break;
}
else
{
displayBoard();
}
}
} while (NoWinner);
}
void TraceBuilder::popBlock() {
assert(!m_savedBlocks.empty());
auto const& top = m_savedBlocks.back();
FTRACE(2, "TraceBuilder popping {}@{} to restore {}@{}\n",
m_curBlock, m_state.marker().show(), top.block, top.marker.show());
m_curBlock = top.block;
setMarker(top.marker);
m_curWhere = top.where;
m_savedBlocks.pop_back();
}
void TraceBuilder::popTrace() {
assert(!m_savedTraces.empty());
auto const& top = m_savedTraces.top();
FTRACE(2, "TraceBuilder popping {}@{} to restore {}@{}\n",
m_curTrace, m_curMarker.show(), top.trace, top.marker.show());
m_curTrace = top.trace;
m_curBlock = top.block;
setMarker(top.marker);
m_curWhere = top.where;
m_savedTraces.pop();
}
void TraceBuilder::pushTrace(IRTrace* t, BCMarker marker, Block* b,
const boost::optional<Block::iterator>& where) {
FTRACE(2, "TraceBuilder saving {}@{} and using {}@{}\n",
m_curTrace, m_state.marker().show(), t, marker.show());
assert(t);
assert(bool(b) == bool(where));
assert(IMPLIES(b, b->trace() == t));
m_savedTraces.push(
TraceState{ m_curTrace, m_curBlock, m_state.marker(), m_curWhere });
m_curTrace = t;
m_curBlock = b;
setMarker(marker);
m_curWhere = where;
}
void Form::showCoordinates(double east, double north, bool saveMarker)
{
qDebug() << "Form, showCoordinates" << east << north;
QString str =
QString("var newLoc = new google.maps.LatLng(%1, %2); ").arg(north).arg(east) +
QString("map.setCenter(newLoc);") +
QString("map.setZoom(%1);").arg(ui->zoomSpinBox->value());
qDebug() << str;
ui->webView->page()->currentFrame()->documentElement().evaluateJavaScript(str);
if (saveMarker)
setMarker(east, north, ui->lePostalAddress->text());
}
QgsMarkerLineSymbolLayerV2Widget::QgsMarkerLineSymbolLayerV2Widget( const QgsVectorLayer* vl, QWidget* parent )
: QgsSymbolLayerV2Widget( parent, vl )
{
mLayer = NULL;
setupUi( this );
connect( spinInterval, SIGNAL( valueChanged( double ) ), this, SLOT( setInterval( double ) ) );
connect( btnChangeMarker, SIGNAL( clicked() ), this, SLOT( setMarker() ) );
connect( chkRotateMarker, SIGNAL( clicked() ), this, SLOT( setRotate() ) );
connect( spinOffset, SIGNAL( valueChanged( double ) ), this, SLOT( setOffset() ) );
connect( radInterval, SIGNAL( clicked() ), this, SLOT( setPlacement() ) );
connect( radVertex, SIGNAL( clicked() ), this, SLOT( setPlacement() ) );
connect( radVertexLast, SIGNAL( clicked() ), this, SLOT( setPlacement() ) );
connect( radVertexFirst, SIGNAL( clicked() ), this, SLOT( setPlacement() ) );
connect( radCentralPoint, SIGNAL( clicked() ), this, SLOT( setPlacement() ) );
}
void TraceBuilder::pushBlock(BCMarker marker, Block* b,
const boost::optional<Block::iterator>& where) {
FTRACE(2, "TraceBuilder saving {}@{} and using {}@{}\n",
m_curBlock, m_state.marker().show(), b, marker.show());
assert(b);
m_savedBlocks.push_back(
BlockState{ m_curBlock, m_state.marker(), m_curWhere });
m_curBlock = b;
setMarker(marker);
m_curWhere = where ? where : b->end();
if (do_assert) {
for (UNUSED auto const& state : m_savedBlocks) {
assert(state.block != b &&
"Can't push a block that's already in the saved stack");
}
}
}
void TextField::insert(std::string ins) {
g_range selected = getSelectedRange();
int first = selected.getFirst();
int last = selected.getLast();
if (first < 0) {
first = 0;
}
if (last > text.size()) {
last = text.size();
}
std::string beforeCursor = text.substr(0, first);
std::string afterCursor = text.substr(last);
text = beforeCursor + ins + afterCursor;
setCursor(first + ins.length());
setMarker(first + ins.length());
markFor(COMPONENT_REQUIREMENT_UPDATE);
}
void LineSpec::setSpec(QString arg)
{
if (spec() == arg) return;
int lastInd = arg.length() - 1;
if (markers().contains(arg.mid(lastInd, 1))) {
setMarker(arg.mid(lastInd, 1));
lastInd--;
}
if (colorMap.contains(arg.mid(lastInd, 1))) {
setColor(colorMap[arg.mid(lastInd, 1)]);
lastInd--;
}
QString style = arg.left(lastInd + 1);
if (!style.isEmpty())
setStyle(style);
emit specChanged(arg);
}
/*
* reoptimize() runs a trace through a second pass of TraceBuilder
* optimizations, like this:
*
* reset state.
* move all blocks to a temporary list.
* compute immediate dominators.
* for each block in trace order:
* if we have a snapshot state for this block:
* clear cse entries that don't dominate this block.
* use snapshot state.
* move all instructions to a temporary list.
* for each instruction:
* optimizeWork - do CSE and simplify again
* if not simplified:
* append existing instruction and update state.
* else:
* if the instruction has a result, insert a mov from the
* simplified tmp to the original tmp and discard the instruction.
* if the last conditional branch was turned into a jump, remove the
* fall-through edge to the next block.
*/
void TraceBuilder::reoptimize() {
FTRACE(5, "ReOptimize:vvvvvvvvvvvvvvvvvvvv\n");
SCOPE_EXIT { FTRACE(5, "ReOptimize:^^^^^^^^^^^^^^^^^^^^\n"); };
assert(m_curTrace->isMain());
assert(m_savedTraces.empty());
m_state.setEnableCse(RuntimeOption::EvalHHIRCse);
m_enableSimplification = RuntimeOption::EvalHHIRSimplification;
if (!m_state.enableCse() && !m_enableSimplification) return;
always_assert(!m_inReoptimize);
m_inReoptimize = true;
BlockList sortedBlocks = rpoSortCfg(m_unit);
auto const idoms = findDominators(m_unit, sortedBlocks);
m_state.clear();
auto& traceBlocks = m_curTrace->blocks();
BlockList blocks(traceBlocks.begin(), traceBlocks.end());
traceBlocks.clear();
for (auto* block : blocks) {
assert(block->trace() == m_curTrace);
FTRACE(5, "Block: {}\n", block->id());
assert(m_curTrace->isMain());
m_state.startBlock(block);
m_curTrace->push_back(block);
auto instructions = std::move(block->instrs());
assert(block->empty());
while (!instructions.empty()) {
auto *inst = &instructions.front();
instructions.pop_front();
m_state.setMarker(inst->marker());
// merging state looks at the current marker, and optimizeWork
// below may create new instructions. Use the marker from this
// instruction.
assert(inst->marker().valid());
setMarker(inst->marker());
auto const tmp = optimizeWork(inst, idoms); // Can generate new instrs!
if (!tmp) {
// Could not optimize; keep the old instruction
appendInstruction(inst, block);
m_state.update(inst);
continue;
}
SSATmp* dst = inst->dst();
if (dst->type() != Type::None && dst != tmp) {
// The result of optimization has a different destination than the inst.
// Generate a mov(tmp->dst) to get result into dst. If we get here then
// assume the last instruction in the block isn't a guard. If it was,
// we would have to insert the mov on the fall-through edge.
assert(block->empty() || !block->back().isBlockEnd());
IRInstruction* mov = m_unit.mov(dst, tmp, inst->marker());
appendInstruction(mov, block);
m_state.update(mov);
}
// Not re-adding inst; remove the inst->taken edge
if (inst->taken()) inst->setTaken(nullptr);
}
if (block->empty()) {
// If all the instructions in the block were optimized away, remove it
// from the trace.
auto it = traceBlocks.end();
--it;
assert(*it == block);
m_curTrace->unlink(it);
} else {
if (block->back().isTerminal()) {
// Could have converted a conditional branch to Jmp; clear next.
block->setNext(nullptr);
}
m_state.finishBlock(block);
}
}
}
/*
* reoptimize() runs a trace through a second pass of TraceBuilder
* optimizations, like this:
*
* reset state.
* move all blocks to a temporary list.
* compute immediate dominators.
* for each block in trace order:
* if we have a snapshot state for this block:
* clear cse entries that don't dominate this block.
* use snapshot state.
* move all instructions to a temporary list.
* for each instruction:
* optimizeWork - do CSE and simplify again
* if not simplified:
* append existing instruction and update state.
* else:
* if the instruction has a result, insert a mov from the
* simplified tmp to the original tmp and discard the instruction.
* if the last conditional branch was turned into a jump, remove the
* fall-through edge to the next block.
*/
void TraceBuilder::reoptimize() {
FTRACE(5, "ReOptimize:vvvvvvvvvvvvvvvvvvvv\n");
SCOPE_EXIT { FTRACE(5, "ReOptimize:^^^^^^^^^^^^^^^^^^^^\n"); };
assert(m_curTrace == m_mainTrace.get());
assert(m_savedTraces.empty());
assert(m_inlineSavedStates.empty());
m_enableCse = RuntimeOption::EvalHHIRCse;
m_enableSimplification = RuntimeOption::EvalHHIRSimplification;
if (!m_enableCse && !m_enableSimplification) return;
if (m_mainTrace->blocks().size() >
RuntimeOption::EvalHHIRSimplificationMaxBlocks) {
// TODO CSEHash::filter is very slow for large block sizes
// t2135219 should address that
return;
}
BlockList sortedBlocks = rpoSortCfg(m_mainTrace.get(), m_irFactory);
auto const idoms = findDominators(sortedBlocks);
clearTrackedState();
auto blocks = std::move(m_mainTrace->blocks());
assert(m_mainTrace->blocks().empty());
while (!blocks.empty()) {
Block* block = blocks.front();
blocks.pop_front();
assert(block->trace() == m_mainTrace.get());
FTRACE(5, "Block: {}\n", block->id());
m_mainTrace->push_back(block);
if (m_snapshots[block]) {
useState(block);
}
auto instructions = std::move(block->instrs());
assert(block->empty());
while (!instructions.empty()) {
auto *inst = &instructions.front();
instructions.pop_front();
// last attempt to elide ActRecs, if we still need the InlineFPAnchor
// it will be added back to the trace when we re-add instructions that
// rely on it
if (inst->op() == InlineFPAnchor) {
continue;
}
// merging state looks at the current marker, and optimizeWork
// below may create new instructions. Use the marker from this
// instruction.
assert(inst->marker().valid());
setMarker(inst->marker());
auto const tmp = optimizeWork(inst, idoms); // Can generate new instrs!
if (!tmp) {
// Could not optimize; keep the old instruction
appendInstruction(inst, block);
updateTrackedState(inst);
continue;
}
SSATmp* dst = inst->dst();
if (dst->type() != Type::None && dst != tmp) {
// The result of optimization has a different destination than the inst.
// Generate a mov(tmp->dst) to get result into dst. If we get here then
// assume the last instruction in the block isn't a guard. If it was,
// we would have to insert the mov on the fall-through edge.
assert(block->empty() || !block->back()->isBlockEnd());
IRInstruction* mov = m_irFactory.mov(dst, tmp, inst->marker());
appendInstruction(mov, block);
updateTrackedState(mov);
}
// Not re-adding inst; remove the inst->taken edge
if (inst->taken()) inst->setTaken(nullptr);
}
if (block->back()->isTerminal()) {
// Could have converted a conditional branch to Jmp; clear next.
block->setNext(nullptr);
} else {
// if the last instruction was a branch, we already saved state
// for the target in updateTrackedState(). Now save state for
// the fall-through path.
saveState(block->next());
}
}
}
Marker::Marker(std::string name, double r, double g, double b)
{
markerName = name;
setMarker((std::size_t)r, (std::size_t)g, (std::size_t)b);
}
/// Set the Marker of the last-added series
bool setLastMarker(Marker& m)
{
return setMarker(markers.size()-1,m);
}
/*
* reoptimize() runs a trace through a second pass of TraceBuilder
* optimizations, like this:
*
* reset state.
* move all blocks to a temporary list.
* compute immediate dominators.
* for each block in trace order:
* if we have a snapshot state for this block:
* clear cse entries that don't dominate this block.
* use snapshot state.
* move all instructions to a temporary list.
* for each instruction:
* optimizeWork - do CSE and simplify again
* if not simplified:
* append existing instruction and update state.
* else:
* if the instruction has a result, insert a mov from the
* simplified tmp to the original tmp and discard the instruction.
* if the last conditional branch was turned into a jump, remove the
* fall-through edge to the next block.
*/
void TraceBuilder::reoptimize() {
FTRACE(5, "ReOptimize:vvvvvvvvvvvvvvvvvvvv\n");
SCOPE_EXIT { FTRACE(5, "ReOptimize:^^^^^^^^^^^^^^^^^^^^\n"); };
assert(m_savedBlocks.empty());
assert(!m_curWhere);
m_state.setEnableCse(RuntimeOption::EvalHHIRCse);
m_enableSimplification = RuntimeOption::EvalHHIRSimplification;
if (!m_state.enableCse() && !m_enableSimplification) return;
setConstrainGuards(false);
BlockList sortedBlocks = rpoSortCfg(m_unit);
auto const idoms = findDominators(m_unit, sortedBlocks);
m_state.clear();
for (auto* block : rpoSortCfg(m_unit)) {
FTRACE(5, "Block: {}\n", block->id());
m_state.startBlock(block);
m_curBlock = block;
auto instructions = std::move(block->instrs());
assert(block->empty());
while (!instructions.empty()) {
auto *inst = &instructions.front();
instructions.pop_front();
// merging state looks at the current marker, and optimizeWork
// below may create new instructions. Use the marker from this
// instruction.
assert(inst->marker().valid());
setMarker(inst->marker());
auto const tmp = optimizeWork(inst, idoms); // Can generate new instrs!
if (!tmp) {
// Could not optimize; keep the old instruction
appendInstruction(inst);
continue;
}
SSATmp* dst = inst->dst();
if (dst->type() != Type::None && dst != tmp) {
// The result of optimization has a different destination than the inst.
// Generate a mov(tmp->dst) to get result into dst. If we get here then
// assume the last instruction in the block isn't a guard. If it was,
// we would have to insert the mov on the fall-through edge.
assert(block->empty() || !block->back().isBlockEnd());
IRInstruction* mov = m_unit.mov(dst, tmp, inst->marker());
appendInstruction(mov);
}
if (inst->isBlockEnd()) {
// Not re-adding inst; replace it with a jump to the next block.
auto next = inst->next();
appendInstruction(m_unit.gen(Jmp, inst->marker(), next));
inst->setTaken(nullptr);
inst->setNext(nullptr);
}
}
assert(!block->empty());
m_state.finishBlock(block);
}
}
void vad_cb(const sensor_msgs::PointCloud2ConstPtr& cloud) {
if ((cloud->width * cloud->height) == 0)
return;
pcl::fromROSMsg (*cloud, cloud_xyzrgb_);
t0 = my_clock();
if( limitPoint( cloud_xyzrgb_, cloud_xyzrgb, distance_th ) > 10 ){
std::cout << "voxelize...." << std::endl;
getVoxelGrid( grid, cloud_xyzrgb, cloud_downsampled, voxel_size );
std::cout << " ...done.." << std::endl;
const int pnum = cloud_downsampled.points.size();
float x_min = 10000000, y_min = 10000000, z_min = 10000000;
float x_max = -10000000, y_max = -10000000, z_max = -10000000;
for( int p=0; p<pnum; p++ ){
if( cloud_downsampled.points[ p ].x < x_min ) x_min = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y < y_min ) y_min = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z < z_min ) z_min = cloud_downsampled.points[ p ].z;
if( cloud_downsampled.points[ p ].x > x_max ) x_max = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y > y_max ) y_max = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z > z_max ) z_max = cloud_downsampled.points[ p ].z;
}
//std::cout << x_min << " " << y_min << " " << z_min << std::endl;
//std::cout << x_max << " " << y_max << " " << z_max << std::endl;
//std::cout << grid.getMinBoxCoordinates() << std::endl;
std::cout << "search start..." << std::endl;
//****************************************
//* object detection start
t1 = my_clock();
search_obj.cleanData();
search_obj.setC3HLAC( dim, color_threshold_r, color_threshold_g, color_threshold_b, grid, cloud_downsampled, voxel_size, box_size );
t1_2 = my_clock();
if( ( search_obj.XYnum() != 0 ) && ( search_obj.Znum() != 0 ) )
search_obj.search();
t2 = my_clock();
//* object detection end
//****************************************
std::cout << " ...search done." << std::endl;
//* show the processing time
tAll += t2 - t0;
process_count++;
std::cout << "voxelize :"<< t1 - t0 << " sec" << std::endl;
std::cout << "feature extraction : "<< t1_2 - t1 << " sec" <<std::endl;
std::cout << "search : "<< t2 - t1_2 << " sec" <<std::endl;
std::cout << "all processes : "<< t2 - t0 << " sec" << std::endl;
std::cout << "AVERAGE : "<< tAll / process_count << " sec" << std::endl;
marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker_range", 1);
marker_array_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 100);
visualization_msgs::MarkerArray marker_array_msg_;
//* show the limited space
//setMarker( id_, pos_x, pos_y, pos_z, scale_x, scale_y, scale_z, ca, cr, cg, cb )
marker_pub_.publish( setMarker( -1, (x_max+x_min)/2, (y_max+y_min)/2, (z_max+z_min)/2, x_max-x_min, y_max-y_min, z_max-z_min, 0.1, 1.0, 0.0, 0.0 ) );
//* publish markers for detected regions
for( int q=0; q<rank_num; q++ ){
if( search_obj.maxDot( q ) < detect_th )
marker_array_msg_.markers.push_back( setMarker( q, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ) );
else{
std::cout << search_obj.maxX( q ) << " " << search_obj.maxY( q ) << " " << search_obj.maxZ( q ) << std::endl;
std::cout << "dot " << search_obj.maxDot( q ) << std::endl;
marker_array_msg_.markers.push_back( setMarker( q, search_obj.maxX( q ) * region_size + sliding_box_size_x/2 + x_min, search_obj.maxY( q ) * region_size + sliding_box_size_y/2 + y_min, search_obj.maxZ( q ) * region_size + sliding_box_size_z/2 + z_min, sliding_box_size_x, sliding_box_size_y, sliding_box_size_z, 0.5, 0.0, 1.0, 0.0 ) );
}
}
//std::cerr << "MARKER ARRAY published with size: " << marker_array_msg_.markers.size() << std::endl;
marker_array_pub_.publish(marker_array_msg_);
}
std::cout << "Waiting msg..." << std::endl;
}
/*
* reoptimize() runs a trace through a second pass of IRBuilder
* optimizations, like this:
*
* reset state.
* move all blocks to a temporary list.
* compute immediate dominators.
* for each block in trace order:
* if we have a snapshot state for this block:
* clear cse entries that don't dominate this block.
* use snapshot state.
* move all instructions to a temporary list.
* for each instruction:
* optimizeWork - do CSE and simplify again
* if not simplified:
* append existing instruction and update state.
* else:
* if the instruction has a result, insert a mov from the
* simplified tmp to the original tmp and discard the instruction.
* if the last conditional branch was turned into a jump, remove the
* fall-through edge to the next block.
*/
void IRBuilder::reoptimize() {
Timer _t("optimize_reoptimize");
FTRACE(5, "ReOptimize:vvvvvvvvvvvvvvvvvvvv\n");
SCOPE_EXIT { FTRACE(5, "ReOptimize:^^^^^^^^^^^^^^^^^^^^\n"); };
always_assert(m_savedBlocks.empty());
always_assert(!m_curWhere);
always_assert(m_state.inlineDepth() == 0);
m_state.setEnableCse(RuntimeOption::EvalHHIRCse);
m_enableSimplification = RuntimeOption::EvalHHIRSimplification;
if (!m_state.enableCse() && !m_enableSimplification) return;
setConstrainGuards(false);
auto blocksIds = rpoSortCfgWithIds(m_unit);
auto const idoms = findDominators(m_unit, blocksIds);
m_state.clear();
for (auto* block : blocksIds.blocks) {
FTRACE(5, "Block: {}\n", block->id());
m_state.startBlock(block);
m_curBlock = block;
auto nextBlock = block->next();
auto backMarker = block->back().marker();
auto instructions = block->moveInstrs();
assert(block->empty());
while (!instructions.empty()) {
auto* inst = &instructions.front();
instructions.pop_front();
// merging state looks at the current marker, and optimizeWork
// below may create new instructions. Use the marker from this
// instruction.
assert(inst->marker().valid());
setMarker(inst->marker());
auto const tmp = optimizeWork(inst, idoms); // Can generate new instrs!
if (!tmp) {
// Could not optimize; keep the old instruction
appendInstruction(inst);
continue;
}
SSATmp* dst = inst->dst();
if (dst != tmp) {
// The result of optimization has a different destination than the inst.
// Generate a mov(tmp->dst) to get result into dst. If we get here then
// assume the last instruction in the block isn't a guard. If it was,
// we would have to insert the mov on the fall-through edge.
assert(block->empty() || !block->back().isBlockEnd());
appendInstruction(m_unit.mov(dst, tmp, inst->marker()));
}
if (inst->isBlockEnd()) {
// We're not re-adding the block-end instruction. Unset its edges.
inst->setTaken(nullptr);
inst->setNext(nullptr);
}
}
if (block->empty() || !block->back().isBlockEnd()) {
// Our block-end instruction was eliminated (most likely a Jmp* converted
// to a nop). Replace it with a jump to the next block.
appendInstruction(m_unit.gen(Jmp, backMarker, nextBlock));
}
m_state.finishBlock(block);
}
}
void RowArea::onSetStartMarker()
{
setMarker(0);
update();
}
void vad_cb(const sensor_msgs::PointCloud2ConstPtr& cloud) {
if ((cloud->width * cloud->height) == 0)
return;
pcl::fromROSMsg (*cloud, cloud_xyzrgb_);
t0 = my_clock();
if( limitPoint( cloud_xyzrgb_, cloud_xyzrgb, distance_th ) > 10 ){
std::cout << "compute normals and voxelize...." << std::endl;
#ifdef CCHLAC_TEST
getVoxelGrid( grid, cloud_xyzrgb, cloud_downsampled, voxel_size );
#else
//****************************************
//* compute normals
computeNormal( cloud_xyzrgb, cloud_normal );
t0_2 = my_clock();
//* voxelize
getVoxelGrid( grid, cloud_normal, cloud_downsampled, voxel_size );
#endif
std::cout << " ...done.." << std::endl;
const int pnum = cloud_downsampled.points.size();
float x_min = 10000000, y_min = 10000000, z_min = 10000000;
float x_max = -10000000, y_max = -10000000, z_max = -10000000;
for( int p=0; p<pnum; p++ ){
if( cloud_downsampled.points[ p ].x < x_min ) x_min = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y < y_min ) y_min = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z < z_min ) z_min = cloud_downsampled.points[ p ].z;
if( cloud_downsampled.points[ p ].x > x_max ) x_max = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y > y_max ) y_max = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z > z_max ) z_max = cloud_downsampled.points[ p ].z;
}
//std::cout << x_min << " " << y_min << " " << z_min << std::endl;
//std::cout << x_max << " " << y_max << " " << z_max << std::endl;
//std::cout << grid.getMinBoxCoordinates() << std::endl;
std::cout << "search start..." << std::endl;
//****************************************
//* object detection start
t1 = my_clock();
search_obj.cleanData();
#ifdef CCHLAC_TEST
search_obj.setC3HLAC( dim, color_threshold_r, color_threshold_g, color_threshold_b, grid, cloud_downsampled, voxel_size, box_size );
#else
search_obj.setVOSCH( dim, color_threshold_r, color_threshold_g, color_threshold_b, grid, cloud_normal, cloud_downsampled, voxel_size, box_size );
#endif
t1_2 = my_clock();
if( ( search_obj.XYnum() != 0 ) && ( search_obj.Znum() != 0 ) )
#ifdef CCHLAC_TEST
search_obj.search();
#else
search_obj.searchWithoutRotation();
#endif
search_obj.removeOverlap();
t2 = my_clock();
//* object detection end
//****************************************
std::cout << " ...search done." << std::endl;
tAll += t2 - t0;
process_count++;
#ifdef CCHLAC_TEST
std::cout << "voxelize :"<< t1 - t0 << " sec" << std::endl;
#else
std::cout << "normal estimation :"<< t0_2 - t0 << " sec" << std::endl;
std::cout << "voxelize :"<< t1 - t0_2 << " sec" << std::endl;
#endif
std::cout << "feature extraction : "<< t1_2 - t1 << " sec" <<std::endl;
std::cout << "search : "<< t2 - t1_2 << " sec" <<std::endl;
std::cout << "all processes : "<< t2 - t0 << " sec" << std::endl;
std::cout << "AVERAGE : "<< tAll / process_count << " sec" << std::endl;
marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker_range", 1);
marker_array_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 100);
visualization_msgs::MarkerArray marker_array_msg_;
#if 0
//* show the limited space
marker_pub_.publish( setMarker( -1, (x_max+x_min)/2, (y_max+y_min)/2, (z_max+z_min)/2, x_max-x_min, y_max-y_min, z_max-z_min, 0.1, 1.0, 0.0, 0.0 ) );
#endif
for( int m=0; m<model_num; m++ ){
for( int q=0; q<rank_num; q++ ){
//if( search_obj.maxDot( m, q ) < detect_th ) break;
std::cout << search_obj.maxX( m, q ) << " " << search_obj.maxY( m, q ) << " " << search_obj.maxZ( m, q ) << std::endl;
std::cout << "dot " << search_obj.maxDot( m, q ) << std::endl;
//if( (search_obj.maxX( m, q )!=0)||(search_obj.maxY( m, q )!=0)||(search_obj.maxZ( m, q )!=0) ){
//* publish markers for detected regions
if( search_obj.maxDot( m, q ) < detect_th )
marker_array_msg_.markers.push_back( setMarker( m, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ) );
else{
marker_array_msg_.markers.push_back( setMarker( m, search_obj.maxX( m, q ) * region_size + sliding_box_size_x/2 + x_min, search_obj.maxY( m, q ) * region_size + sliding_box_size_y/2 + y_min, search_obj.maxZ( m, q ) * region_size + sliding_box_size_z/2 + z_min, sliding_box_size_x, sliding_box_size_y, sliding_box_size_z, 0.5, marker_color_r[ m ], marker_color_g[ m ], marker_color_b[ m ] ) );
}
}
}
//std::cerr << "MARKER ARRAY published with size: " << marker_array_msg_.markers.size() << std::endl;
marker_array_pub_.publish(marker_array_msg_);
}
std::cout << "Waiting msg..." << std::endl;
}
void RowArea::onSetStopMarker()
{
setMarker(1);
update();
}
