void beggingForSemaphore(void* p)
{
SoftTimer myTimer(50);
while(1)
{
if(xSemaphoreTake(buttonSemaphore, portMAX_DELAY))
{
uart0_puts("TASK HAS SEMAPHORE!");
xSemaphoreGive(buttonSemaphore);
myTimer.reset();
while(!myTimer.expired())
{
}
}
}
}
//////////////////////////////////////////////////////////////////////////////////
//
// 入口点函数
//
//////////////////////////////////////////////////////////////////////////////////
int main( void )
{
lifeInfo emberInfo( 30, 20, 200, 100, "ember" );
lifeInfo evilInfo( 20, 15, 100, 100, "evil" );
srand( (unsigned int)time(NULL) );
timer myTimer( 200 );
emberManager em( emberInfo, "ember", "testModel1.txt",
evilInfo, "jack", "testModel2.txt" );
while ( 1 )
{
if( myTimer.isTimeToDoSomething() )
{
em.update();
}
}
return 0;
}
Foam::fileStat::fileStat(const fileName& fName, const unsigned int maxTime)
{
// Work on volatile
volatile bool locIsValid = false;
timer myTimer(maxTime);
if (!timedOut(myTimer))
{
if (::stat(fName.c_str(), &status_) != 0)
{
locIsValid = false;
}
else
{
locIsValid = true;
}
}
// Copy into (non-volatile, possible register based) member var
isValid_ = locIsValid;
}
void process()
{
/*
static int tmc=0;
tmc++;
if(tmc==30)
{
tmc=0;
set_indoor();
}
*/
int x, y;
//初始化背景
if(bInited==0)
{
static int initCount=0;
if(initCount<30)
{
initCount++;
img_copy(&BACK, &Y);
}
else
{
bInited=1;
img_fill(&UBAK, 0);
set_target_pan_tilt(0, 0);
set_zoom(1);
}
return;
}
img_fill(&COMP, 0);
img_fill(&COMPB, 0);
img_fill(&CLUS, 0);
img_fill(&LABEL, 0);
//计算准备
for (y = masky1; y < masky2; y++)
for (x = maskx1; x < maskx2; x++)
{
COMP.data[y][x] = abs(Y.data[y][x] - PREV.data[y][x]) > compare_th ? 255 : 0;
if(Y.data[y][x] < dark || PREV.data[y][x] < dark)
COMP.data[y][x] = abs(Y.data[y][x] - PREV.data[y][x]) > compare_th*dark_th ? 255 : 0;
COMPB.data[y][x] = abs(Y.data[y][x] - BACK.data[y][x]) > compare_th2 ? 255 : 0;
if(Y.data[y][x] < dark || BACK.data[y][x] < dark)
COMPB.data[y][x] = abs(Y.data[y][x] - BACK.data[y][x]) > compare_th2*dark_th ? 255 : 0;
}
img_copy(&PREV, &Y);
//选择性更新背景
updataBack();
//聚类
cluster();
img_copy(&LABEL, &CLUS);
//连通域
int k=label();
img_plot_rect(&Y, maskx1, masky1,
maskx2 - maskx1, masky2 - masky1, 128);
//输出连通域信息
int i=0;
mon("count: %d\n", k);
for(i=0;i<k;i++)
{
mon("area%d: %d\n", i, Box[i].area);
}
float p=0,t=0,z=1;
int ml=IMG_W,mr=0,cx=0,dx;
for(i=0;i<k;i++)
{
if(Box[i].left < ml) ml = Box[i].left;
if(Box[i].right > mr) mr = Box[i].right;
}
cx=(ml+mr)/2;
dx=(mr-ml)*1.3;
p = (float)(cx - IMG_W/2) / (IMG_W/2) * (PAN_RANGE/2);
z = CAM_RANGE / ((float)dx / IMG_W * PAN_RANGE);
if(z<1.0)
z=1;
printf("--------%d\n",k);
myTimer(&p,&t,&z,k);
set_target_pan_tilt(p, t);
set_zoom(z);
mon("PZ:%f,%f\n", p, z);
}
bool Foam::ping
(
const word& destName,
const label destPort,
const label timeOut
)
{
char *serverAddress;
struct in_addr *ptr;
struct hostent *hostPtr;
volatile int sockfd;
struct sockaddr_in destAddr; // will hold the destination addr
u_int addr;
if ((hostPtr = gethostbyname(destName.c_str())) == NULL)
{
FatalErrorIn
(
"Foam::ping(const word&, const label)"
) << "gethostbyname error " << h_errno << " for host " << destName
<< abort(FatalError);
}
// Get first of the SLL of addresses
serverAddress = *(hostPtr->h_addr_list);
ptr = reinterpret_cast<struct in_addr*>(serverAddress);
addr = ptr->s_addr;
// Allocate socket
sockfd = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd < 0)
{
FatalErrorIn
(
"Foam::ping(const word&, const label)"
) << "socket error"
<< abort(FatalError);
}
// Fill sockaddr_in structure with dest address and port
memset (reinterpret_cast<char *>(&destAddr), '\0', sizeof(destAddr));
destAddr.sin_family = AF_INET;
destAddr.sin_port = htons(ushort(destPort));
destAddr.sin_addr.s_addr = addr;
timer myTimer(timeOut);
if (timedOut(myTimer))
{
// Setjmp from timer jumps back to here
fdClose(sockfd);
return false;
}
if
(
connect
(
sockfd,
reinterpret_cast<struct sockaddr*>(&destAddr),
sizeof(struct sockaddr)
) != 0
)
{
// Connection refused. Check if network was actually used or not.
int connectErr = errno;
fdClose(sockfd);
if (connectErr == ECONNREFUSED)
{
return true;
}
//perror("connect");
return false;
}
fdClose(sockfd);
return true;
}
void marginalize(
std::vector<aslam::backend::DesignVariable*>& inDesignVariables,
std::vector<aslam::backend::ErrorTerm*>& inErrorTerms,
int numberOfInputDesignVariablesToRemove,
bool useMEstimator,
boost::shared_ptr<aslam::backend::MarginalizationPriorErrorTerm>& outPriorErrorTermPtr,
Eigen::MatrixXd& outCov,
std::vector<aslam::backend::DesignVariable*>& outDesignVariablesInRTop,
size_t numTopRowsInCov,
size_t /* numThreads */)
{
SM_WARN_STREAM_COND(inDesignVariables.size() == 0, "Zero input design variables in the marginalizer!");
// check for duplicates!
std::unordered_set<aslam::backend::DesignVariable*> inDvSetHT;
for(auto it = inDesignVariables.begin(); it != inDesignVariables.end(); ++it)
{
auto ret = inDvSetHT.insert(*it);
SM_ASSERT_TRUE(aslam::Exception, ret.second, "Error! Duplicate design variables in input list!");
}
std::unordered_set<aslam::backend::ErrorTerm*> inEtSetHT;
for(auto it = inErrorTerms.begin(); it != inErrorTerms.end(); ++it)
{
auto ret = inEtSetHT.insert(*it);
SM_ASSERT_TRUE(aslam::Exception, ret.second, "Error! Duplicate error term in input list!");
}
SM_DEBUG_STREAM("NO duplicates in input design variables or input error terms found.");
// Partition the design varibles into removed/remaining.
int dimOfDesignVariablesToRemove = 0;
std::vector<aslam::backend::DesignVariable*> remainingDesignVariables;
int k = 0;
size_t dimOfDvsInTopBlock = 0;
for(std::vector<aslam::backend::DesignVariable*>::const_iterator it = inDesignVariables.begin(); it != inDesignVariables.end(); ++it)
{
if (k < numberOfInputDesignVariablesToRemove)
{
dimOfDesignVariablesToRemove += (*it)->minimalDimensions();
} else
{
remainingDesignVariables.push_back(*it);
}
if(dimOfDvsInTopBlock < numTopRowsInCov)
{
outDesignVariablesInRTop.push_back(*it);
}
dimOfDvsInTopBlock += (*it)->minimalDimensions();
k++;
}
// store original block indices to prevent side effects
std::vector<int> originalBlockIndices;
std::vector<int> originalColumnBase;
// assign block indices
int columnBase = 0;
for (size_t i = 0; i < inDesignVariables.size(); ++i) {
originalBlockIndices.push_back(inDesignVariables[i]->blockIndex());
originalColumnBase.push_back(inDesignVariables[i]->columnBase());
inDesignVariables[i]->setBlockIndex(i);
inDesignVariables[i]->setColumnBase(columnBase);
columnBase += inDesignVariables[i]->minimalDimensions();
}
int dim = 0;
std::vector<size_t> originalRowBase;
for(std::vector<aslam::backend::ErrorTerm*>::iterator it = inErrorTerms.begin(); it != inErrorTerms.end(); ++it)
{
originalRowBase.push_back((*it)->rowBase());
(*it)->setRowBase(dim);
dim += (*it)->dimension();
}
aslam::backend::DenseQrLinearSystemSolver qrSolver;
qrSolver.initMatrixStructure(inDesignVariables, inErrorTerms, false);
SM_INFO_STREAM("Marginalization optimization problem initialized with " << inDesignVariables.size() << " design variables and " << inErrorTerms.size() << " error terrms");
SM_INFO_STREAM("The Jacobian matrix is " << dim << " x " << columnBase);
qrSolver.evaluateError(1, useMEstimator);
qrSolver.buildSystem(1, useMEstimator);
const Eigen::MatrixXd& jacobian = qrSolver.getJacobian();
const Eigen::VectorXd& b = qrSolver.e();
// check dimension of jacobian
int jrows = jacobian.rows();
int jcols = jacobian.cols();
int dimOfRemainingDesignVariables = jcols - dimOfDesignVariablesToRemove;
//int dimOfPriorErrorTerm = jrows;
// check the rank
Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(jacobian);
//lu_decomp.setThreshold(1e-20);
double threshold = lu_decomp.threshold();
int rank = lu_decomp.rank();
int fullRank = std::min(jacobian.rows(), jacobian.cols());
SM_DEBUG_STREAM("Rank of jacobian: " << rank << " (full rank: " << fullRank << ", threshold: " << threshold << ")");
bool rankDeficient = rank < fullRank;
if(rankDeficient)
{
SM_WARN("Marginalization jacobian is rank deficient!");
}
//SM_ASSERT_FALSE(aslam::Exception, rankDeficient, "Right now, we don't want the jacobian to be rank deficient - ever...");
Eigen::MatrixXd R_reduced;
Eigen::VectorXd d_reduced;
if (jrows < jcols)
{
SM_THROW(aslam::Exception, "underdetermined LSE!");
// underdetermined LSE, don't do QR
R_reduced = jacobian.block(0, dimOfDesignVariablesToRemove, jrows, jcols - dimOfDesignVariablesToRemove);
d_reduced = b;
} else {
// PTF: Do we know what will happen when the jacobian matrix is rank deficient?
// MB: yes, bad things!
// do QR decomposition
sm::timing::Timer myTimer("QR Decomposition");
Eigen::HouseholderQR<Eigen::MatrixXd> qr(jacobian);
Eigen::MatrixXd Q = qr.householderQ();
Eigen::MatrixXd R = qr.matrixQR().triangularView<Eigen::Upper>();
Eigen::VectorXd d = Q.transpose()*b;
myTimer.stop();
if(numTopRowsInCov > 0)
{
sm::timing::Timer myTimer("Covariance computation");
Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(R);
Eigen::MatrixXd Rinv = lu_decomp.inverse();
Eigen::MatrixXd covariance = Rinv * Rinv.transpose();
outCov = covariance.block(0, 0, numTopRowsInCov, numTopRowsInCov);
myTimer.stop();
}
// size_t numRowsToKeep = rank - dimOfDesignVariablesToRemove;
// SM_ASSERT_TRUE_DBG(aslam::Exception, rankDeficient || (numRowsToKeep == dimOfRemainingDesignVariables), "must be the same if full rank!");
// get the top left block
SM_ASSERT_GE(aslam::Exception, R.rows(), numTopRowsInCov, "Cannot extract " << numTopRowsInCov << " rows of R because it only has " << R.rows() << " rows.");
SM_ASSERT_GE(aslam::Exception, R.cols(), numTopRowsInCov, "Cannot extract " << numTopRowsInCov << " cols of R because it only has " << R.cols() << " cols.");
//outRtop = R.block(0, 0, numTopRowsInRtop, numTopRowsInRtop);
// cut off the zero rows at the bottom
R_reduced = R.block(dimOfDesignVariablesToRemove, dimOfDesignVariablesToRemove, dimOfRemainingDesignVariables, dimOfRemainingDesignVariables);
//R_reduced = R.block(dimOfDesignVariablesToRemove, dimOfDesignVariablesToRemove, numRowsToKeep, dimOfRemainingDesignVariables);
d_reduced = d.segment(dimOfDesignVariablesToRemove, dimOfRemainingDesignVariables);
//d_reduced = d.segment(dimOfDesignVariablesToRemove, numRowsToKeep);
//dimOfPriorErrorTerm = dimOfRemainingDesignVariables;
}
// now create the new error term
boost::shared_ptr<aslam::backend::MarginalizationPriorErrorTerm> err(new aslam::backend::MarginalizationPriorErrorTerm(remainingDesignVariables, d_reduced, R_reduced));
outPriorErrorTermPtr.swap(err);
// restore initial block indices to prevent side effects
for (size_t i = 0; i < inDesignVariables.size(); ++i) {
inDesignVariables[i]->setBlockIndex(originalBlockIndices[i]);
inDesignVariables[i]->setColumnBase(originalColumnBase[i]);
}
int index = 0;
for(std::vector<aslam::backend::ErrorTerm*>::iterator it = inErrorTerms.begin(); it != inErrorTerms.end(); ++it)
{
(*it)->setRowBase(originalRowBase[index++]);
}
}
int main(int argc, char *argv[])
{
WSADATA wsa;
struct sockaddr_in server, address;
int max_clients = 30, activity, addrlen, i, valread;
//size of our receive buffer, this is string length.
int MAXRECV = 1024;
//set of socket descriptors
fd_set readfds;
//1 extra for null character, string termination
char *buffer;
buffer = (char*)malloc((MAXRECV + 1) * sizeof(char));
for (i = 0; i < TOTAL_POSSIBLE_NEIGHBOURS; i++)
{
client_socket[i] = 0;
}
printf("\nInitialising Winsock...");
if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0)
{
printf("Failed. Error Code : %d", WSAGetLastError());
exit(EXIT_FAILURE);
}
printf("Initialised.\n");
//Create a socket
if ((master = socket(AF_INET, SOCK_STREAM, 0)) == INVALID_SOCKET)
{
printf("Could not create socket : %d", WSAGetLastError());
exit(EXIT_FAILURE);
}
printf("Socket created.\n");
//Prepare the sockaddr_in structure
server.sin_family = AF_INET;
server.sin_addr.s_addr = INADDR_ANY;
server.sin_port = htons(MY_PORT);
//Bind
if (bind(master, (struct sockaddr *)&server, sizeof(server)) == SOCKET_ERROR)
{
printf("Bind failed with error code : %d", WSAGetLastError());
exit(EXIT_FAILURE);
}
puts("Bind done");
//Listen to incoming connections
listen(master, 3);
//Accept and incoming connection
puts("Waiting for incoming connections...\n\n");
addrlen = sizeof(struct sockaddr_in);
extend_network(bootstrap_ip, port, activeNeighbours);
myTimer(hTimerQueue);
while (TRUE)
{
//clear the socket fd set
FD_ZERO(&readfds);
//add master socket to fd set
FD_SET(master, &readfds);
//add child sockets to fd set
for (i = 0; i < 5; i++)
{
s = client_socket[i];
if (s > 0)
{
FD_SET(s, &readfds);
}
}
//wait for an activity on any of the sockets, timeout is NULL , so wait indefinitely
activity = select(0, &readfds, NULL, NULL, NULL);
if (activity == SOCKET_ERROR)
{
printf("[activity] select call failed with error code : %d", WSAGetLastError());
getchar();
exit(EXIT_FAILURE);
}
//If something happened on the master socket , then its an incoming connection
if (FD_ISSET(master, &readfds))
{
if ((new_socket = accept(master, (struct sockaddr *)&address, (int *)&addrlen)) < 0)
{
perror("accept");
getchar();
exit(EXIT_FAILURE);
}
//inform user of socket number - used in send and receive commands
printf("New connection , socket fd is %d , ip is : %s , port : %d \n", new_socket, inet_ntoa(address.sin_addr), ntohs(address.sin_port));
//add new socket to array of sockets
for (i = 0; i < max_clients; i++)
{
if (client_socket[i] == 0)
{
client_socket[i] = new_socket;
printf("Adding to list of sockets at index %d \n", i);
break;
}
}
}
//else its some IO operation on some other socket :)
for (i = 0; i < max_clients; i++)
{
s = client_socket[i];
//if client presend in read sockets
if (FD_ISSET(s, &readfds))
{
//get details of the client
getpeername(s, (struct sockaddr*)&address, (int*)&addrlen);
//Check if it was for closing , and also read the incoming message
//recv does not place a null terminator at the end of the string (whilst printf %s assumes there is one).
valread = recv(s, buffer, MAXRECV, 0);
if (valread == SOCKET_ERROR)
{
int error_code = WSAGetLastError();
if (error_code == WSAECONNRESET)
{
//Somebody disconnected , get his details and print
printf("Host disconnected unexpectedly , ip %s , port %d \n", inet_ntoa(address.sin_addr), ntohs(address.sin_port));
//removeFromNeighboursList(i, inet_ntoa(address.sin_addr));
//Close the socket and mark as 0 in list for reuse
closesocket(s);
client_socket[i] = 0;
}
else
{
printf("recv failed with error code : %d", error_code);
}
}
if (valread == 0)
{
//Somebody disconnected , get his details and print
printf("Host disconnected , ip %s , port %d \n", inet_ntoa(address.sin_addr), ntohs(address.sin_port));
//removeFromNeighboursList(i, inet_ntoa(address.sin_addr));
//Close the socket and mark as 0 in list for reuse
closesocket(s);
client_socket[i] = 0;
}
//Echo back the message that came in
else
{
//add null character, if you want to use with printf/puts or other string handling functions
process_receive(buffer, i);
}
}
}
}
if (WaitForSingleObject(gDoneEvent, INFINITE) != WAIT_OBJECT_0)
printf("WaitForSingleObject failed (%d)\n", GetLastError());
closesocket(s);
WSACleanup();
getchar();
return 0;
}
