ExtendedPose2dVectorPtr ProcessDepth::getObjects(cv::Mat image)
{
cv::Mat imageBackup = image.clone();//TODO remove this
cv::Mat clean = calcCleanImg(image);
std::vector<cv::Point2d> objects;
std::vector<int> depth;
findObjects(clean,imageBackup,objects,depth);
ExtendedPose2dVectorPtr objectPoses(new oadrive::core::ExtendedPose2dVector());
for( unsigned int i = 0; i< objects.size(); i++ )
{
oadrive::core::ExtendedPose2d ObjectPosition;
if(depth[i] != 0){
// std::cout<<"Depth: "<<depth[i]<<" "<<std::endl;
// std::cout<<"Pos in Car "<<mPosConv->depthPixel2Car(objects[i],(float)((float)depth[i]/1000.0))<<" "<<std::endl;
ObjectPosition = mPosConv->car2World(
Environment::getInstance()->getCarPose(),
mPosConv->depthPixel2Car(objects[i],(float)((float)depth[i]/1000.0)));
objectPoses->push_back(ObjectPosition);
}
// if(mPosConv->depthPixel2World(mEnviroment->getCarPose(),objects[i],ObjectPosition))
// {objectPoses->push_back(ObjectPosition);}
}
return objectPoses;
}
void ChangeVisibilityEditorWidget::updateData(Action * action)
{
mCurrentAction = qobject_cast<ChangeVisibility*>(action);
if (! mCurrentAction)
return;
mFadeEditorWidget->updateData(mCurrentAction->fadeAction());
mSlideEditorWidget->updateData(mCurrentAction->slideAction());
setGroupName(mCurrentAction->name());
mObjectsWidget->clear();
mObjects = findObjects(! mCurrentAction->toShow());
mObjectsWidget->blockSignals(true);
for (int i=0; i < mObjects.size(); i++)
mObjectsWidget->addItem(mObjects[i]->objectName());
mObjectsWidget->blockSignals(false);
mObjectsWidget->setCurrentIndex(0);
if (! mCurrentAction->sceneObject() && mObjects.size()) {
mCurrentAction->setSceneObject(mObjects[0]);
//if (lastChangeVisibilityActionForObject(mObjects[0]) == mCurrentAction)
// mObjects[0]->setAvailable(! mObjects[0]->isAvailable());
}
}
void ProcessDepth::processDepthImageDebug(cv::Mat image)
{
cv::Mat imageBackup;
image.copyTo(imageBackup);
cv::Mat output;
cv::cvtColor( image, output, CV_GRAY2BGRA );
imshow( "input", image );
cv::Mat clean = calcCleanImg(image);
cv::Mat show;
clean.convertTo(show,CV_8U);
cv::cvtColor(show,show,cv::COLOR_GRAY2BGR);
std::vector<cv::Point2d> objects;
std::vector<int> depth;
findObjects(clean,imageBackup,objects,depth);
for( unsigned int i = 0; i< objects.size(); i++ )
{
cv::circle(show,objects[i],4,cv::Scalar(0,0,255));
//std::cout<<mPosConv.depthPixel2World(carPose,objects[i])<<std::endl;
}
cv::imshow("conturs",show);
cv::waitKey(1);
}
bool EnvironmentHandler::loadContent( )
{
if( !m_bitmap.loadFromFile( "../../Resources/levelOne.png" ) )
{
return EXIT_FAILURE;
}
m_iHorizontalBitmapSize = m_bitmap.getSize().x;
m_iVerticalBitmapSize = m_bitmap.getSize().y;
if(!m_tiles.loadFromFile( "../../Resources/tiles.png" ) )
{
return EXIT_FAILURE;
}
m_floorArray.reserve( m_iHorizontalBitmapSize * m_iVerticalBitmapSize );
bitmapToArray( );
m_sprites.reserve( m_floorArray.size( ) );
createLevel( );
checkWalls( );
if(!m_objectBitmap.loadFromFile( "../../Resources/levelOneObjects.png" ) )
{
return EXIT_FAILURE;
}
m_objects.reserve( m_floorArray.size( ) );
findObjects();
return EXIT_SUCCESS;
}
cCharSectorIterator* cSectorMaps::findChars( unsigned char map, uint x1, uint y1, uint x2, uint y2, bool includeoffline )
{
std::map<unsigned char, cSectorMap*>::const_iterator it = charmaps.find( map );
if ( it == charmaps.end() )
{
Console::instance()->log( LOG_ERROR, QString( "Couldn't find a map with the id %1. (cSectorMaps::findChars)" ).arg( map ) );
return new cCharSectorIterator( 0, 0 ); // Return an empty iterator
}
if (includeoffline) {
return static_cast<cCharSectorIterator*>( findObjects( MT_CHARSANDOFFLINE, it->second, x1, y1, x2, y2 ) );
} else {
return static_cast<cCharSectorIterator*>( findObjects( MT_CHARS, it->second, x1, y1, x2, y2 ) );
}
}
void GridDatabase::findObjects(U8 typeNumber, Vector<DatabaseObject *> &fillVector, const Rect *extents, const IntRect *bins) const
{
static Vector<U8> types;
types.resize(1);
types[0] = typeNumber;
findObjects(types, fillVector, extents, bins);
}
int aimAtSideMostTarget(int side){
int most_blob, i, center, error;
int attempts = 0;
do{
captureImage();
openImage(SOURCE_FILE_NAME);
gaussianSmooth();
isolateRed();
findObjects();
if(numBlobs <= 0)
{
//printf("Couldn't find any targets =(\n");
return(0);
}
most_blob = 0;
if (side)
{
for(i = 1; i < numBlobs; i++)
{
if (blobs[most_blob].rightX < blobs[i].rightX && blobs[i].rightX - blobs[i].leftX > MIN_BLOB_WIDTH)
{
most_blob = i;
}
}
}
else
{
for(i = 1; i < numBlobs; i++)
{
if (blobs[most_blob].leftX > blobs[i].leftX && blobs[i].rightX - blobs[i].leftX > MIN_BLOB_WIDTH)
{
most_blob = i;
}
}
}
if(blobs[most_blob].rightX - blobs[most_blob].leftX >= MIN_BLOB_WIDTH)
{
//printf("Found target:\n");
//printf(" Width: %d\n", blobs[most_blob].rightX-blobs[most_blob].leftX);
center = (blobs[most_blob].rightX-blobs[most_blob].leftX)/2 + blobs[most_blob].leftX;
//printf(" Center: %d\n", center);
error = center - IMAGE_CENTER;
if(abs(error) <= MAX_TARGET_ERROR)
{
break;
}
aimAtTarget(error);
}
attempts++;
}
while (abs(error) > MAX_TARGET_ERROR && attempts < MAX_AIM_ATTEMPTS);
return(1);
}
void GCF::Components::ProjectManagerComponent::componentDestroyed(GCF::AbstractComponent* comp)
{
QString ifaceName = QString("%1.IProjectPart").arg(comp->componentName());
QStringList compNames;
QList<QObject*> projectParts = findObjects(ifaceName, compNames);
for(int i=0; i<projectParts.count(); i++)
{
IProjectPart* projectPart = qobject_cast<IProjectPart*>(projectParts[i]);
d->visProject.removeProjectPart(projectPart);
}
}
cItemSectorIterator* cSectorMaps::findMultis( unsigned char map, uint x1, uint y1, uint x2, uint y2 )
{
std::map<unsigned char, cSectorMap*>::const_iterator it = itemmaps.find( map );
if ( it == itemmaps.end() )
{
Console::instance()->log( LOG_ERROR, QString( "Couldn't find a map with the id %1. (cSectorMaps::findMultis)" ).arg( map ) );
return new cItemSectorIterator( 0, 0 ); // Return an empty iterator
}
return static_cast<cItemSectorIterator*>( findObjects( MT_MULTIS, it->second, x1, y1, x2, y2 ) );
}
cCharSectorIterator* cSectorMaps::findChars( unsigned char map, uint x, uint y )
{
std::map<unsigned char, cSectorMap*>::const_iterator it = charmaps.find( map );
if ( it == charmaps.end() )
{
Console::instance()->log( LOG_ERROR, QString( "Couldn't find a map with the id %1. (cSectorMaps::findChars)" ).arg( map ) );
return new cCharSectorIterator( 0, 0 ); // Return an empty iterator
}
return static_cast<cCharSectorIterator*>( findObjects( MT_CHARS, it->second, x, y ) );
}
void Barrier::render(U32 layerIndex)
{
Color b(0,0,0.15), f(0,0,1);
//Color b(0.0,0.0,0.075), f(.3 ,0.3,0.8);
if(layerIndex == 0)
{
Vector<Point> colPoly;
getCollisionPoly(colPoly);
glColor(b);
glBegin(GL_POLYGON);
for(S32 i = 0; i < colPoly.size(); i++)
glVertex2f(colPoly[i].x, colPoly[i].y);
glEnd();
}
else if(layerIndex == 1)
{
if(mLastBarrierChangeIndex != mBarrierChangeIndex)
{
mLastBarrierChangeIndex = mBarrierChangeIndex;
mRenderLineSegments.clear();
Vector<Point> colPoly;
getCollisionPoly(colPoly);
S32 last = colPoly.size() - 1;
for(S32 i = 0; i < colPoly.size(); i++)
{
mRenderLineSegments.push_back(colPoly[last]);
mRenderLineSegments.push_back(colPoly[i]);
last = i;
}
static Vector<GameObject *> fillObjects;
fillObjects.clear();
Rect bounds(start, end);
bounds.expand(Point(mWidth, mWidth));
findObjects(BarrierType, fillObjects, bounds);
for(S32 i = 0; i < fillObjects.size(); i++)
{
colPoly.clear();
if(fillObjects[i] != this && fillObjects[i]->getCollisionPoly(colPoly))
clipRenderLinesToPoly(colPoly);
}
}
glColor(f);
glBegin(GL_LINES);
for(S32 i = 0; i < mRenderLineSegments.size(); i++)
glVertex2f(mRenderLineSegments[i].x, mRenderLineSegments[i].y);
glEnd();
}
}
void GCF::Components::ProjectManagerComponent::componentCreated(GCF::AbstractComponent* comp)
{
QString ifaceName = QString("%1.IProjectPart").arg(comp->componentName());
QStringList compNames;
QList<QObject*> projectParts = findObjects(ifaceName, compNames);
for(int i=0; i<projectParts.count(); i++)
{
IProjectPart* projectPart = qobject_cast<IProjectPart*>(projectParts[i]);
d->visProject.addProjectPart(projectPart);
}
if(comp->componentName() == "ComponentLoaderComponent")
{
// all components have just been loaded. We ensure that the modified flag is set as false now
d->visProject.newProject();
}
}
Vision::Results ImageAnalyzer::analyze(cv::Mat& img)
{
findObjects(img);
/* draw result on image (debug) */
BlobInfo* ghost_result = ghost_buffer.lastValidBlob();
if (ghost_result != NULL && ghost_result->getClass() == GHOST_CLASS)
{
cv::rectangle(img, ghost_result->a, ghost_result->b,
CV_RGB(254,0,0), 2, 8, 0);
}
ghostwidth_filter.update(ghost_result != NULL ? ghost_result->getWidth() : 0);
ghostheight_filter.update(ghost_result != NULL ? ghost_result->getHeight() : 0);
distanceCalculator.insertGhostData((int)ghostheight_filter.curValue(),
(int) ghostwidth_filter.curValue());
return composeMessage();
}
Manager::Manager(QObject* root, QObject *parent) :
QObject(parent),
firstFileWasLoaded(false),
root(root)
{
timer = new QTimer(this);
connect(timer, SIGNAL(timeout()), SLOT(moveAllFurther()));
samplesManager = new SamplesManager(this);
connect(samplesManager, SIGNAL(haveBeenLoaded(int)), SLOT(haveBeenLoaded(int)));
findObjects();
connect(starter, SIGNAL(start()), SLOT(start()));
connect(starter, SIGNAL(pause()), timer, SLOT(stop()));
connect(mover, SIGNAL(valueChanged()), SLOT(moveAllToMoverPos()));
connect(root, SIGNAL(pathsArePrepared()), SLOT(loadData()));
connect(root, SIGNAL(closeAllFiles()), samplesManager, SLOT(clear()));
connect(root, SIGNAL(closeAllFiles()), SLOT(resetFileLoadingFlag()));
}
void GCF::Components::ProjectManagerComponent::creationAnnounced()
{
// Find out all objects that implement IProjectPart and register them with
// the project object owned by this component.
QStringList compNames;
QList<QObject*> projectParts = findObjects("IProjectPart", compNames);
for(int i=0; i<projectParts.count(); i++)
{
IProjectPart* projectPart = qobject_cast<IProjectPart*>(projectParts[i]);
d->visProject.addProjectPart(projectPart);
}
QStringList rfList = settings()["RecentFiles"].toStringList();
for(int i=0; i<rfList.count(); i++)
d->visProject.addRecentFile( rfList[i] );
connect(&d->visProject, SIGNAL(projectModified(bool)), this, SLOT(projectModified(bool)));
projectModified(false);
d->visProject.setRecentFileListUnModified();
}
void ObjectDetector::tryImageIMSync()
{
// std::cout<<"imageQueue.size "<<imageQueue.size()<<std::endl;
// std::cout<<"navdataQueue.size "<<navdataQueue.size()<<std::endl;
//for (int i = 0; i < imageQueue.size(); i++) cout << imageQueue[i]->header.stamp << ", ";
//cout << " | ";
//for (int i = 0; i < navdataQueue.size(); i++) cout << navdataQueue[i].header.stamp << ", ";
//cout << endl;
while(imageQueue.size() > 0)
{
cv_bridge::CvImagePtr cv_ptr = imageQueue.front();
while (1 < navdataQueue.size() && navdataQueue[1].header.stamp < cv_ptr->header.stamp) navdataQueue.pop_front();
if (1 < navdataQueue.size())
{
if (cv_ptr->header.stamp >= navdataQueue[0].header.stamp)
{
undistort(cv_ptr->image, frame, K, D);
navdata_prev = navdata;
ardrone_autonomy::Navdata & nd0 = navdataQueue[0], nd1 = navdataQueue[1];
navdata = navdataInterpolate(nd0, nd1, cv_ptr->header.stamp);
navdata.altd = 800;
if (navdata.altd > 0)
{
view_H = getBirdsEyeTransform(navdata.rotZ*GRAD, navdata.rotY*GRAD, navdata.rotX*GRAD, navdata.altd*0.001);
warpPerspective(frame, view, view_H, view_size, INTER_CUBIC | WARP_INVERSE_MAP);
imshow("view", view);
findObjects();
}
imshow("frame", frame);
}
imageQueue.pop_front();
} else
break;
}
waitKey(1);
}
// FIXME: Scale objects according to distance and ignore those below a given threshold (which improves with better sensors)
const Sensor::TrackCollection& Sensor::FindTracksInRange() const
{
assert(player);
collection.clear();
// Find all units within range
static float maxUnitRadius =
XMLSupport::parse_float(vs_config->getVariable("graphics", "hud", "radar_search_extra_radius", "1000"));
static bool allGravUnits =
XMLSupport::parse_bool(vs_config->getVariable("graphics", "hud", "draw_gravitational_objects", "true"));
UnitWithinRangeLocator<CollectRadarTracks> unitLocator(GetMaxRange(), maxUnitRadius);
unitLocator.action.init(this, &collection, player);
if (! is_null(player->location[Unit::UNIT_ONLY]))
{
findObjects(_Universe->activeStarSystem()->collidemap[Unit::UNIT_ONLY],
player->location[Unit::UNIT_ONLY],
&unitLocator);
}
if (allGravUnits)
{
Unit *target = player->Target();
Unit *gravUnit;
bool foundtarget = false;
for (un_kiter i = _Universe->activeStarSystem()->gravitationalUnits().constIterator();
(gravUnit = *i) != NULL;
++i)
{
unitLocator.action.acquire(gravUnit, UnitUtil::getDistance(player, gravUnit));
if (gravUnit == target)
foundtarget = true;
}
if (target && !foundtarget)
unitLocator.action.acquire(target, UnitUtil::getDistance(player, target));
}
return collection;
}
cv::Mat ProcessDepth::getDebugImage(cv::Mat image)
{
cv::Mat clean = calcCleanImg(image);
std::vector<cv::Point2d> objects;
std::vector<int> depth;
findObjects(clean,image,objects,depth);
cv::cvtColor(clean,clean,CV_GRAY2BGR);
clean.convertTo(clean,CV_8UC3);
for( unsigned int i = 0; i< objects.size(); i++ )
{
cv::circle(clean,objects.at(i),3,cv::Scalar(0,0,255));
}
cv::line( clean,
cv::Point2d( 0, mHorizonRow ),
cv::Point2d( clean.cols, mHorizonRow ),
cv::Scalar(0,255,255));
cv::line( clean,
cv::Point2d( 0, mMaxDepthRow ),
cv::Point2d( clean.cols, mMaxDepthRow ),
cv::Scalar(0,128,255));
//cv::line(clean,cv::Point2d(0,mPosConv->getFirstRow()/2),cv::Point2d(clean.cols,mPosConv->getFirstRow()/2),cv::Scalar(0,255,0));
//cv::line(clean,cv::Point2d(0,mPosConv->getLastRow()/2),cv::Point2d(clean.cols,mPosConv->getLastRow()/2),cv::Scalar(0,255,0));
return clean;
}
vector<PObject*>* Procrustes::findObjects() {
findObjects(0,0);
// cout << objs -> size() << endl;
return objs;
}
