int32_t test40::Dec(int32_t Value)
{
int32_t I, X, _idx__0001;
int32_t result = 0;
// Print a decimal number
// Check for max negative
X = -(Value == (int32_t)0x80000000U);
if (Value < 0) {
// If negative, make positive; adjust for max negative
Value = abs((Value + X));
// and output sign
Tx('-');
}
// Initialize divisor
I = 1000000000;
for(_idx__0001 = 0; _idx__0001 < 10; _idx__0001++) {
// Loop for 10 digits
if (Value >= I) {
// If non-zero digit, output digit; adjust for max negative
Tx((((Value / I) + '0') + (X * -(I == 1))));
// and digit from value
Value = Value % I;
// flag non-zero found
result = -1;
} else {
if ((result) || (I == 1)) {
// If zero digit (or only digit) output it
Tx('0');
}
}
// Update divisor
I = I / 10;
}
return result;
}
Tx BlockDataViewer::getPrevTx(TxIn & txin) const
{
if (txin.isCoinbase())
return Tx();
OutPoint op = txin.getOutPoint();
return getTxByHash(op.getTxHash());
}
cv::Point3d PinholeCameraModel::projectPixelTo3dRay(const cv::Point2d& uv_rect) const
{
assert( initialized() );
cv::Point3d ray;
ray.x = (uv_rect.x - cx() - Tx()) / fx();
ray.y = (uv_rect.y - cy() - Ty()) / fy();
ray.z = 1.0;
return ray;
}
//\fn void ExtrinsicParam::changePanTilt(double pan, double tilt);
///\brief This function computes the new rotation matrix and the new translation vector of the extrinsic parameters when the camera has changed its position.
///\param pan Value of the new camera panoramique.
///\param tilt Value of the new camera tilt.
void ExtrinsicParam::changePanTilt(double pan, double tilt)
{
// Compute the rotation matrices with the new values of pan and tilt
Eigen::Matrix3d Rx, Ry;
Rx.setZero();
Ry.setZero();
Rx(0,0) = 1;
Rx(1,1) = cos((-(tilt-this->tilt))*PI/180.);
Rx(1,2) = -sin((-(tilt-this->tilt))*PI/180.);
Rx(2,1) = sin((-(tilt-this->tilt))*PI/180.);
Rx(2,2) = cos((-(tilt-this->tilt))*PI/180.);
Ry(0,0) = cos((-(pan-this->pan))*PI/180.);
Ry(0,2) = sin((-(pan-this->pan))*PI/180.);
Ry(1,1) = 1;
Ry(2,0) = -sin((-(pan-this->pan))*PI/180.);
Ry(2,2) = cos((-(pan-this->pan))*PI/180.);
Eigen::Vector3d Tx, Ty;
Tx.setZero();
Ty.setZero();
Tx(0) = 2*3.3*sin(0.5*(this->pan-pan)*PI/180.)*cos(0.5*(this->pan-pan)*PI/180.);
Tx(2) = -2*3.3*sin(0.5*(this->pan-pan)*PI/180.)*cos((90.-0.5*(this->pan-pan))*PI/180.);
Ty(1) = 2*3.3*sin(0.5*(this->tilt-tilt)*PI/180.)*cos(0.5*(this->tilt-tilt)*PI/180.);
Ty(2) = -2*3.3*sin(0.5*(this->tilt-tilt)*PI/180.)*cos((90.-0.5*(this->tilt-tilt))*PI/180.);
// Compute the new values of the extrinsic parameters
Eigen::Matrix4d Rx1, Ry1, Rt;
Rt << initial_rotation, initial_translation,
0,0,0,1;
Rx1 << Rx, Tx,
0,0,0,1;
Ry1 << Ry, Ty,
0,0,0,1;
Rt.noalias() = Rx1*Ry1*Rt;
rotation(0,0) = Rt(0,0);rotation(0,1) = Rt(0,1);rotation(0,2) = Rt(0,2);
rotation(1,0) = Rt(1,0);rotation(1,1) = Rt(1,1);rotation(1,2) = Rt(1,2);
rotation(2,0) = Rt(2,0);rotation(2,1) = Rt(2,1);rotation(2,2) = Rt(2,2);
translation(0) = Rt(0,3);
translation(1) = Rt(1,3);
translation(2) = Rt(2,3);
}
void
WLEyes::SetPointOfView(int mousePosX, int mousePosY)
{
TPoint mouse;
mouse.x = Tx(mousePosX, mousePosY, &m_trans);
mouse.y = Ty(mousePosX, mousePosY, &m_trans);
for (int i = 0; i < 2; ++i){
TPoint newPupil = ComputePupil(i, mouse, NULL);
m_eyes[i]->CreatePupil(newPupil.x, newPupil.y, BALL_DIAM, m_trans);
}
}
Tx BlockDataViewer::getSpenderTxForTxOut(uint32_t height, uint32_t txindex,
uint16_t txoutid) const
{
StoredTxOut stxo;
db_->getStoredTxOut(stxo, height, txindex, txoutid);
if (!stxo.isSpent())
return Tx();
TxRef txref(stxo.spentByTxInKey_.getSliceCopy(0, 6));
return txref.attached(db_).getTxCopy();
}
/* ARGSUSED */
static void draw_it (
XtPointer client_data,
XtIntervalId *id) /* unused */
{
EyesWidget w = (EyesWidget)client_data;
Window rep_root, rep_child;
int rep_rootx, rep_rooty;
unsigned int rep_mask;
int dx, dy;
TPoint mouse;
Display *dpy = XtDisplay (w);
Window win = XtWindow (w);
TPoint newpupil[2];
XPoint xnewpupil, xpupil;
if (XtIsRealized((Widget)w)) {
XQueryPointer (dpy, win, &rep_root, &rep_child,
&rep_rootx, &rep_rooty, &dx, &dy, &rep_mask);
mouse.x = Tx(dx, dy, &w->eyes.t);
mouse.y = Ty(dx, dy, &w->eyes.t);
if (!TPointEqual (mouse, w->eyes.mouse)) {
computePupils (mouse, newpupil);
xpupil.x = Xx(w->eyes.pupil[0].x, w->eyes.pupil[0].y, &w->eyes.t);
xpupil.y = Xy(w->eyes.pupil[0].x, w->eyes.pupil[0].y, &w->eyes.t);
xnewpupil.x = Xx(newpupil[0].x, newpupil[0].y, &w->eyes.t);
xnewpupil.y = Xy(newpupil[0].x, newpupil[0].y, &w->eyes.t);
if (!XPointEqual (xpupil, xnewpupil)) {
if (w->eyes.pupil[0].x != -1000 || w->eyes.pupil[0].y != -1000)
eyeBall (w, w->eyes.centerGC, 0);
w->eyes.pupil[0] = newpupil[0];
eyeBall (w, w->eyes.pupGC, 0);
}
xpupil.x = Xx(w->eyes.pupil[1].x, w->eyes.pupil[1].y, &w->eyes.t);
xpupil.y = Xy(w->eyes.pupil[1].x, w->eyes.pupil[1].y, &w->eyes.t);
xnewpupil.x = Xx(newpupil[1].x, newpupil[1].y, &w->eyes.t);
xnewpupil.y = Xy(newpupil[1].x, newpupil[1].y, &w->eyes.t);
if (!XPointEqual (xpupil, xnewpupil)) {
if (w->eyes.pupil[1].x != -1 || w->eyes.pupil[1].y != -1)
eyeBall (w, w->eyes.centerGC, 1);
w->eyes.pupil[1] = newpupil[1];
eyeBall (w, w->eyes.pupGC, 1);
}
w->eyes.mouse = mouse;
w->eyes.update = 0;
} else {
if (delays[w->eyes.update + 1] != 0)
++w->eyes.update;
}
}
w->eyes.interval_id =
XtAppAddTimeOut(XtWidgetToApplicationContext((Widget) w),
delays[w->eyes.update], draw_it, (XtPointer)w);
} /* draw_it */
BOOL CWspStream::OnWrite( int nErr )
{
// Lock the transmit buffer
CTlLocalLock ll( *Tx() );
if ( !ll.IsLocked() )
return FALSE;
// Not blocking now
m_bTxBusy = FALSE;
// Send more data
return OnTx();
}
cv::Point2d PinholeCameraModel::project3dToPixel(const cv::Point3d& xyz) const
{
assert( initialized() );
assert(P_(2, 3) == 0.0); // Calibrated stereo cameras should be in the same plane
// [U V W]^T = P * [X Y Z 1]^T
// u = U/W
// v = V/W
cv::Point2d uv_rect;
uv_rect.x = (fx()*xyz.x + Tx()) / xyz.z + cx();
uv_rect.y = (fy()*xyz.y + Ty()) / xyz.z + cy();
return uv_rect;
}
int32_t test29::Str(int32_t Stringptr)
{
while (lockset(Strlock)) {
Yield__();
}
{
int32_t _idx__0000;
int32_t _limit__0001 = strlen((char *) Stringptr);
for(_idx__0000 = 1; _idx__0000 <= _limit__0001; (_idx__0000 = (_idx__0000 + 1))) {
Tx(((uint8_t *)(Stringptr++))[0]);
}
}
lockclr(Strlock);
return 0;
}
static void draw_it_core(EyesWidget w)
{
Window rep_root, rep_child;
int rep_rootx, rep_rooty;
unsigned int rep_mask;
int dx, dy;
TPoint mouse;
Display *dpy = XtDisplay (w);
Window win = XtWindow (w);
XQueryPointer (dpy, win, &rep_root, &rep_child,
&rep_rootx, &rep_rooty, &dx, &dy, &rep_mask);
mouse.x = Tx(dx, dy, &w->eyes.t);
mouse.y = Ty(dx, dy, &w->eyes.t);
drawEyes(w, mouse);
}
cv::Point2d PinholeCameraModel::unrectifyPoint(const cv::Point2d& uv_rect) const
{
assert( initialized() );
if (cache_->distortion_state == NONE)
return uv_rect;
if (cache_->distortion_state == UNKNOWN)
throw Exception("Cannot call unrectifyPoint when distortion is unknown.");
assert(cache_->distortion_state == CALIBRATED);
/// @todo Make this just call projectPixelTo3dRay followed by cv::projectPoints. But
/// cv::projectPoints requires 32-bit float, which is annoying.
// Formulae from docs for cv::initUndistortRectifyMap,
// http://opencv.willowgarage.com/documentation/cpp/camera_calibration_and_3d_reconstruction.html
// x <- (u - c'x) / f'x
// y <- (v - c'y) / f'y
// c'x, f'x, etc. (primed) come from "new camera matrix" P[0:3, 0:3].
double x = (uv_rect.x - cx() - Tx()) / fx();
double y = (uv_rect.y - cy() - Ty()) / fy();
// [X Y W]^T <- R^-1 * [x y 1]^T
double X = R_(0,0)*x + R_(1,0)*y + R_(2,0);
double Y = R_(0,1)*x + R_(1,1)*y + R_(2,1);
double W = R_(0,2)*x + R_(1,2)*y + R_(2,2);
// x' <- X/W, y' <- Y/W
double xp = X / W;
double yp = Y / W;
// x'' <- x'(1+k1*r^2+k2*r^4+k3*r^6) + 2p1*x'*y' + p2(r^2+2x'^2)
// y'' <- y'(1+k1*r^2+k2*r^4+k3*r^6) + p1(r^2+2y'^2) + 2p2*x'*y'
// where r^2 = x'^2 + y'^2
double r2 = xp*xp + yp*yp;
double r4 = r2*r2;
double r6 = r4*r2;
double a1 = 2*xp*yp;
double k1 = D_(0,0), k2 = D_(0,1), p1 = D_(0,2), p2 = D_(0,3), k3 = D_(0,4);
double barrel_correction = 1 + k1*r2 + k2*r4 + k3*r6;
if (D_.cols == 8)
barrel_correction /= (1.0 + D_(0,5)*r2 + D_(0,6)*r4 + D_(0,7)*r6);
double xpp = xp*barrel_correction + p1*a1 + p2*(r2+2*(xp*xp));
double ypp = yp*barrel_correction + p1*(r2+2*(yp*yp)) + p2*a1;
// map_x(u,v) <- x''fx + cx
// map_y(u,v) <- y''fy + cy
// cx, fx, etc. come from original camera matrix K.
return cv::Point2d(xpp*K_(0,0) + K_(0,2), ypp*K_(1,1) + K_(1,2));
}
static void computePupils (
EyesWidget w,
TPoint mouse,
TPoint pupils[2])
{
TRectangle screen, *sp = NULL;
if (w->eyes.distance) {
Window r, cw;
int x, y;
r = RootWindowOfScreen(w->core.screen);
XTranslateCoordinates(XtDisplay(w), XtWindow(w), r, 0, 0, &x, &y, &cw);
screen.x = Tx(-x, -y, &w->eyes.t);
screen.y = Ty(-x, -y, &w->eyes.t);
screen.width = Twidth (w->core.screen->width, w->core.screen->height,
&w->eyes.t);
screen.height = Theight(w->core.screen->width, w->core.screen->height,
&w->eyes.t);
sp = &screen;
}
pupils[0] = computePupil (0, mouse, sp);
pupils[1] = computePupil (1, mouse, sp);
}
EXPORT unsigned char libxbee::Con::operator<< (std::vector<char> data) {
return Tx(data);
}
EXPORT unsigned char libxbee::Con::operator<< (std::string data) {
return Tx(data);
}
int
process(const tendrils& in, const tendrils& out)
{
cv::Mat R, T, K, depth;
in.get<cv::Mat>("R").convertTo(R, CV_64F);
in.get<cv::Mat>("T").convertTo(T, CV_64F);
in.get<cv::Mat>("K").convertTo(K, CV_64F);
depth = in.get<cv::Mat>("depth");
cv::Mat mask;
if (!depth.empty())
{
mask = cv::Mat::zeros(depth.size(), CV_8UC1);
}
else
return ecto::OK;
if (R.empty() || T.empty() || K.empty())
return 0;
double fx, fy, cx, cy;
fx = K.at<double>(0, 0);
fy = K.at<double>(1, 1);
cx = K.at<double>(0, 2);
cy = K.at<double>(1, 2);
box_mask.create(depth.size());
box_mask.setTo(cv::Scalar(0));
std::vector<cv::Point3f> box(8), tbox(8);
box[0] = cv::Point3f(*x_crop, *y_crop, *z_min);
box[1] = cv::Point3f(-*x_crop, *y_crop, *z_min);
box[2] = cv::Point3f(-*x_crop, -*y_crop, *z_min);
box[3] = cv::Point3f(*x_crop, -*y_crop, *z_min);
box[4] = cv::Point3f(*x_crop, *y_crop, *z_crop);
box[5] = cv::Point3f(-*x_crop, *y_crop, *z_crop);
box[6] = cv::Point3f(-*x_crop, -*y_crop, *z_crop);
box[7] = cv::Point3f(*x_crop, -*y_crop, *z_crop);
cv::Mat RT(3, 4, CV_64F);
cv::Mat _T(RT.colRange(3, 4));
cv::Mat _R(RT.colRange(0, 3));
R.copyTo(_R);
T.copyTo(_T);
cv::transform(box, tbox, RT);
std::vector<cv::Point2f> projected, hull;
cv::projectPoints(box, R, T, K, cv::Mat(4, 1, CV_64FC1, cv::Scalar(0)), projected);
cv::convexHull(projected, hull, true);
std::vector<cv::Point> points(hull.size());
std::copy(hull.begin(), hull.end(), points.begin());
cv::fillConvexPoly(box_mask, points.data(), points.size(), cv::Scalar::all(255));
cv::Matx<double, 3, 3> A_x;
int width = mask.size().width;
int height = mask.size().height;
cv::Mat_<float_t>::iterator dit = depth.begin<float_t>();
cv::Mat_<uint8_t>::iterator it = mask.begin<uint8_t>();
cv::Mat_<uint8_t>::iterator mit = box_mask.begin();
cv::Matx<double, 3, 1> p, p_r, Tx(T); //Translation
cv::Matx<double, 3, 3> Rx; //inverse Rotation
Rx = cv::Mat(R.t());
// std::cout << cv::Mat(Rx) << "\n" << cv::Mat(Tx) << std::endl;
// std::cout << fx << " " << fy << " " << cx << " " << cy << " " << std::endl;
double z_min_ = *z_min, z_max_ = *z_crop, x_min_ = -*x_crop, x_max_ = *x_crop, y_min_ = -*y_crop,
y_max_ = *y_crop;
for (int v = 0; v < height; v++)
{
for (int u = 0; u < width; u++, ++it, ++mit, ++dit)
{
if (*mit == 0)
continue;
//calculate the point based on the depth
p(2) = *dit; //depth value in meters
p(0) = (u - cx) * p(2) / fx;
p(1) = (v - cy) * p(2) / fy;
p_r = Rx * (p - Tx);
// std::cout <<"p=" << cv::Mat(p) << ",p_r="<< cv::Mat(p_r) << std::endl;
if (p_r(2) > z_min_ && p_r(2) < z_max_ && p_r(0) > x_min_ && p_r(0) < x_max_ && p_r(1) > y_min_
&& p_r(1) < y_max_)
*it = 255;
}
}
out["mask"] << mask;
return ecto::OK;
}
void ZeroConfContainer::loadZeroConfMempool(
function<bool(const BinaryData&)> filter,
bool clearMempool)
{
//run this in its own scope so the iter and tx are closed in order to open
//RW tx afterwards
{
auto dbs = db_->getDbSelect(HISTORY);
LMDBEnv::Transaction tx;
db_->beginDBTransaction(&tx, dbs, LMDB::ReadOnly);
LDBIter dbIter(db_->getIterator(dbs));
if (!dbIter.seekToStartsWith(DB_PREFIX_ZCDATA))
{
enabled_ = true;
return;
}
do
{
BinaryDataRef zcKey = dbIter.getKeyRef();
if (zcKey.getSize() == 7)
{
//Tx, grab it from DB
StoredTx zcStx;
db_->getStoredZcTx(zcStx, zcKey);
//add to newZCMap_
Tx& zcTx = newZCMap_[zcKey.getSliceCopy(1, 6)];
zcTx = Tx(zcStx.getSerializedTx());
zcTx.setTxTime(zcStx.unixTime_);
}
else if (zcKey.getSize() == 9)
{
//TxOut, ignore it
continue;
}
else
{
//shouldn't hit this
LOGERR << "Unknown key found in ZC mempool";
break;
}
} while (dbIter.advanceAndRead(DB_PREFIX_ZCDATA));
}
if (clearMempool == true)
{
vector<BinaryData> keysToWrite, keysToDelete;
for (const auto& zcTx : newZCMap_)
keysToDelete.push_back(zcTx.first);
newZCMap_.clear();
updateZCinDB(keysToWrite, keysToDelete);
}
else if (newZCMap_.size())
{
//copy newZCmap_ to keep the pre parse ZC map
auto oldZCMap = newZCMap_;
//now parse the new ZC
parseNewZC(filter);
//set the zckey to the highest used index
if (txMap_.size() > 0)
{
BinaryData topZcKey = txMap_.rbegin()->first;
topId_.store(READ_UINT32_BE(topZcKey.getSliceCopy(2, 4)) +1);
}
//intersect oldZCMap and txMap_ to figure out the invalidated ZCs
vector<BinaryData> keysToWrite, keysToDelete;
for (const auto& zcTx : oldZCMap)
{
if (txMap_.find(zcTx.first) == txMap_.end())
keysToDelete.push_back(zcTx.first);
}
//no need to run this in a side thread, this code only runs when we have
//full control over the main thread
updateZCinDB(keysToWrite, keysToDelete);
}
enabled_ = true;
}
void main (void) {
volatile uint32_t i = 0; /* Dummy idle counter */
uint8_t success=0;
uint8_t byteReceived=0;
uint8_t opcode=0;
uint8_t payload=0;
char msg[32];
uint8_t msgLength=0;
clock = 0;
initModesAndClock(); /* MPC56xxP/B/S: Initialize mode entries, set sysclk = 64 MHz*/
initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */
disableWatchdog(); /* Disable watchdog */
EXCEP_InitExceptionHandlers(); /* Initialize exceptions: only need to load IVPR */
initADC();
initPIT(); /* Initialize PIT1 for 10Hz IRQ, priority 2 */
initPads(); /* Initialize software interrupt 4 */
initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */
initEMIOS_1(); /* Initialize eMIOS channels as counter, SAIC, OPWM */
initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/
initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/
initEMIOS_0ch8(); /* Initialize eMIOS 0 channel 8 as modulus counter*/
//just to make sure the wheels are facing straight
initDrive();
Drive();
SIU.PCR[64].R = 0x0100; /* Program the drive enable pin of S1 (PE0) as input*/
while((SIU.PGPDI[2].R & 0x80000000) == 0x80000000); /*Wait until S1 switch is pressed*/
for(i=0;i<100000;i++);
while((SIU.PGPDI[2].R & 0x80000000) != 0x80000000); /*Wait until S1 switch is released*/
INTC_InitVector();
INTC_InstallINTCInterruptHandler(&SwIrq4ISR,4,3);
INTC_InstallINTCInterruptHandler(&EOC_ISR,62,5);
INTC_InstallINTCInterruptHandler(&Pit1ISR,60,2);
INTC_InstallINTCInterruptHandler(&Pit2ISR,61,4);
INTC_InstallINTCInterruptHandler(&Pit3ISR,127,4);
initSerial();
flag_lineDone = -1;
initCamera();
initSteeringController();
INTC_InitINTCInterrupts();
INTC.CPR.B.PRI = 0; /* Single Core: Lower INTC's current priority */
initDrive();
// setPWMRw(48);
// setPWMLw(48);
setDirection(FORWARD);
in = getInBuffer();
out = getOutBuffer();
MESSAGE("I'm running\n\r");
fifo_write(&out->fifo,msg,msgLength);
while(isCameraReady()!=STATE_READY);
for(;;)
{
if(!(flag_lineDone==1))
{
Drive();
success=0;
success = fifo_read(&in->fifo,&byteReceived,1);
if(success==1)
{
if(opcode==0)
{
opcode = byteReceived;
MESSAGE("Command Received: ");
fifo_write(&out->fifo,msg,msgLength);
}
else
{
payload = byteReceived;
switch(opcode)
{
case DRIVE:
// TransmitData("Drive Command\n\r");
MESSAGE("Drive Command\n\r");
fifo_write(&out->fifo,msg,msgLength);
setDirection(payload);
break;
case SET_SPEED:
// TransmitData("Set Speed Command\n\r");
MESSAGE("Set Speed Command\n\r");
fifo_write(&out->fifo,msg,msgLength);
setPWMRw(payload);
setPWMLw(payload);
break;
case STEERING:
// TransmitData("Set Steering Command\n\r");
MESSAGE("Set Steering Command\n\r");
fifo_write(&out->fifo,msg,msgLength);
setAngle(payload);
break;
default:
// TransmitData("Bad command\n\r");
MESSAGE("Bad command\n\r");
fifo_write(&out->fifo,msg,msgLength);
break;
}
opcode = 0;
}
}
}
if(TRANSMISSION_DONE()&&(out->fifo.length>0))
Tx();
if(RECEPTION_DONE()&&(in->fifo.length<IN_BUFFER_SIZE))
Rx();
}
}
vector<double> Plink::calcMantelHaenszel_ORD(vector<int> & X,
vector<int> & Y,
vector<int> & Z)
{
// X is SNP coding
// Y is phenotype (assumed to be ordinal, integers)
// Z is cluster
if (X.size() != Y.size() || Y.size() != Z.size() || X.size() != Z.size() )
error("Internal problem:\n problem in calcMantelHaenszel_ORD()...uneven input columns");
// Determine unique elements
int nx=0, ny=0, nz=0;
map<int,int> mx;
map<int,int> my;
map<int,int> mz;
for (unsigned int i=0; i<X.size(); i++)
{
if (mx.find(X[i]) == mx.end())
mx.insert(make_pair(X[i],nx++));
if (my.find(Y[i]) == my.end())
my.insert(make_pair(Y[i],ny++));
if (mz.find(Z[i]) == mz.end())
mz.insert(make_pair(Z[i],nz++));
}
// Generic function to calculate generalized ordinal IxJxK CMH
// Assumes no missing data
vector< vector<double> > N(nz); // observed counts
vector< vector<double> > U(nz); // expected
vector< vector< vector<double> > > V(nz); // variance matrix
vector<vector<double> > Tx(nz); // marginal totals
vector<vector<double> > Ty(nz); // ..
vector<double> T(nz); // totals (per K)
for (int k=0; k<nz; k++)
{
Tx[k].resize(nx);
Ty[k].resize(ny);
N[k].resize((nx-1)*(ny-1));
U[k].resize((nx-1)*(ny-1));
V[k].resize((nx-1)*(ny-1));
for (int k2=0; k2<(nx-1)*(ny-1); k2++)
{
N[k][k2] = U[k][k2] = 0;
V[k][k2].resize((nx-1)*(ny-1));
for (int k3=0; k3<(nx-1)*(ny-1); k3++)
V[k][k2][k3] = 0;
}
}
// Create counts
// Consider each observation
for (int i=0; i<X.size(); i++)
{
int vx = mx.find(X[i])->second;
int vy = my.find(Y[i])->second;
int vz = mz.find(Z[i])->second;
// exclude nx + ny (upper limits)
if (vx<nx-1 && vy<ny-1)
N[vz][ vx + vy*(nx-1) ]++;
Tx[vz][vx]++;
Ty[vz][vy]++;
T[vz]++;
}
// Determine valid clusters (at least 2 people)
vector<bool> validK(nk,false);
for (int k=0; k<nk; k++)
if (T[k]>=2) validK[k]=true;
// Calculate expecteds
for (int k=0; k<nz; k++)
{
if (validK[k])
{
for (int ix=0; ix<nx-1; ix++)
for (int iy=0; iy<ny-1; iy++)
{
U[k][ix+iy*(nx-1)] = ( Tx[k][ix] * Ty[k][iy] ) / T[k];
}
}
}
////////////////////
// Ordinal CMH test
// ordered scores u_i and v_i
// T_k = sum_i sum_j u_i v_j n_ijk
// E(T_k) = [ ( \sum_i u_i n_{i+k} ) ( \sum_j v_j n_{+jk} ) ] / n_{++k}
// var(T_k) = ( 1 / ( n_{++k} - 1 )
// * [ \sum_i u_i^2 n_{i+k} - ( ( \sum_i u_i n_{i+k} )^2 / n_{++k} ) ]
// * [ \sum_j v_j^2 n_{+jk} - ( ( \sum_j v_j n_{+jk} )^2 / n_{++k} ) ]
// statistic [ T_k - E(T_k)]/[var(T_k)]^{1/2} - correl between X and Y
// in stratum k, multiplied by sqrt( n_{++k} - 1 )
// To summarize across K strata:
// M^2 = ( \sum_k [ T_k - E(T_k) ] )^2 / sum_k var(T_k)
// which has chi-sq 1 df null distrib
}
void StereoReconstructor::computeRTRandom(std::string cam1Folder, std::string cam2Folder) {
bool load1 = loadMatrixAndCoe(cam1Folder, camMatrix1, distCoeffs1);
bool load2 = loadMatrixAndCoe(cam2Folder, camMatrix2, distCoeffs2);
if (load1 == false || load2 == false) {
std::cout << "Load matrix and distortion failed!" << std::endl;
return;
}
std::vector<std::string> imgFiles1 = Utilities::folderImagesScan(cam1Folder.c_str());
std::vector<std::string> imgFiles2 = Utilities::folderImagesScan(cam2Folder.c_str());
cv::Mat img = cv::imread(cam1Folder + imgFiles1[0].c_str(), 1);
camImageSize = img.size();
int totalN = imgFiles1.size();
std::ofstream Tx("辅助_T_x.txt"); //x方向位移
std::ofstream Ty("辅助_T_y.txt"); //y方向位移
std::ofstream Tz("辅助_T_z.txt"); //z方向位移
std::ofstream Rx("辅助_R_x.txt"); //旋转轴x方向
std::ofstream Ry("辅助_R_y.txt"); //旋转轴y方向
std::ofstream Rz("辅助_R_z.txt"); //旋转轴z方向
std::ofstream Ra("辅助_R_a.txt"); //旋转角度
std::ofstream All("辅助_All.txt"); //所有数据
All << "Tx " << "Ty " << "Tz " << "T " << "Rx " << "Ry " << "Rz " << "Ra " << std::endl;
for (int k = 1; k <= totalN; k++) {
std::cout << k;
double tbegin = cv::getTickCount();
std::vector<int> nums;
while (nums.size() < k) {
srand(time(NULL));
int random = rand() % totalN;
if (nums.empty() || std::find(nums.begin(), nums.end(), random) == nums.end()) {
nums.push_back(random);
}
}
for (size_t i = 0; i < nums.size(); ++i) {
imgPoints1.push_back(load2DPoints(cam1Folder + imgFiles1[nums[i]].substr(0, imgFiles1[nums[i]].size() - 4) + "_imgCorners.txt"));
imgPoints2.push_back(load2DPoints(cam2Folder + imgFiles2[nums[i]].substr(0, imgFiles2[nums[i]].size() - 4) + "_imgCorners.txt"));
objPoints.push_back(load3DPoints(cam1Folder + "ObjCorners.txt"));
}
cv::Mat E, F;
cv::stereoCalibrate(objPoints, imgPoints1, imgPoints2, camMatrix1, distCoeffs1, camMatrix2, distCoeffs2, camImageSize, R, T, E, F,
cv::TermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 100, 1e-5), CV_CALIB_FIX_INTRINSIC);
//罗德里格斯（Rodrigues）变换
cv::Mat R2T(3, 1, CV_64F);
cv::Rodrigues(R, R2T);
float tx = Utilities::matGet2D(T, 0, 0);
float ty = Utilities::matGet2D(T, 0, 1);
float tz = Utilities::matGet2D(T, 0, 2);
float T = std::sqrt(tx*tx + ty*ty + tz*tz);
Tx << tx << std::endl;
Ty << ty << std::endl;
Tz << tz << std::endl;
double dx = Utilities::matGet2D(R2T, 0, 0);
double dy = Utilities::matGet2D(R2T, 0, 1);
double dz = Utilities::matGet2D(R2T, 0, 2);
double dl = std::sqrt(dx*dx + dy*dy + dz*dz);
float angle = dl * 180 / 3.1415926535897932;
float fdx = dx / dl;
float fdy = dy / dl;
float fdz = dz / dl;
Rx << fdx << std::endl;
Ry << fdy << std::endl;
Rz << fdz << std::endl;
Ra << angle << std::endl;
All << tx << " " << ty << " " << tz << " " << T << " " << fdx << " " << fdy << " " << fdz << " " << angle << std::endl;
nums.clear();
imgPoints1.clear();
imgPoints2.clear();
objPoints.clear();
double tend = cv::getTickCount();
double frequency = cv::getTickFrequency();
int span = (tend - tbegin) / frequency;
std::cout << "副图像,标定时间 " << span << ",基线长度" << T << std::endl;
}
Tx.close();
Ty.close();
Tz.close();
Rx.close();
Ry.close();
Rz.close();
Ra.close();
All.close();
}
int
process(const tendrils& in, const tendrils& out)
{
cv::Mat depth = in.get<cv::Mat>("depth");
if (depth.empty())
return ecto::OK;
cv::Mat R, T, K;
in.get<cv::Mat>("R").convertTo(R, CV_64F);
in.get<cv::Mat>("T").convertTo(T, CV_64F);
in.get<cv::Mat>("K").convertTo(K, CV_64F);
if (R.empty() || T.empty() || K.empty())
return ecto::OK;
cv::Mat mask = cv::Mat::zeros(depth.size(), CV_8UC1);
box_mask.create(depth.size());
box_mask.setTo(cv::Scalar(0));
std::vector<cv::Point3f> box(8);
box[0] = cv::Point3f(*x_crop, *y_crop, *z_min);
box[1] = cv::Point3f(-*x_crop, *y_crop, *z_min);
box[2] = cv::Point3f(-*x_crop, -*y_crop, *z_min);
box[3] = cv::Point3f(*x_crop, -*y_crop, *z_min);
box[4] = cv::Point3f(*x_crop, *y_crop, *z_crop);
box[5] = cv::Point3f(-*x_crop, *y_crop, *z_crop);
box[6] = cv::Point3f(-*x_crop, -*y_crop, *z_crop);
box[7] = cv::Point3f(*x_crop, -*y_crop, *z_crop);
std::vector<cv::Point2f> projected, hull;
cv::projectPoints(box, R, T, K, cv::Mat(4, 1, CV_64FC1, cv::Scalar(0)), projected);
cv::convexHull(projected, hull, true);
std::vector<cv::Point> points(hull.size());
std::copy(hull.begin(), hull.end(), points.begin());
cv::fillConvexPoly(box_mask, points.data(), points.size(), cv::Scalar::all(255));
int width = mask.size().width;
int height = mask.size().height;
cv::Mat_<uint8_t>::iterator it = mask.begin<uint8_t>();
cv::Mat_<uint8_t>::iterator mit = box_mask.begin();
cv::Mat_<cv::Vec3f> points3d;
depthTo3dMask(K, depth, box_mask, points3d);
if (points3d.empty())
return ecto::OK;
cv::Matx<double, 3, 1> p, p_r, Tx(T); //Translation
cv::Matx<double, 3, 3> Rx; //inverse Rotation
Rx = cv::Mat(R.t());
cv::Mat_<cv::Vec3f>::iterator point = points3d.begin();
// std::cout << cv::Mat(Rx) << "\n" << cv::Mat(Tx) << std::endl;
// std::cout << fx << " " << fy << " " << cx << " " << cy << " " << std::endl;
double z_min_ = *z_min, z_max_ = *z_crop, x_min_ = -*x_crop, x_max_ = *x_crop, y_min_ = -*y_crop,
y_max_ = *y_crop;
for (int v = 0; v < height; v++)
{
for (int u = 0; u < width; u++, ++it, ++mit)
{
if (*mit == 0)
continue;
//calculate the point based on the depth
p(0) = (*point).val[0];
p(1) = (*point).val[1];
p(2) = (*point).val[2];
++point;
p_r = Rx * (p - Tx);
// std::cout <<"p=" << cv::Mat(p) << ",p_r="<< cv::Mat(p_r) << std::endl;
if (p_r(2) > z_min_ && p_r(2) < z_max_ && p_r(0) > x_min_ && p_r(0) < x_max_ && p_r(1) > y_min_
&& p_r(1) < y_max_)
*it = 255;
}
}
out["mask"] << mask;
return ecto::OK;
}
EXPORT unsigned char libxbee::Con::operator<< (const std::vector<char> &data) const {
return Tx(data);
}
EXPORT unsigned char libxbee::Con::operator<< (const std::string &data) const {
return Tx(data);
}
static void
paint_eyes(ModeInfo * mi, Eyes * e, Fly * f, Eyes * eyes, int num_eyes)
{
EyeScrInfo *ep = &eye_info[MI_SCREEN(mi)];
Display *display = MI_DISPLAY(mi);
Window window = MI_WINDOW(mi);
GC gc = ep->eyeGC;
Bool iconic = MI_IS_ICONIC(mi);
int focusx = (f->x + (f->width / 2)) - e->x;
int focusy = (f->y + (f->height / 2)) - e->y;
Pixmap pix = e->pixmap;
TPoint point;
int i;
if (pix == None) {
e->time_to_die = 0; /* "should not happen" */
}
if (ep->time >= e->time_to_die) {
/* Sorry Bud, your time is up */
if (e->painted) {
/* only unpaint it if previously painted */
XSetForeground(display, gc, MI_BLACK_PIXEL(mi));
XFillRectangle(display, window, gc, e->x, e->y, e->width, e->height);
}
/* randomly place the eyes elsewhere */
create_eyes(mi, e, eyes, num_eyes);
pix = e->pixmap; /* pixmap may have changed */
}
/* If the bouncer would intersect this pair of eyes, force the
* eyes to move. This simplifies the code, because we do not
* have to deal with drawing the bouncer on top of the eyes.
* When trying to do so, there was too much annoying flashing
* and ghost images from the undraw. I decided to observe the
* KISS principle and keep it simple. I think the effect is
* better also.
* We must draw the flyer on the eyes when iconic, but that is
* easy because the eyes repaint the whole box each time.
*/
if ((!iconic) && (fly_touches_eye(f, e))) {
e->time_to_die = 0;
}
if (e->time_to_die == 0) {
return; /* collides with something */
}
/* set the point to look at and compute the pupil position */
point.x = Tx(focusx, focusy, &e->transform);
point.y = Ty(focusx, focusy, &e->transform);
computePupil(0, point, &(e->pupil[0]));
computePupil(1, point, &(e->pupil[1]));
if (e->painted) {
/* if still looking at the same point, do nothing further */
if (TPointEqual(e->pupil[0], e->last_pupil[0]) &&
TPointEqual(e->pupil[1], e->last_pupil[1])) {
return;
}
}
for (i = 0; i < 2; i++) {
/* update the eye, calculates the changed rectangle */
make_eye(mi, pix, e, i, False);
/* Only blit the change if the full image has been painted */
if (e->painted) {
/* copy the changed rectangle out to the screen */
XCopyArea(display, pix, window, gc,
e->bbox.x, e->bbox.y,
(int) e->bbox.width, (int) e->bbox.height,
e->x + e->bbox.x, e->y + e->bbox.y);
}
/* remember where we're looking, for the next time around */
e->last_pupil[i] = e->pupil[i];
}
/* always do full paint when iconic, eliminates need to track fly */
if (iconic || (!e->painted)) {
XCopyArea(display, pix, window, gc, 0, 0,
e->width, e->height, e->x, e->y);
}
/* when iconic, pretend to never paint, causes full paint each time */
if (!iconic) {
e->painted++; /* note that a paint has been done */
}
}
vector<double> Plink::calcMantelHaenszel_IxJxK(vector<int> & X,
vector<int> & Y,
vector<int> & Z)
{
if (X.size() != Y.size() || Y.size() != Z.size() || X.size() != Z.size() )
error("Internal problem:\n problem in calcMantelHaenszel_IxJxK()...uneven input columns");
// Determine unique elements
int nx=0, ny=0, nz=0;
map<int,int> mx;
map<int,int> my;
map<int,int> mz;
for (unsigned int i=0; i<X.size(); i++)
{
if (mx.find(X[i]) == mx.end())
mx.insert(make_pair(X[i],nx++));
if (my.find(Y[i]) == my.end())
my.insert(make_pair(Y[i],ny++));
if (mz.find(Z[i]) == mz.end())
mz.insert(make_pair(Z[i],nz++));
}
// Generic function to calculate generalized IxJxK CMH
// Assumes no missing data
vector< vector<double> > N(nz); // observed counts
vector< vector<double> > U(nz); // expected
vector< vector< vector<double> > > V(nz); // variance matrix
vector<vector<double> > Tx(nz); // marginal totals
vector<vector<double> > Ty(nz); // ..
vector<double> T(nz); // totals (per K)
for (int k=0; k<nz; k++)
{
Tx[k].resize(nx);
Ty[k].resize(ny);
N[k].resize((nx-1)*(ny-1));
U[k].resize((nx-1)*(ny-1));
V[k].resize((nx-1)*(ny-1));
for (int k2=0; k2<(nx-1)*(ny-1); k2++)
{
N[k][k2] = U[k][k2] = 0;
V[k][k2].resize((nx-1)*(ny-1));
for (int k3=0; k3<(nx-1)*(ny-1); k3++)
V[k][k2][k3] = 0;
}
}
// Consider each observation
for (int i=0; i<X.size(); i++)
{
int vx = mx.find(X[i])->second;
int vy = my.find(Y[i])->second;
int vz = mz.find(Z[i])->second;
// exclude nx + ny (upper limits)
if (vx<nx-1 && vy<ny-1)
N[vz][ vx + vy*(nx-1) ]++;
Tx[vz][vx]++;
Ty[vz][vy]++;
T[vz]++;
}
// Determine valid clusters (at least 2 people)
vector<bool> validK(nk,false);
for (int k=0; k<nk; k++)
if (T[k]>=2) validK[k]=true;
// Calculate expecteds
for (int k=0; k<nz; k++)
{
if (validK[k])
{
for (int ix=0; ix<nx-1; ix++)
for (int iy=0; iy<ny-1; iy++)
{
U[k][ix+iy*(nx-1)] = ( Tx[k][ix] * Ty[k][iy] ) / T[k];
for (int ix2=0; ix2<nx-1; ix2++)
for (int iy2=0; iy2<ny-1; iy2++)
{
int dx=0;
int dy=0;
if (ix==ix2) dx=1;
if (iy==iy2) dy=1;
V[k][ix + iy*(nx-1)][ix2 + iy2*(nx-1)] = ( ( Tx[k][ix] * ( dx * T[k] - Tx[k][ix2] )
* Ty[k][iy] * ( dy *T[k] - Ty[k][iy2] ) )
/ ( T[k]*T[k]*(T[k]-1) ) );
if (ix==ix2 && iy==iy2)
V[k][ix + iy*(nx-1)][ix2 + iy2*(nx-1)]
= abs(V[k][ix + iy*(nx-1)][ix2 + iy2*(nx-1)]);
}
}
}
}
vector<vector<double> > V0((nx-1)*(ny-1));
for (int k2=0; k2<(nx-1)*(ny-1); k2++)
V0[k2].resize((nx-1)*(ny-1));
vector<double> N0((nx-1)*(ny-1));
vector<double> U0((nx-1)*(ny-1));
// Sum N, U and V over K
for (int k=0; k<nz; k++)
{
if (validK[k])
{
for (int i=0; i<(nx-1)*(ny-1); i++)
{
N0[i] += N[k][i];
U0[i] += U[k][i];
for (int i2=0; i2<(nx-1)*(ny-1); i2++)
V0[i][i2] += V[k][i][i2];
}
}
}
bool flag = true;
vector<double> tmp1((nx-1)*(ny-1),0);
vector<double> tmp2((nx-1)*(ny-1),0);
V0 = svd_inverse(V0,flag);
for (int i=0; i<(nx-1)*(ny-1); i++)
tmp1[i] = N0[i] - U0[i];
// Matrix mult -- rows by columns
for (int i=0; i<(nx-1)*(ny-1); i++)
for (int j=0; j<(nx-1)*(ny-1); j++)
tmp2[j] += tmp1[i] * V0[i][j];
vector<double> result(2);
// CMH Chi-square
result[0]=0;
for (int i=0; i<(nx-1)*(ny-1); i++)
result[0] += tmp2[i] * tmp1[i];
// DF
result[1] = (nx-1)*(ny-1);
return result;
}
