int main(int argc, char *argv[]) {
Network yarp;
yarp::os::Node *rosNode;
rosNode = new yarp::os::Node("/ros_tests");
yarp::os::Subscriber<geometry_msgs_Point> xd_inputPort;
xd_inputPort.setReadOnly();
bool outputOk = xd_inputPort.topic("/my_xd_in");
BufferedPort<geometry_msgs_Point> xd_outputPort;
bool outputOk_3 = xd_outputPort.open("/gaze_point");
BufferedPort<geometry_msgs_Point> receiverBuff1Mux1;
bool receiver1Mux1Ok = receiverBuff1Mux1.open("/my_other_port_in");
yarp::os::Publisher<geometry_msgs_Point> outputPort;
outputPort.setWriteOnly();
bool outputOk_1 = outputPort.topic("/my_x_out");
if(!outputOk_3)
{
printf("outputOk_3 failed to open\n");
return -1;
}
if(!outputOk_1)
{
printf("outputOk_1 failed to open\n");
return -1;
}
bool connectSuccess = false;
while(!connectSuccess)
{
printf("\nTrying to connect to /iKinGazeCtrl/x:o ... ");
connectSuccess = yarp.connect("/iKinGazeCtrl/x:o", receiverBuff1Mux1.getName());
yarp::os::Time::delay(1);
}
connectSuccess = false;
while(!connectSuccess)
{
printf("\nTrying to connect to /iKinGazeCtrl/xd:i ... ");
connectSuccess = yarp.connect(xd_outputPort.getName(),"/iKinGazeCtrl/xd:i");
yarp::os::Time::delay(1);
}
while(true){
geometry_msgs_Point *reading1Mux1;
reading1Mux1 = xd_inputPort.read(false);
if (reading1Mux1 != NULL){
geometry_msgs_Point & out = xd_outputPort.prepare();
out = *reading1Mux1;
xd_outputPort.write();
}
geometry_msgs_Point *reading1Mux;
reading1Mux = receiverBuff1Mux1.read();
geometry_msgs_Point & out_ = outputPort.prepare();
out_ = *reading1Mux;
outputPort.write();
}
return 0;
}
void test2() {
using namespace Utility;
using namespace std;
using namespace ml;
typedef double T;
Timer<float> timer;
timer.start();
Network<T>* network = new Network<T>();
ILayer<T>* l1 = new Layer<T>(100);
ILayer<T>* l2 = new Layer<T>(200);
ILayer<T>* l3 = new Layer<T>(500);
ILayer<T>* l4 = new Layer<T>(10);
l1->setName("L1");
l2->setName("L2");
l3->setName("L3");
l4->setName("L4");
network->setInputLayer(l1);
network->connect(l1, l2);
network->connect(l2, l3);
network->connect(l3, l4);
network->setOutputLayer(l4);
network->init();
ml::Mat<T> samples(100, 100, 1);
ml::Mat<T> nominals(1, 10, 0);
network->train(samples, nominals);
timer.stop();
cout << timer.getTime() << endl;
}
void test_crazy_network_1() {
using namespace std;
using namespace ml;
typedef int T;
Timer<float> timer;
timer.start();
// Defining network
Network<T>* network = new Network<T>();
ILayer<T>* l1 = new Layer<T>(100, "L1");
ILayer<T>* l2 = new Layer<T>(200, "L2");
ILayer<T>* l3 = new Layer<T>(500, "L3");
// Defining subnet, middle node is actually l2. sorta recurrent
Network<T>* aSubNet = new Network<T>();//.. = something else
// Defining rest of network
ILayer<T>* l4 = aSubNet;
ILayer<T>* l5 = new Layer<T>(2, "L5");
l4->setName("N4");
network->setInputLayer(l1);
network->connect(l1, l2);
network->connect(l2, l3);
network->connect(l3, l4);
network->connect(l3, l2); // note: circularity back to l2
network->connect(l4, l2); // here again as well
network->connect(l4, l5); // note: connecting subnet arbitrarily to another layer..
network->setOutputLayer(l5);
// Proper way to finish defining a subnet after completely defining which
// network owns the recurrent layers (l2 is owned by parent network..)
{
ILayer<T>* s1 = new Layer<T>(100, "S1");
ILayer<T>* s3 = new Layer<T>(5000, "S3");
aSubNet->setInputLayer(s1);
aSubNet->connect(s1, l2);
aSubNet->connect(l2, s3);
aSubNet->setOutputLayer(s3);
aSubNet->setName("SubNet 1");
}
// Finish defining parent network and init.. feed.. train..
network->init();
ml::Mat<T> input(1, 100, 1); // input vec must be the same size as the input layer's size
network->feed(input);
ml::Mat<T> samples(100, 100, 1);
ml::Mat<T> nominals(1, 2, 0);
// network->train(samples, nominals);
timer.stop();
cout << timer.getTime() << endl;
}
bool test_live() {
printf("\n*** test_live()\n");
Network yarp;
yarp.setLocalMode(true);
Demo client;
Server server;
Port client_port,server_port;
client_port.open("/client");
server_port.open("/server");
yarp.connect(client_port.getName(),server_port.getName());
int x = 0;
client.yarp().attachAsClient(client_port);
server.yarp().attachAsServer(server_port);
x = client.add_one(99);
printf("Result %d\n", x);
client.test_void(200);
client.test_void(201);
x = client.add_one(100);
printf("Result %d\n", x);
client.test_1way(200);
client.test_1way(201);
x = client.add_one(101);
printf("Result %d\n", x);
return (x==102);
}
int main(int argc, char *argv[]) {
if (argc<3) {
fprintf(stderr, "Please supply (1) a port name for the client\n");
fprintf(stderr, " (2) a port name for the server\n");
return 1;
}
Network yarp;
const char *client_name = argv[1];
const char *server_name = argv[2];
RpcClient port;
port.open(client_name);
int ct = 0;
while (true) {
if (port.getOutputCount()==0) {
printf("Trying to connect to %s\n", server_name);
yarp.connect(client_name,server_name);
} else {
Bottle cmd;
cmd.addString("COUNT");
cmd.addInt32(ct);
ct++;
printf("Sending message... %s\n", cmd.toString().c_str());
Bottle response;
port.write(cmd,response);
printf("Got response: %s\n", response.toString().c_str());
}
Time::delay(1);
}
}
void receiveMe(const std::string &ip, int port, const std::string &dir, void (*cb)(int, void*), void *data) {
Network net;
FILE *file;
char *buff;
int received;
int i = 0, odoSize;
ANetwork::t_frame frame;
bool reading = true;
try {
net.init(port, ANetwork::TCP_MODE);
net.connect(ip);
net.write(CreateRequest::create(1, 2, 3, "File"));
frame = net.read();
odoSize = atoi(frame.data);
frame = net.read();
file = fopen(std::string(dir + frame.data).c_str(), "wb");
while (reading) {
buff = (char*) net.getSocket()->read(D_BUF, &received);
if (received <= 0)
break;
i += fwrite(buff, 1, received, file);
cb(i * 100 / odoSize, data);
}
fclose(file);
} catch (const std::exception &e) {
std::cout << e.what() << std::endl;
}
};
int main(int argc, char *argv[])
{
Network yarpy;
bool run = true;
BufferedPort<Bottle> inputBottle1;
inputBottle1.open("/localNeuronPotentialReader1:i");
BufferedPort<Bottle> inputBottle2;
inputBottle2.open("/localNeuronPotentialReader2:i");
BufferedPort<Bottle> inputBottle3;
inputBottle3.open("/localNeuronPotentialReader3:i");
BufferedPort<Bottle> inputBottle4;
inputBottle4.open("/localNeuronPotentialReader4:i");
BufferedPort<Bottle> inputBottle5;
inputBottle5.open("/localNeuronPotentialReader5:i");
yarpy.connect( "/outputRF1", "/localNeuronPotentialReader1:i");
yarpy.connect( "/outputRF2", "/localNeuronPotentialReader2:i");
yarpy.connect( "/outputRF3", "/localNeuronPotentialReader3:i");
yarpy.connect( "/outputRF4", "/localNeuronPotentialReader4:i");
yarpy.connect( "/outputRF5", "/localNeuronPotentialReader5:i");
vector<double> potential;
while(run)
{
Bottle *tempData1; Bottle *tempData2; Bottle *tempData3; Bottle *tempData4; Bottle *tempData5;
tempData1 = inputBottle1.read(); tempData2 = inputBottle2.read(); tempData3 = inputBottle3.read(); tempData4 = inputBottle4.read();
tempData5 = inputBottle5.read();
for (int i=0; i<tempData2->size(); i++)
{
cout << tempData2->get(i).asDouble() << " ";
}
cout << endl;
}
}
void test3() {
using namespace std;
using namespace ml;
typedef int T;
Timer<float> timer;
timer.start();
Network<T>* aSubNet = new Network<T>();//.. = something else
aSubNet->setInputLayer(new Layer<T>(50));
aSubNet->connect(aSubNet->getInputLayer(), new Layer<T>(1000));
aSubNet->setOutputLayer(aSubNet->getInputLayer()->siblings[0]);
aSubNet->setName("SubNet 1");
Network<T>* network = new Network<T>();
ILayer<T>* l1 = new Layer<T>(100);
ILayer<T>* l2 = new Layer<T>(200);
ILayer<T>* l3 = new Layer<T>(500);
ILayer<T>* l4 = aSubNet;
l1->setName("L1");
l2->setName("L2");
l3->setName("L3");
l4->setName("L4");
network->setInputLayer(l1);
network->connect(l1, l2);
network->connect(l2, l3);
network->connect(l3, l4);
network->connect(l3, l2); // Sigmoid(l1 * W1 + l3 * W4) -> L2 output
network->connect(l4, l2);
network->setOutputLayer(l4);
network->init();
ml::Mat<T> input(1, 100, 1);
network->feed(input);
/*
network->init();
for sample in samples:
out = network->feed(sample);
// todo: back prop out into network
*/
timer.stop();
cout << timer.getTime() << endl;
}
RobotInterface::RobotInterface(PolyDriver* rd, IPositionControl* p, IVelocityControl* v, IEncoders* e, BufferedPort<Vector>* tdp,
std::vector<bool>& af, std::vector<bool>& aj) :
robotDevice(rd),
pos(p),
vel(v),
encs(e),
tactileDataPort(tdp),
activeFingers(af),
activeJoints(aj),
jointsNumber(0),
activeFingersNumber(0),
activeJointsNumber(0),
iteration(0)
{
Network yarp;
// acquire the number of joints
if(!pos->getAxes(&jointsNumber)) printf("ERROR: impossible to get the number of joints\n");
// retrieve the number of active fingers
for(unsigned int i = 0; i < activeFingers.size(); i++)
{
if(activeFingers[i] == true) activeFingersNumber++;
}
// retrieve the number of active joints
for(unsigned int i = 0; i < activeJoints.size(); i++)
{
if(activeJoints[i] == true) activeJointsNumber++;
}
// go to home position (only for simulation, the real robot should be positioned manually)
Vector home = Vector(jointsNumber, HOME_POSITION_VECTOR);
positionMoveHand(home);
// resize vectors so that they are coherent with the number of joints
homePosition.resize(jointsNumber);
// save the current joint position as an home position for each trial
encs->getEncoders(homePosition.data());
// debug
printf("HOME POSITION:\t");
for(int p = 0; p < jointsNumber; p++) printf("%f ", homePosition(p));
// rpc connection for calibration
rpcSkin.open(RPC_CLIENT_PORT.c_str());
yarp.connect(RPC_CLIENT_PORT.c_str(), RPC_SERVER_PORT.c_str());
}
bool test_live_rpc() {
printf("\n*** test_live_rpc()\n");
Network yarp;
yarp.setLocalMode(true);
Demo client;
Server server;
Port client_port,server_port;
client_port.open("/client");
server_port.open("/server");
yarp.connect(client_port.getName(),server_port.getName());
int x = 0;
client.yarp().attachAsClient(client_port);
server.yarp().attachAsServer(server_port);
// Check regular RPC works, even with oneways
Bottle cmd, reply;
cmd.fromString("[add] [one] 5");
client_port.write(cmd,reply);
printf("Cmd %s reply %s\n", cmd.toString().c_str(), reply.toString().c_str());
if (reply.get(0).asInt()!=6) return false;
cmd.fromString("[test] [void] 5");
client_port.write(cmd,reply);
printf("Cmd %s reply %s\n", cmd.toString().c_str(), reply.toString().c_str());
cmd.fromString("[add] [one] 6");
client_port.write(cmd,reply);
printf("Cmd %s reply %s\n", cmd.toString().c_str(), reply.toString().c_str());
if (reply.get(0).asInt()!=7) return false;
cmd.fromString("[test] [1way] 5");
client_port.write(cmd,reply);
printf("Cmd %s reply %s\n", cmd.toString().c_str(), reply.toString().c_str());
cmd.fromString("[add] [one] 7");
client_port.write(cmd,reply);
printf("Cmd %s reply %s\n", cmd.toString().c_str(), reply.toString().c_str());
if (reply.get(0).asInt()!=8) return false;
return true;
}
int main(int argc, char *argv[]) {
printf("This test creates two ports and writes from one to the other.\n");
printf("Make sure no other YARP programs are running.\n");
printf("(or else remove the yarpNetworkSetLocalMode line)\n");
Network yarp;
yarp.setLocalMode(1);
Port p1, p2;
bool ok = p1.open("/test1") && p2.open("/test2");
if (!ok) {
printf("failed to open ports\n");
return 1;
}
ok = yarp.connect("/test1","/test2",NULL);
if (!ok) {
printf("failed to connect ports\n");
return 1;
}
p1.enableBackgroundWrite(true);
MyPortable i1, i2;
i1.val = 15;
printf("Writing (in background)...\n");
p1.write(i1);
printf("Reading...\n");
p2.read(i2);
printf("After read, received %d\n", i2.val);
if (i2.val==15) {
printf("Correct!\n");
} else {
printf("That's not right, something failed.\n");
return 1;
}
p1.close();
p2.close();
return 0;
}
int main(int argc, char *argv[]) {
Property config;
config.fromCommand(argc,argv);
Network yarp;
Port client_port;
std::string servername= config.find("server").asString().c_str();
client_port.open("/demo/client");
if (!yarp.connect("/demo/client",servername.c_str()))
{
std::cout << "Error! Could not connect to server " << servername << std::endl;
return -1;
}
Demo demo;
demo.yarp().attachAsClient(client_port);
PointD point;
point.x = 0;
point.y = 0;
point.z = 0;
PointD offset;
offset.x = 1;
offset.y = 2;
offset.z = 3;
std::cout << "== get_answer ==" << std::endl;
int answer=demo.get_answer();
std::cout << answer << std::endl;
std::cout<<"== add_one =="<<std::endl;
answer = demo.add_one(answer);
std::cout << answer << std::endl;
std::cout<<"== double_down =="<<std::endl;
answer = demo.double_down(answer);
std::cout << answer << std::endl;
std::cout<<"== add_point =="<<std::endl;
point = demo.add_point(point,offset);
std::cout<<("== done! ==\n");
return 0;
}
bool test_defaults_with_rpc() {
printf("\n*** test_defaults_with_rpc()\n");
Network yarp;
yarp.setLocalMode(true);
Server server;
Port client_port,server_port;
client_port.open("/client");
server_port.open("/server");
yarp.connect(client_port.getName(),server_port.getName());
server.yarp().attachAsServer(server_port);
Bottle msg, reply;
msg.fromString("test_tail_defaults");
client_port.write(msg,reply);
printf("%s -> %s\n", msg.toString().c_str(), reply.toString().c_str());
if (reply.get(0).asInt() != 42) return false;
msg.fromString("test_tail_defaults 55");
client_port.write(msg,reply);
printf("%s -> %s\n", msg.toString().c_str(), reply.toString().c_str());
if (reply.get(0).asInt() != 999) return false;
msg.fromString("test longer tail defaults");
client_port.write(msg,reply);
printf("%s -> %s\n", msg.toString().c_str(), reply.toString().c_str());
if (reply.get(0).asVocab() != VOCAB4('f','a','i','l')) return false;
msg.fromString("test longer tail defaults 888");
client_port.write(msg,reply);
printf("%s -> %s\n", msg.toString().c_str(), reply.toString().c_str());
if (reply.get(0).asInt() != 999) return false;
msg.fromString("test longer tail defaults 888 ENUM2 47");
client_port.write(msg,reply);
printf("%s -> %s\n", msg.toString().c_str(), reply.toString().c_str());
if (reply.get(0).asInt() != 47) return false;
return true;
}
int main(int argc, char *argv[]) {
Property config;
config.fromCommand(argc,argv);
Network yarp;
Port client_port;
std::string servername= config.find("server").asString().c_str();
client_port.open("/demo/client");
if (!yarp.connect("/demo/client",servername.c_str()))
{
std::cout << "Error! Could not connect to server " << servername << std::endl;
return -1;
}
Demo demo;
demo.yarp().attachAsClient(client_port);
// Let's chat with the server!
std::cout << "Hey are you up and running?" << std::endl;
while(!demo.is_running())
{
std::cout << "No? Well... start!" << std::endl;
demo.start();
}
std::cout << "Wonderful! I have a question for you... so, what's the answer??" << std::endl;
int32_t answer=demo.get_answer();
std::cout << "What?? " << answer << "?? Are you kidding??";
answer = demo.add_one(answer);
std::cout << " It's definitely " << answer << "!!" << std::endl;
demo.set_answer(answer);
std::cout << "Got it? So, repeat after me: the answer is ... " << demo.get_answer() << "! Great!" << std::endl;
std::cout << "Ok you can relax now, I'll leave you alone" << std::endl;
demo.stop();
std::cout<<"Bye" << std::endl;
return 0;
}
int main(int argc, char *argv[]) {
if (argc!=2) {
fprintf(stderr,"please supply name of rpc port to talk to\n");
return 1;
}
Network yarp;
Port port;
if (!port.open("/motor/client")) {
fprintf(stderr,"Failed to open a port, maybe run: yarpserver\n");
return 1;
}
//yarp.connect("/motor/client",argv[1],"text_ack");
yarp.connect("/motor/client",argv[1]);
Motor motor;
motor.query(port);
int nj = motor.get_axes();
vector<double> poss;
for (int i=0; i<nj; i++) {
poss.push_back(0.5*i);
}
motor.set_poss(poss);
for (int i=0; i<10; i++) {
motor.link().stream();
motor.set_pos(0,i);
motor.link().query();
double v = motor.get_enc(0);
printf("Motor %d at %g of %d axes\n", 0, v, motor.get_axes());
vector<double> all = motor.get_encs();
printf(" all encs:");
for (int i=0; i<(int)all.size(); i++) {
printf(" %d:%g", i, all[i]);
}
printf("\n");
}
port.close();
return 0;
}
int main()
{
char *vars = getenv("QUERY_STRING");
ANetwork::t_frameMonit *frame = (ANetwork::t_frameMonit*) malloc(sizeof(ANetwork::t_frame));
Network net;
std::cout << // "Content-type: application/json"
"Access-Control-Allow-Origin: *" << std::endl << std::endl;
if (!vars || sscanf(vars, "id=%d&data=%s", &(frame->idRequest), &(frame->data)) != 2) {
std::cout << "{\"result\":\"Invalid parameters !\"}" << std::endl;
return (-1);
}
try {
net.init(9999, ANetwork::TCP_MODE);
net.connect("127.0.0.1");
net.getSocket()->write((void*) frame, sizeof(ANetwork::t_frameMonit));
frame = (ANetwork::t_frameMonit*) net.read(sizeof(ANetwork::t_frameMonit));
net.close();
std::cout << "{\"result\":" << frame->data << "}" << std::endl;
} catch (const std::exception &e) {
std::cout << "{\"error\":\"" << e.what() << "\"}" << std::endl;
}
};
bool test_defaults() {
printf("\n*** test_defaults()\n");
Network yarp;
yarp.setLocalMode(true);
Demo client;
Server server;
Port client_port,server_port;
client_port.open("/client");
server_port.open("/server");
yarp.connect(client_port.getName(),server_port.getName());
client.yarp().attachAsClient(client_port);
server.yarp().attachAsServer(server_port);
bool ok1 = client.test_defaults();
bool ok2 = client.test_defaults(14);
printf("42 %d 14 %d\n", ok1?1:0, ok2?1:0);
return ok1 && !ok2;
}
int main(int argc, char *argv[]) {
Property config;
config.fromCommand(argc,argv);
Network yarp;
/* This port will be used to talk to the remote server*/
Port client_port;
std::string servername= config.find("server").asString().c_str();
client_port.open("/demo/client");
/* connect to server */
if (!yarp.connect("/demo/client",servername.c_str()))
{
std::cout << "Error! Could not connect to server " << servername << std::endl;
return -1;
}
/* Instatate proxy object and attach it to the port -- the proxy will use this port to talk to the server*/
Demo demo;
demo.yarp().attachAsClient(client_port);
/* Now we are ready to chat with the server! */
/* Notice that from now on we will invoke only the server methods declared in demo.thrift/Demo.h */
std::cout << "Hey are you up and running?" << std::endl;
while(!demo.is_running())
{
std::cout << "No? Well... start!" << std::endl;
demo.start();
}
std::cout << "Wonderful! I have a question for you... so, what's the answer??" << std::endl;
int32_t answer=demo.get_answer();
std::cout << "What?? " << answer << "?? Are you kidding??";
answer = demo.add_one(answer);
std::cout << " It's definitely " << answer << "!!" << std::endl;
demo.set_answer(answer);
std::cout << "Got it? So, repeat after me: the answer is ... " << demo.get_answer() << "! Great!" << std::endl;
std::cout << "Ok you can relax now, I'll leave you alone" << std::endl;
demo.stop();
std::cout<<"Bye" << std::endl;
return 0;
}
bool test_names_with_spaces() {
printf("\n*** test_names_with_spaces()\n");
Network yarp;
yarp.setLocalMode(true);
Server server;
Port client_port,server_port;
client_port.open("/client");
server_port.open("/server");
yarp.connect(client_port.getName(),server_port.getName());
server.yarp().attachAsServer(server_port);
Bottle msg, reply;
msg.fromString("add_one 42");
client_port.write(msg,reply);
printf("%s -> %s\n", msg.toString().c_str(), reply.toString().c_str());
if (reply.get(0).asInt() != 43) return false;
msg.fromString("add one 52");
client_port.write(msg,reply);
printf("%s -> %s\n", msg.toString().c_str(), reply.toString().c_str());
if (reply.get(0).asInt() != 53) return false;
msg.fromString("get_answer");
client_port.write(msg,reply);
printf("%s -> %s\n", msg.toString().c_str(), reply.toString().c_str());
if (reply.get(0).asInt() != 42) return false;
msg.fromString("get answer");
client_port.write(msg,reply);
printf("%s -> %s\n", msg.toString().c_str(), reply.toString().c_str());
if (reply.get(0).asInt() != 42) return false;
return true;
}
bool test_enums() {
printf("\n*** test_enums()\n");
Network yarp;
yarp.setLocalMode(true);
Demo client;
Server server;
Port client_port,server_port;
client_port.open("/client");
server_port.open("/server");
yarp.connect(client_port.getName(),server_port.getName());
client.yarp().attachAsClient(client_port);
server.yarp().attachAsServer(server_port);
std::vector<DemoEnum> lst1;
lst1.push_back(ENUM1);
lst1.push_back(ENUM2);
std::vector<DemoEnum> lst2 = client.test_enum_vector(lst1);
printf("lst1 %d lst2 %d\n", lst1.size(), lst2.size());
return (lst2.size()==3 && lst1[0]==lst2[0] && lst1[1]==lst2[1]);
}
int main(int argc, char** argv)
{
/*
######################################################################################
############################## GENERAL PORT CREATION #################################
######################################################################################
*/
bool imgIn = imageInPort.open("/home/image/in"); // open the ports and save the return value
bool imgOut = imageOutPort.open("/home/image/out"); // ^^
bool iKinIn = iKinInPort.open("/home/iKinIn"); // ^^
bool iKinOut = iKinOutPort.open("/home/iKinOut"); // ^^
waitKey(50); // give the connections some time to be established
if (!iKinIn || !iKinOut || !imgIn || !imgOut) { // Error handling in case a connection couldn't be established
printf("Failed to create ports.\n"); // notify the user
printf("Maybe you need to start a nameserver (run 'yarpserver' \n");
printf("Press Return to Exit\n");
std::cin.get(); // Wait for the Return Key
return ERROR_DEV_NOT_EXIST; // give an appropriate error code
}
bool yc1 = yarpPort.connect("/icubSim/cam/left", "/home/image/in"); // connect the ports and save the return value
bool yc2 = yarpPort.connect("/iKinGazeCtrl/x:o", iKinInPort.getName()); // ^^
bool yc3 = yarpPort.connect(iKinOutPort.getName(), "/iKinGazeCtrl/xd:i"); // ^^
yarpPort.connect(imageOutPort.getName(), "/view/ase"); // we connect the YarpView -> but in case its not yet open, we don't care
waitKey(250); // give the connections some time to be established
if (!yc1 || !yc2 || !yc3) { // Error handling in case a connection couldn't be established
printf("Failed to connect the ports.\n"); // notify the user
printf("Maybe you need to start a nameserver (run 'yarpserver' \n");
printf("Press Return to Exit\n");
std::cin.get(); // Wait for the Return Key
return ERROR_DEV_NOT_EXIST; // give an appropriate error code
}
/*
######################################################################################
#################################### MAIN LOOP #######################################
######################################################################################
*/
for (;;)//(int i = 0;i < 500;i++)
{
if (int return_val = MainLoop()) // Running the inner while encapsulated, helps managing the memory
return return_val; // catching possible error messages
}
// #################
// hypothetical exit (for using an ending loop)
// #################
yarpPort.disconnect("/icubSim/cam/left", "/home/image/in"); // we disconnect all connections to ensure a nice application exit
yarpPort.disconnect("/iKinGazeCtrl/x:o", iKinInPort.getName());
yarpPort.disconnect(iKinOutPort.getName(), "/iKinGazeCtrl/xd:i");
std::cin.get(); // if we ever get here, wait for user input
return ERROR_SUCCESS;
}
bool test_partial() {
printf("\n*** test_partial()\n");
Network yarp;
yarp.setLocalMode(true);
Server server;
Port client_port,server_port;
client_port.open("/client");
server_port.open("/server");
yarp.connect(client_port.getName(),server_port.getName());
server.yarp().attachAsServer(server_port);
Bottle msg, reply;
msg.fromString("add pair 4 3");
client_port.write(msg,reply);
printf("%s -> %s\n", msg.toString().c_str(), reply.toString().c_str());
if (reply.get(0).asInt() != 7) return false;
msg.fromString("add pair 4");
client_port.write(msg,reply);
printf("(incomplete) %s -> %s\n", msg.toString().c_str(), reply.toString().c_str());
if (reply.get(0).asVocab() != VOCAB4('f','a','i','l')) return false;
msg.fromString("add pair");
reply.fromString("0");
client_port.write(msg,reply);
printf("(incomplete) %s -> %s\n", msg.toString().c_str(), reply.toString().c_str());
if (reply.get(0).asVocab() != VOCAB4('f','a','i','l')) return false;
msg.fromString("add");
reply.fromString("0");
client_port.write(msg,reply);
printf("(incomplete) %s -> %s\n", msg.toString().c_str(), reply.toString().c_str());
if (reply.get(0).asVocab() != VOCAB4('f','a','i','l')) return false;
msg.fromString("");
reply.fromString("0");
client_port.write(msg,reply);
printf("(incomplete) %s -> %s\n", msg.toString().c_str(), reply.toString().c_str());
if (reply.get(0).asVocab() != VOCAB4('f','a','i','l')) return false;
msg.fromString("add pair 10 20");
reply.fromString("0");
client_port.write(msg,reply);
printf("%s -> %s\n", msg.toString().c_str(), reply.toString().c_str());
if (reply.get(0).asInt() != 30) return false;
msg.fromString("test partial 10 (40 50 60) 5");
reply.fromString("0");
client_port.write(msg,reply);
printf("%s -> %s\n", msg.toString().c_str(), reply.toString().c_str());
if (reply.get(0).asInt() != 15) return false;
msg.fromString("test partial 10 (40 50)");
reply.fromString("0");
client_port.write(msg,reply);
printf("%s -> %s\n", msg.toString().c_str(), reply.toString().c_str());
if (reply.get(0).asVocab() != VOCAB4('f','a','i','l')) return false;
return true;
}
int main(int ntargets, char *targets[]) {
Network yarp;
Port port;
string hist_file(getenv("HOME") );
hist_file += "/.yarp_write_history";
read_history(hist_file.c_str());
if (companion_active_port==NULL) {
companion_install_handler();
}
ntargets--;
targets++;
if (!port.open(targets[0])) {
printf("\nCould not open write port.\n");
return 1;
}
ntargets--;
targets++;
if (adminMode) {
port.setAdminMode();
}
bool raw = false;
for (int i=0; i<ntargets; i++) {
if (string(targets[i])=="verbatim") {
raw = true;
} else {
if (!yarp.connect(port.getName().c_str(),targets[i])) {
printf("\nCould not connect ports\n");
return 1;
}
}
}
printf("\nTo stop: CTRL+C ENTER\n\n");
rl_catch_signals = 1;
rl_set_signals();
while (!done) {
rl_on_new_line();
string txt(rl_gets());
if (!done) {
if (txt[0]<32 && txt[0]!='\n' &&
txt[0]!='\r' && txt[0]!='\0' && txt[0]!='\t') {
break; // for example, horrible windows ^D
}
Bottle bot;
if (raw) {
bot.addInt(0);
bot.addString(txt.c_str());
} else {
bot.fromString(txt.c_str());
}
//core.send(bot);
port.write(bot);
}
}
write_history(hist_file.c_str());
companion_active_port = NULL;
if (raw) {
Bottle bot;
bot.addInt(1);
bot.addString("<EOF>");
//core.send(bot);
port.write(bot);
}
port.close();
return 0;
}
int main(char** argv, int argc)
{
Network network;
BufferedPort<ImageOf<PixelRgb> >* imageInputPort = new BufferedPort<ImageOf<PixelRgb> >();
BufferedPort<ImageOf<PixelRgb> >* imageOutputPort = new BufferedPort<ImageOf<PixelRgb> >();
BufferedPort<Bottle>* bbOutputPort = new BufferedPort<Bottle>();
yarp::os::BufferedPort<yarp::os::Bottle > dataPortMec;
imageInputPort->open("/img:i");
imageOutputPort->open("/img:o");
//bbOutputPort->open("/bb:o"); editted VM
network.connect("/icub/camcalib/left/out", "/img:i");
dataPortMec.open("/Shapedata:o");
//string para_yml_file = "data/para_cmp8toys.yml"; //modified VM
string para_yml_file = "data_blocks/para_blocks.yml";
/////////////////////////////////////////////////
// STEP1: initiate
/////////////////////////////////////////////////
bool flag;
CTLdetector detector;
flag = detector.initiate(para_yml_file);
if (!flag) return 0;
/////////////////////////////////////////////////
// STEP2: train
/////////////////////////////////////////////////
flag = detector.train();
if (!flag) return 0;
/////////////////////////////////////////////////
// STEP3: detect
/////////////////////////////////////////////////
int key = 0;
cv::Mat cvMatImage;
std::cout<<"***Detecting..."<<std::endl;
while(key != 27)
{
ImageOf<PixelRgb> *img = imageInputPort->read(true);
cvMatImage.create(img->height(), img->width(), CV_8UC3);
unsigned char* ptr = img->getRawImage();
memcpy(cvMatImage.data, ptr, cvMatImage.cols * cvMatImage.rows * 3);
cv::cvtColor(cvMatImage, cvMatImage, CV_RGB2BGR);
detector.detect(cvMatImage);
//detector.showDetObjs(cvMatImage,Scalar(0,255,0),Scalar(255,255,255),480); //chaged 3.2.2013 Rea changed to orinal size
detector.showDetObjs(cvMatImage,Scalar(0,255,0),Scalar(255,255,255),0);
//detector.dispDetObjs();
std::vector<DetObj> objects;
objects = detector.getDetObjs();
//sending out through image port the image out
/*
if(imageOutputPort->getOutputCount()) {
cv::Mat ppIm = detector.getPostProcessIm();
//cv::cvtColor(ppIm, ppIm, CV_BGR2RGB);
// printf("image %d %d \n", cvMatImage.cols, cvMatImage.rows);
ImageOf<PixelRgb>& tmpImage = imageOutputPort->prepare();
tmpImage.resize(img->width(),img->height());
tmpImage.zero();
unsigned char* ptrd = tmpImage.getRawImage();
unsigned char* ptrs = ppIm.data;
int padding = tmpImage.getPadding();
for (int row = 0; row < img->height(); row++) {
for(int col = 0; col < img->width(); col++) {
*ptrd = 255;
ptrd++;
ptrs++;
*ptrd = 255;
ptrd++;
ptrs++;
*ptrd = 255;
ptrd++;
ptrs++;
//ptrs++;
}
ptrd += padding;
}
//memcpy(ptrd,ptrs, cvMatImage.cols * cvMatImage.rows * 3 );
imageOutputPort->write();
}
*/
Bottle& ShapOp = dataPortMec.prepare();
ShapOp.clear();
//Bottle output = bbOutputPort->prepare();
for (int i = 0; i < objects.size(); i++){
ShapOp.addInt(objects[i].box_tight.x);
ShapOp.addInt(objects[i].box_tight.y);
ShapOp.addInt(objects[i].box_tight.width);
ShapOp.addInt(objects[i].box_tight.height);
ShapOp.addInt(objects[i].id_label);
//ShapOp.addInt(10);
//if want to know the object name: detector.all_obj_cls[objects[i].id_label]
}
// free the memory of the objects.
for (int i=0;i<objects.size();i++){
objects[i].mat_edge_NN_tr.release();
}
objects.clear();
//bbOutputPort->write();
dataPortMec.write();
key = cv::waitKey(100);
}
cvMatImage.release();
std::cout<<"***Done."<<std::endl;
return 0;
}
int main(int argc, char *argv[])
{
Network yarp;
ResourceFinder rf;
BufferedPort<Bottle> inPort, outPort;
rf.setVerbose(true);
rf.configure(argc,argv);
if (rf.check("help"))
{
usage();
return 0;
}
std::string name=rf.check("name",Value("com2gui")).asString();
std::string wholebody=rf.check("wholebody",Value("wholeBodyDynamics")).asString();
bool ok=inPort.open("/"+name+"/COM:i");
ok=ok && outPort.open("/"+name+"/objects:o");
if (!ok)
{
yError() << "Could not open yarp ports - is yarpserver running happily ?";
return EXIT_FAILURE;
}
yarp.connect(outPort.getName(),"/iCubGui/objects","udp");
yarp.connect("/"+wholebody+"/com:o",inPort.getName(),"udp");
yInfo() << "Configured: ";
yInfo() << " name= " << name;
yInfo() << " wholebody= " << wholebody << " (reading from /"+wholebody+"/com:o)";
yInfo() << " writing to /iCubGui/objects";
int i=0;
while (true)
{
Bottle *in = inPort.read();
if (in==NULL) {
yError() << "Failed to read message";
return EXIT_FAILURE;
}
double x=in->get(0).asDouble();
double y=in->get(1).asDouble();
double z=in->get(2).asDouble();
Bottle &obj=outPort.prepare();
obj.clear();
obj.addString("object"); // command to add/update an object
obj.addString("COM");
// object dimensions in mm
// (it will be displayed as an ellipsoid with the tag "COM")
obj.addDouble(50);
obj.addDouble(50);
obj.addDouble(50);
// object position in millimiters
// reference frame: X=fwd, Y=left, Z=up
obj.addDouble(x);
obj.addDouble(y);
obj.addDouble(z);
// object orientation (roll, pitch, yaw) in degrees
obj.addDouble(0);
obj.addDouble(0);
obj.addDouble(0);
// object color (0-255)
obj.addInt(255);
obj.addInt(128);
obj.addInt(128);
// transparency (0.0=invisible 1.0=solid)
obj.addDouble(1.0);
outPort.write(true);
}
return EXIT_SUCCESS;
}
int main(int argc, char** argv)
{
std::string imageInPortName;
std::string stereoInPortName;
std::string magnoInPortName;
std::string saliencyOutPortName;
std::string gazeOutPortName;
std::string facesInPortName;
Network yarp;
if(argc < 6)
{
std::cout << "Not enough arguments. Must provide port name to the input and output ports" << endl;
std::cout << "Exiting ..." << endl;
return -1;
}
else
{
imageInPortName = argv[1];
stereoInPortName = argv[2];
magnoInPortName = argv[3];
facesInPortName = argv[4];
saliencyOutPortName = argv[5];
gazeOutPortName = argv[6];
}
int numGPU = cuda::getCudaEnabledDeviceCount();
if (numGPU == 0)
{
std::cout << "No GPU found or library compiled without GPU support" << endl;
std::cout << "Exiting ..." << endl;
return 0;
}
for (int i = 0; i < numGPU; i++)
{
cuda::setDevice(i);
cuda::DeviceInfo GPUlist;
bool compatible = GPUlist.isCompatible();
if (compatible == false)
{
std::cout << "Library not compiled with appropriate architecture" << endl;
std::cout << "Exiting ..." << endl;
return 0;
}
}
std::cout << "Found " << numGPU << " CUDA enabled device/s" << endl;
cv::cuda::Stream cuStream[5];
yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> > imageInPort;
yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgbFloat> > stereoInPort;
yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelMono> > magnoInPort;
yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelMono> > facesInPort;
yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelFloat> > saliencyOutPort;
yarp::os::Port gazeOutPort;
bool inOpen = imageInPort.open(imageInPortName.c_str());
//bool inOpen2 = stereoInPort.open(stereoInPortName.c_str());
bool inOpen4 = facesInPort.open(facesInPortName.c_str());
//bool inOpen3 = magnoInPort.open(magnoInPortName.c_str());
bool outOpen = saliencyOutPort.open(saliencyOutPortName.c_str());
bool outOpen2 = gazeOutPort.open(gazeOutPortName.c_str());
if(!inOpen | !inOpen4 |!outOpen | !outOpen2)
{
std::cout << "Could not open ports. Exiting" << endl;
return -1;
}
#ifdef devDebug
{
//yarp.connect("/icub/cam/left","/imin","udp+mjpeg+recv.bayer+method.nearest");
//yarp.connect("/icub/cam/left","/imin","udp+mjpeg");
//yarp.connect("/icubcam", "/imin");
yarp.connect("/icub/cam/left","/inputCalibImage", "udp+mjpeg+recv.bayer+method.nearest");
//yarp.connect("/stereo/out","/inputStereoMap");
//yarp.connect("/magno/out", "/magnoInput");
yarp.connect("/salientOut", "/salientFacesIn");
yarp.connect("/saliencyOut","/showSaliency");
yarp.connect("/gazeOut", "/iKinGazeCtrl/mono:i");
}
#endif
//using vision for stereo cameras:
// for all features except for depth, stitch the two images together to remove overlap and thus including peripheral vision.
// after stitching process through retina model and extract features
// for depth, process the overlapping section of the image only then after getting a saliency map, reincorporate this into
// an image whose dimensions are equal to the rest by setting the remaining pixels = 0
//instead of using optical flow for motion detection use magnocellular pathway implemented in retina module to extract motion saliency
//use of several gpus is done by getting a list of devices and setting which gpu does which operation:
//- requires either optimisation of event sequences where each gpu has a separate task to implement or striping of image
//separate tasks could possibly be more processing efficient due to the several 'independent' tasks that need to be carried out
//
bool setup = false;
int count = 0;
Size imageSize;
int border = 100;
int inCount = 0, inCount1 = 0, inCount2 = 0, inCount3 = 0;
int outCount = 0;
int step = 0;
int stepMagno = 0;
int stepStereo = 0;
int stepFaces = 0;
double t = 0.0, time = 0.0;
while(setup == false)
{
inCount = imageInPort.getInputCount();
//inCount1 = stereoInPort.getInputCount();
//inCount2 = magnoInPort.getInputCount();
inCount2 = facesInPort.getInputCount();
if (inCount == 0)
{
cout << "Awaiting input images" << endl;
pauseExec(100);
}
else
{
ImageOf<PixelRgb> *image = imageInPort.read();
//ImageOf<PixelRgbFloat> *stereo = stereoInPort.read();
//ImageOf<PixelMono> *magno = magnoInPort.read();
ImageOf<PixelMono> *faces = facesInPort.read();
if (image != NULL & faces != NULL)
{
count = 0;
step = image->getRowSize() + image->getPadding();
//stepMagno = magno->getRowSize() + magno->getPadding();
//stepStereo = stereo->getRowSize() + stereo->getPadding();
stepFaces = faces->getRowSize() + faces->getPadding();
Mat left_cpuRGB(image->height(), image->width(), CV_8UC3, image->getRawImage(), step);
imageSize = left_cpuRGB.size();
setup = true;
}
}
}
int numPix = imageSize.height*imageSize.width;
char mess[100];
GpuMat Intensity_past(imageSize, CV_32F);
Intensity_past.setTo(Scalar::all(0.0));
//prepare gabor filters
int gaborWidth = 5;
Size gaborSize = Size(gaborWidth, gaborWidth);
Mat cgabor_0, cgabor_45, cgabor_90, cgabor_135;
double sigma = 1, lambda = 2.0, gamma = 1.0, phase = 0;
double Gstep = CV_PI / 4.0;
double orien[4] = { (2 * Gstep), Gstep, 0, (3 * Gstep) };
cgabor_0 = getGaborKernel(gaborSize, sigma, orien[0], lambda, gamma, phase, CV_32F);
cgabor_45 = getGaborKernel(gaborSize, sigma, orien[1], lambda, gamma, phase, CV_32F);
cgabor_90 = getGaborKernel(gaborSize, sigma, orien[2], lambda, gamma, phase, CV_32F);
cgabor_135 = getGaborKernel(gaborSize, sigma, orien[3], lambda, gamma, phase, CV_32F);
GpuMat Gabor0_0, Gabor0_90, Gabor0_45, Gabor0_135;
Ptr<Filter> gaborFilt0 = cuda::createLinearFilter(CV_32F, CV_32F, cgabor_0, Point(-1, -1), BORDER_CONSTANT, 0);
Ptr<Filter> gaborFilt90 = cuda::createLinearFilter(CV_32F, CV_32F, cgabor_90, Point(-1, -1), BORDER_CONSTANT, 0);
Ptr<Filter> gaborFilt45 = cuda::createLinearFilter(CV_32F, CV_32F, cgabor_45, Point(-1, -1), BORDER_CONSTANT, 0);
Ptr<Filter> gaborFilt135 = cuda::createLinearFilter(CV_32F, CV_32F, cgabor_135, Point(-1, -1), BORDER_CONSTANT, 0);
//prepare pyramid sizes array
const int numPyrLevels = 8;
int numFMaps = 6;
int conspMapLevel = 4;
vector<int> centreVec;
centreVec.push_back(2);
centreVec.push_back(3);
centreVec.push_back(4);
vector<int> surroundOffsetVec;
surroundOffsetVec.push_back(3);
surroundOffsetVec.push_back(4);
vector<GpuMat> gMagnoPYR, gMagnoFMaps, gIntensityPYR, gIntensityFMaps, RGantPYR, RGantFMaps, BYantPYR, BYantFMaps, Gabor0PYR, Gabor90PYR, Gabor45PYR, Gabor135PYR, Gabor0FMaps, Gabor90FMaps, Gabor45FMaps, Gabor135FMaps;
vector<Mat> intensity;
Mat intPast, IOR;
GpuMat gTempY, gTempX, gRG;
Size pyrSizes[numPyrLevels+1];
Rect pyrRect[numPyrLevels+1];
Point origin = Point(0, 0);
int tempH, tempW;
Intensity_past.download(intPast);
for (int i = 0; i <= numPyrLevels; i++)
{
tempH = ceil(imageSize.height / pow(2,i));
tempW = ceil(imageSize.width / pow(2, i));
pyrSizes[i] = Size(tempW, tempH);
pyrRect[i] = Rect(origin, pyrSizes[i]);
gIntensityPYR.push_back(Intensity_past.clone());
//gMagnoPYR.push_back(Intensity_past.clone());
RGantPYR.push_back(Intensity_past.clone());
BYantPYR.push_back(Intensity_past.clone());
Gabor0PYR.push_back(Intensity_past.clone());
Gabor90PYR.push_back(Intensity_past.clone());
Gabor45PYR.push_back(Intensity_past.clone());
Gabor135PYR.push_back(Intensity_past.clone());
if (i < numFMaps)
{
gIntensityFMaps.push_back(Intensity_past.clone());
//gMagnoFMaps.push_back(Intensity_past.clone());
RGantFMaps.push_back(Intensity_past.clone());
BYantFMaps.push_back(Intensity_past.clone());
Gabor0FMaps.push_back(Intensity_past.clone());
Gabor90FMaps.push_back(Intensity_past.clone());
Gabor45FMaps.push_back(Intensity_past.clone());
Gabor135FMaps.push_back(Intensity_past.clone());
}
intensity.push_back(intPast);
}
GpuMat gIntensityConspMap(pyrSizes[conspMapLevel], CV_32F);
//GpuMat gMagnoConspMap(pyrSizes[conspMapLevel], CV_32F);
GpuMat RGantConspMap(pyrSizes[conspMapLevel], CV_32F);
GpuMat BYantConspMap(pyrSizes[conspMapLevel], CV_32F);
GpuMat Gabor0ConspMap(pyrSizes[conspMapLevel], CV_32F);
GpuMat Gabor90ConspMap(pyrSizes[conspMapLevel], CV_32F);
GpuMat Gabor45ConspMap(pyrSizes[conspMapLevel], CV_32F);
GpuMat Gabor135ConspMap(pyrSizes[conspMapLevel], CV_32F);
GpuMat OrientationConspMap(pyrSizes[conspMapLevel], CV_32F);
GpuMat IORMap(pyrSizes[conspMapLevel], CV_32F);
GpuMat gMask(Size(3, numPix), CV_8UC1);
IORMap.setTo(Scalar::all(0.0));
//initialise maximum filtering
//Ptr<Filter> maxFilt = cuda::createBoxMaxFilter(CV_8UC1, Size(5, 5), Point(-1, -1), BORDER_CONSTANT, 0);
Mat kernel, cSaliencyInt;
kernel = getStructuringElement(MORPH_RECT, Size(5, 5), Point(-1, -1));
Ptr<Filter> maxFilt = cuda::createMorphologyFilter(MORPH_DILATE, CV_8UC1, kernel, Point(-1, -1), 1);
double avgFps, cumuTime = 0, avgTime, minSalVal, maxSalVal;
Point minSalPoint, maxSalPoint;
Mat m1, m2, m3, m0, imageFloat, cSaliency, magnoFloat, facesFloat;
GpuMat gTemp3, gMaxRGB, gMinRG, gMagnoDisparity, gRightInt, gFaces, gFacesConspMap, gMagno3D, gLeftInt, gLeftROI, gMagnoROI, gMagnoLeftCorrect1, gMagnoRightCorrect1, gMagnoRightInt, gMagnoLeftInt, gRight, gR, gMagnoLeft, gMagnoRight, gMagno0, gG, gB, gIntensityCurr, gIntensity0, gLeft, pano, RGant0, temp, BYant0, Flicker0, gTemp, gTemp2, gTemp4, gTemp5, gTemp6, gTemp7, gTemp8, gSaliency, ColorConspMap;
vector<GpuMat> bgr, rg;
int countFrames = 0, countGaze = 0;
Scalar arrMean, arrStddev;
int lenX = pow(2,conspMapLevel);
int lenY = lenX;
int salX = 0, salY = 0;
int cumulativeX = 0, cumulativeY = 0;
Rect salientRegion, saliencyMapRegion;
Scalar meanPos;
double decayFactor = 0.95;
int gazePeriod = 6;
int midX = floor(gazePeriod/2);
std::vector<int> vecX(gazePeriod);
std::vector<int> vecY(gazePeriod);
std::vector<int> sortX(gazePeriod);
vector<int>::iterator medianIdx;
int medianX;
int salIdx;
//std::deque<Point> saccadePts;
//saccadePts.push_back(Point(0,0));
//saccadePts.push_back(Point(0,0));
//saccadePts.push_back(Point(0,0));
Point2f avgPoint;
while(true)
{
inCount = imageInPort.getInputCount();
//inCount1 = stereoInPort.getInputCount();
//inCount2 = magnoInPort.getInputCount();
inCount3 = facesInPort.getInputCount();
outCount = saliencyOutPort.getOutputCount();
if(inCount == 0 || inCount3 == 0 || outCount == 0)
{
std::cout << "Awaiting input and output connections" << endl;
pauseExec(100);
}
else
{
ImageOf<PixelRgb> *imageIn = imageInPort.read();
//ImageOf<PixelRgbFloat> *stereoIn = stereoInPort.read();
//ImageOf<PixelMono> *magnoIn = magnoInPort.read();
ImageOf<PixelMono> *facesIn = facesInPort.read();
if ((imageIn != NULL) & (facesIn != NULL))
{
t = (double)getTickCount();
Mat inImage(imageIn->height(), imageIn->width(), CV_8UC3, imageIn->getRawImage(), step);
//Mat stereoImage(stereoIn->height(), stereoIn->width(), CV_32FC3, stereoIn->getRawImage(), stepStereo);
//Mat magnoImage(magnoIn->height(), magnoIn->width(), CV_8UC1, magnoIn->getRawImage(), stepMagno);
Mat facesImage(facesIn->height(), facesIn->width(), CV_8UC1, facesIn->getRawImage(), stepFaces);
cv::cvtColor(inImage, inImage, COLOR_RGB2BGR, -1);
//magnoImage.convertTo(magnoFloat, CV_32FC1);
//magnoFloat /= 255;
inImage.convertTo(imageFloat, CV_32FC3);
imageFloat /= 255;
facesImage.convertTo(facesFloat, CV_32FC1);
facesFloat /= 255;
gLeft.upload(imageFloat);
//gMagno0.upload(magnoFloat);
gFaces.upload(facesFloat);
//------------ start of saliency---------------------//
//create intensity map
cuda::split(gLeft, bgr, cuStream[0]);
cuda::add(bgr[1], bgr[0], gTemp2, noArray(), -1, cuStream[0]);
cuda::addWeighted(gTemp2, 0.33333, bgr[2], 0.33333, 0, gIntensity0, -1, cuStream[0]);
cuda::threshold(gIntensity0, gMask, 0.1, 1, THRESH_BINARY, cuStream[0]);
cudaConspMap(&gIntensity0, &gIntensityPYR, &gIntensityFMaps, &gIntensityConspMap, numPyrLevels, centreVec, surroundOffsetVec, conspMapLevel, pyrSizes, maxFilt, cuStream[1]);
//creating colour antagonist maps
cuda::max(bgr[0], bgr[1], gTemp2, cuStream[0]);
cuda::max(gTemp2, bgr[2], gTemp3, cuStream[0]);
cuda::multiply(gTemp3, gMask, gMaxRGB, 1.0, -1, cuStream[0]);
cuda::min(bgr[2], bgr[1], gMinRG, cuStream[0]);
cuda::subtract(bgr[2], bgr[1], RGant0, noArray(), -1, cuStream[0]);
cuda::divide(RGant0, gMaxRGB, RGant0, 1, -1, cuStream[0]);
cudaConspMap(&RGant0, &RGantPYR, &RGantFMaps, &RGantConspMap, numPyrLevels, centreVec, surroundOffsetVec, conspMapLevel, pyrSizes, maxFilt, cuStream[1]);
cuda::add(gIntensityConspMap, RGantConspMap, gSaliency);
cuda::subtract(bgr[0], gMinRG, BYant0, noArray(), -1, cuStream[0]);
cuda::divide(BYant0, gMaxRGB, BYant0, 1, -1, cuStream[0]);
cudaConspMap(&BYant0, &BYantPYR, &BYantFMaps, &BYantConspMap, numPyrLevels, centreVec, surroundOffsetVec, conspMapLevel, pyrSizes, maxFilt, cuStream[1]);
cuda::add(gSaliency, BYantConspMap, gSaliency);
cuda::resize(gFaces, gFacesConspMap, pyrSizes[conspMapLevel], 0.0, 0.0, 1.0, cuStream[1]);
//--------------------------------------------------------//
//flicker map (use retina magno channel)
//cudaConspMap(&gMagno0, &gMagnoPYR, &gMagnoFMaps, &gMagnoConspMap, numPyrLevels, centreVec, surroundOffsetVec, conspMapLevel, pyrSizes, maxFilt, cuStream[1]);
//gabor filtering on intensity map
gaborFilt0->apply(gIntensity0, Gabor0_0, cuStream[0]);
cudaConspMap(&Gabor0_0, &Gabor0PYR, &Gabor0FMaps, &Gabor0ConspMap, numPyrLevels, centreVec, surroundOffsetVec, conspMapLevel, pyrSizes, maxFilt, cuStream[1]);
gaborFilt90->apply(gIntensity0, Gabor0_90, cuStream[0]);
cudaConspMap(&Gabor0_90, &Gabor90PYR, &Gabor90FMaps, &Gabor90ConspMap, numPyrLevels, centreVec, surroundOffsetVec, conspMapLevel, pyrSizes, maxFilt, cuStream[1]);
gaborFilt45->apply(gIntensity0, Gabor0_45, cuStream[0]);
cudaConspMap(&Gabor0_45, &Gabor45PYR, &Gabor45FMaps, &Gabor45ConspMap, numPyrLevels, centreVec, surroundOffsetVec, conspMapLevel, pyrSizes, maxFilt, cuStream[1]);
gaborFilt135->apply(gIntensity0, Gabor0_135, cuStream[0]);
cudaConspMap(&Gabor0_135, &Gabor135PYR, &Gabor135FMaps, &Gabor135ConspMap, numPyrLevels, centreVec, surroundOffsetVec, conspMapLevel, pyrSizes, maxFilt, cuStream[1]);
OrientationConspMap.setTo(Scalar::all(0.0));
normImage(&Gabor0ConspMap, maxFilt, &Gabor0ConspMap, pyrSizes[conspMapLevel], cuStream[1]);
cuda::add(OrientationConspMap, Gabor0ConspMap, OrientationConspMap);
normImage(&Gabor90ConspMap, maxFilt, &Gabor90ConspMap, pyrSizes[conspMapLevel], cuStream[1]);
cuda::add(OrientationConspMap, Gabor90ConspMap, OrientationConspMap);
normImage(&Gabor45ConspMap, maxFilt, &Gabor45ConspMap, pyrSizes[conspMapLevel], cuStream[1]);
cuda::add(OrientationConspMap, Gabor45ConspMap, OrientationConspMap);
normImage(&Gabor135ConspMap, maxFilt, &Gabor135ConspMap, pyrSizes[conspMapLevel], cuStream[1]);
cuda::add(OrientationConspMap, Gabor135ConspMap, OrientationConspMap);
cuda::addWeighted(gSaliency, 0.333, OrientationConspMap, 0.333, 0, gSaliency, -1 ,cuStream[1]);
//cuda::addWeighted(gSaliency, 0.75 , gMagnoConspMap, 0.25, 0, gSaliency, -1, cuStream[1]);
cuda::addWeighted(gSaliency, 0.5, gFacesConspMap, 0.5, 0, gSaliency, -1, cuStream[1]);
cuda::multiply(gSaliency,255, gSaliency, 1.0, -1, cuStream[1]);
cuda::subtract(gSaliency, IORMap, gSaliency, noArray(), CV_32FC1, cuStream[1]);
cuda::minMaxLoc(gSaliency, &minSalVal, &maxSalVal, &minSalPoint, &maxSalPoint, noArray()); //--------------------------------------------------//
gSaliency.download(cSaliency);
salX = maxSalPoint.x;
salY = maxSalPoint.y;
if(salX == 0) salX = 1;
if(salY == 0) salY = 1;
saliencyMapRegion = Rect(salX, salY, 1, 1);
salX = floor(salX*pow(2,conspMapLevel));
salY = floor(salY*pow(2,conspMapLevel));
//cumulativeX += salX;
//cumulativeY += salY;
vecX[countGaze] = salX;
vecY[countGaze] = salY;
salientRegion = Rect(salX-ceil(lenX/2), salY-ceil(lenY/2), lenX, lenY);
rectangle(inImage, salientRegion, Scalar(0, 0, 200), 3, 8, 0);
imshow("salientAttention", inImage);
waitKey(1);
countGaze++;
if(countGaze == gazePeriod)
{
countGaze = 0;
sortX = vecX;
nth_element(sortX.begin(), sortX.begin()+midX, sortX.end());
medianX = sortX[midX];
medianIdx = std::find(vecX.begin(), vecX.end(), medianX);
salIdx = medianIdx - vecX.begin();
//vecX.begin()
Bottle gaze;
gaze.clear();
gaze.addString("left");
gaze.addDouble(vecX[salIdx]);
gaze.addDouble(vecY[salIdx]);
gaze.addDouble(1.0);
gazeOutPort.write(gaze);
cout << "gaze written" << endl;
cumulativeX = 0;
cumulativeY = 0;
cuda::multiply(IORMap, decayFactor, IORMap, 1.0, CV_32FC1, cuStream[1]);
cuda::add(IORMap(saliencyMapRegion), 500, IORMap(saliencyMapRegion), noArray(), CV_32FC1, cuStream[1]);
}
time = (1000/(getTickFrequency() / ((double)getTickCount() - t)));
cumuTime += time;
countFrames++;
avgTime = cumuTime / countFrames;
avgFps = 1000 / avgTime;
if (countFrames == 50)
{
cumuTime = 0;
countFrames = 0;
}
//gDisparityBM.download(cDisparity);
//g3D.download(cDisparity);
//imshow("Disparity", cDisparity);
//gMagnoDisparity.download(cMagnoDisparity);
//gMagno3D.download(cMagnoDisparity);
//imshow("Magno Disparity", cMagnoDisparity);
//waitKey(1);
//gIntensityConspMap.download(cSaliency, cuStream[1]);
std::sprintf(mess, "Avg Time= %3.4f, Avg FPS = %3.2f", avgTime, avgFps);
//putText(cSaliency, mess, cvPoint(30, 30), FONT_HERSHEY_COMPLEX, 0.2, cvScalar(200, 0, 0), 1, CV_AA);
std::cout << mess << endl;
//for (int i = 0; i < numFMaps; i++)
//{
// gIntensityPYR[i].download(intensity[i]);
// if (i == 0) putText(intensity[i], mess, cvPoint(30, 30), FONT_HERSHEY_COMPLEX, 0.5, cvScalar(200, 0, 0), 1, CV_AA);
// sprintf(win, "window %d", i);
// namedWindow(win, WINDOW_NORMAL);
//imshow(win, intensity[i]);
//}
//cv::namedWindow("Saliency Map", WINDOW_NORMAL);
//cv::imshow("Saliency Map", cSaliency);
//cv::waitKey(1);
//cSaliency.convertTo(cSaliencyInt, CV_8UC1, 1, 0);
yarp::sig::ImageOf<yarp::sig::PixelFloat>& saliencyYarpOut = saliencyOutPort.prepare();
CVtoYarp(cSaliency, saliencyYarpOut);
saliencyOutPort.write();
}
}
}
}
int main(int numArgs, char** args)
{
// Initial parameters of the Blob Tracker
double x0 = 128/2;
double y0 = 128/2;
double sig_x = 5;
double sig_y = 5;
double sig_xy = 0;
double alpha_pos = 0.001;
double alpha_shape = 0.000;
// if(numArgs > 1)
// {
// alpha_pos = (double) *args[0];
// alpha_shape = (double) *args[1];
// }
// else if (numArgs > 0)
// {
// alpha_pos = (double) *args[0];
// }
// Constant for the representation of the ellipses
double K = 2.2;
BlobTracker blob_tracker(x0, y0, sig_x, sig_y, sig_xy, alpha_pos, alpha_shape);
// Threshold for updating the position
double min_p = 0.0;
int min_nb_ev = 1;
// Vector containing the degrees (in radians) for plotting the ellipse
std::vector<double> theta;
for(int ii=0; ii<360; ii++){
theta.push_back(ii*M_PI/180);
}
Network yarp;
string dvsPort = "/aexGrabber/eventBottle:o";
//ImageOf<PixelRGB> clusterImage;
BufferedPort<eventBottle> readDVS;
(readDVS).open("/dvsReadLocal:i");
printf("Created port /dvsReadLocal:i to get data from DVS!!\n\n");
bool connectionOk = yarp.connect(dvsPort.c_str(), "/dvsReadLocal:i");
printf("Waiting for aexGrabber port to go alive!!!\n");
while (!connectionOk){
connectionOk = yarp.connect(dvsPort.c_str(), "/dvsReadLocal:i");
}
printf("Connected to iCub's DVS port\n");
Port imagePort;
imagePort.open("/David/imagePort:o");
yarp::sig::ImageOf<yarp::sig::PixelRgb> image;
image.resize(128,128);
image.zero();
// Define cluster init position.
// Num clusters = 5 (const init);
const int numClusters = 5;
// Cluster events = 0 (variable init);
Vector numEventsPerCluster;
numEventsPerCluster.resize(5);
Vector currentEventNumbers, previousEventNumbers;
currentEventNumbers.resize(128,128);
eventBottle *evBottle;
bool run = true;
double a, b, alpha, cen_x, cen_y;
printf("About to enter the while loop\n");
while(run){
// printf("Read\n");
// With each event shift cluster position to new value.
evBottle = readDVS.read();
// printf("Done\n");
cout << (*evBottle).get_sizeOfPacket() << endl;
Bottle *bottle = (*evBottle).get_packet();
emorph::ecodec::eEventQueue q;
emorph::ecodec::eEvent::decode(*bottle, q);
int sizeOfQ = q.size();
// printf("Received %i events \n", sizeOfQ);
// If the number of events is lower than a predefined threshold, then we assume we are getting just noise, and we do nothing
if(sizeOfQ > min_nb_ev){
int ev_x, ev_y, pol, channel;
// We go through all the events
for (int ii=0; ii < sizeOfQ; ii++){
if (q[ii]->getType() == "AE"){
emorph::ecodec::AddressEvent *ev = static_cast<emorph::ecodec::AddressEvent*>(q[ii]);
if (ev->isValid())
{
ev_x = ev->getY();
ev_y = 127-ev->getX();
pol = ev->getPolarity();
channel = ev->getChannel();
if (channel == 0)
{
double p = blob_tracker.compute_p(ev_x, ev_y);
printf("Probabilities %f\n", p);
if(p>min_p && blob_tracker.dist2Event(ev_x, ev_y) < 30){
blob_tracker.update_position(ev_x, ev_y);
}
blob_tracker.get_ellipse_parameters(cen_x, cen_y, a, b, alpha);
printf ("X = %d, Y=%d, Cen_X= %f, Cen_Y= %f \n", ev_x, ev_y, cen_x, cen_y);
image.pixel(ev_x, ev_y).r = 255;
}
}
}
}
// We update the image of the ellipse
blob_tracker.get_ellipse_parameters(cen_x, cen_y, a, b, alpha);
a*=K;
b*=K;
double c_a = cos(alpha);
double s_a = sin(alpha);
for(int ii=0; ii<theta.size(); ii++){
double x0 = cen_x + a*cos(theta[ii]);
double y0 = cen_y + b*sin(theta[ii]);
int x = round(x0*c_a - y0*s_a);
int y = round(x0*s_a + y0*c_a);
if(x>0 && x<128 && y>0 && y<128)
image.pixel(x, y).b = 255;
}
}
printf("Sending image \n");
imagePort.write(image);
image.zero();
}
//
//eventPacket.();
// Add event to this cluster
// Main cluster = max event clusters
// Get [x,y] for each eye
// Do gaze control using these values.
// Reset all clusters
}
