int main() {
Nef_polyhedron N1(Nef_polyhedron::COMPLETE);
Sphere_circle c(1,1,1); // c : x + y + z = 0
Nef_polyhedron N2(c, Nef_polyhedron::INCLUDED);
Nef_polyhedron N3(N2.complement());
CGAL_assertion(N1 == N2.join(N3));
Sphere_point p1(1,0,0), p2(0,1,0), p3(0,0,1);
Sphere_segment s1(p1,p2), s2(p2,p3), s3(p3,p1);
Sphere_segment triangle[3] = { s1, s2, s3 };
Nef_polyhedron N4(triangle, triangle+3);
Nef_polyhedron N5;
N5 += N2;
N5 = N5.intersection(N4);
CGAL_assertion(N5 <= N2 && N5 != N4);
return 0;
}
bool Mda::write16ui(const QString& path) const
{
FILE* output_file = fopen(path.toLatin1().data(), "wb");
if (!output_file) {
printf("Warning: Unable to open mda file for writing: %s\n", path.toLatin1().data());
return false;
}
MDAIO_HEADER H;
H.data_type = MDAIO_TYPE_UINT16;
H.num_bytes_per_entry = 2;
for (int i = 0; i < MDAIO_MAX_DIMS; i++)
H.dims[i] = 1;
for (int i = 0; i < MDA_MAX_DIMS; i++)
H.dims[i] = d->dims(i);
H.num_dims = d->determine_num_dims(N1(), N2(), N3(), N4(), N5(), N6());
mda_write_header(&H, output_file);
mda_write_float64((double*)d->constData(), &H, d->totalSize(), output_file);
d->incrementBytesWrittenCounter(d->totalSize() * H.num_bytes_per_entry);
fclose(output_file);
return true;
}
int main() {
Nef_polyhedron N0;
Nef_polyhedron N1(Nef_polyhedron::EMPTY);
Nef_polyhedron N2(Nef_polyhedron::COMPLETE);
Nef_polyhedron N3(Plane_3( 1, 2, 5,-1));
Nef_polyhedron N4(Plane_3( 1, 2, 5,-1), Nef_polyhedron::INCLUDED);
Nef_polyhedron N5(Plane_3( 1, 2, 5,-1), Nef_polyhedron::EXCLUDED);
CGAL_assertion(N0 == N1);
CGAL_assertion(N3 == N4);
CGAL_assertion(N0 != N2);
CGAL_assertion(N3 != N5);
CGAL_assertion(N4 >= N5);
CGAL_assertion(N5 <= N4);
CGAL_assertion(N4 > N5);
CGAL_assertion(N5 < N4);
N5 = N5.closure();
CGAL_assertion(N4 >= N5);
CGAL_assertion(N4 <= N5);
return 0;
}
int Mda::ndims() const
{
return d->determine_num_dims(N1(), N2(), N3(), N4(), N5(), N6());
}
// mcmc_helper(im,im_prev,buf,int(xwho),int(xwho2));
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
// trw_bprop_helper2(model, psi_ij_rho, psi_i, rho, maxiter, n, m1, m2, b_i,...
// dm1, dm2, dn, dpsi_i, dpsi_ij);
int i=0;
int nnodes = mapdouble(mxGetField(prhs[i],0,"nnodes"))(0);
int ncliques = mapdouble(mxGetField(prhs[i],0,"ncliques"))(0);
int nvals = mapdouble(mxGetField(prhs[i],0,"nvals"))(0);
MatrixXi pairs = mapdouble(mxGetField(prhs[i],0,"pairs")).cast<int>();
pairs.array() -= 1;
MatrixXi N1 = mapdouble(mxGetField(prhs[i],0,"N1")).cast<int>();
N1.array() -= 1;
MatrixXi N2 = mapdouble(mxGetField(prhs[i],0,"N2")).cast<int>();
N2.array() -= 1;
i++;
MatrixMd psi_ij = mapdouble(prhs[i++]);
MatrixMd psi_i = mapdouble(prhs[i++]);
double rho = mapdouble(prhs[i++])(0);
int maxiter = mapdouble(prhs[i++])(0);
MatrixMd n = mapdouble(prhs[i++]);
MatrixMd m1 = mapdouble(prhs[i++]);
MatrixMd m2 = mapdouble(prhs[i++]);
MatrixMd b_i = mapdouble(prhs[i++]);
MatrixMd mstor = mapdouble(prhs[i++]);
MatrixMd dm1 = mapdouble(prhs[i++]);
MatrixMd dm2 = mapdouble(prhs[i++]);
MatrixMd dn = mapdouble(prhs[i++]);
MatrixMd dpsi_i = mapdouble(prhs[i++]);
MatrixMd dpsi_ij = mapdouble(prhs[i++]);
MatrixMd b_ij0 = mapdouble(prhs[i++]);
MatrixMd db_ij0 = mapdouble(prhs[i++]);
int dorec = mapdouble(prhs[i++]).cast<int>()(0);
int w = mstor.rows()-1;
MatrixXd S (nvals,nvals);
MatrixXd m0(nvals,1);
for(int c=0; c<ncliques; c++){
int i = pairs(c, 0);
int j = pairs(c, 1);
for(int yi=0; yi<nvals; yi++){
for(int yj=0; yj<nvals; yj++){
int index = yi + yj*nvals;
//b_ij0(index,c) = b_ij0(index,c)*psi_i(yi,i)*psi_i(yj,j)*n(yi,i)*n(yj,j)/m1(yi,c)/m2(yj,c);
dpsi_i(yi, i) = dpsi_i(yi, i) + db_ij0(index, c)*b_ij0(index, c)/psi_i(yi, i);
dpsi_i(yj, j) = dpsi_i(yj, j) + db_ij0(index, c)*b_ij0(index, c)/psi_i(yj, j);
dn(yi, i) = dn(yi, i) + db_ij0(index, c)*b_ij0(index, c)/n(yi, i);
dn(yj, j) = dn(yj, j) + db_ij0(index, c)*b_ij0(index, c)/n(yj, j);
dm1(yi, c) = dm1(yi, c) - db_ij0(index, c)*b_ij0(index, c)/m1(yi, c);
dm2(yj, c) = dm2(yj, c) - db_ij0(index, c)*b_ij0(index, c)/m2(yj, c);
}
}
}
for(int i=0; i<b_i.cols(); i++){
for(int yi=0; yi<nvals; yi++){
for(int k=0; k<N1.cols(); k++){
int d = N1(i, k);
if(d==-2) continue;
//n(yi, i) *= m1(yi, d);
dm1(yi,d) += rho * dn(yi,i)*n(yi,i)/m1(yi,d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(i, k);
if(d==-2) continue;
//n(yi, i) *= m2(yi, d);
dm2(yi,d) += rho * dn(yi,i)*n(yi,i)/m2(yi,d);
}
}
}
int reps;
double conv;
for(reps=0; reps<maxiter; reps++){
for(int c0=2*ncliques-1; c0>=0; c0--){
int c, mode;
if(c0<ncliques){
c = c0;
mode = 1;
} else{
c = ncliques - 1 - (c0-ncliques);
mode = 2;
}
int i = pairs(c,0);
int j = pairs(c,1);
if( mode==1 ){
for(int yi=0; yi<nvals; yi++)
n(yi, i) = 1;
for(int k=0; k<N1.cols(); k++){
int d = N1(i, k);
if(d==-2) continue;
for(int yi=0; yi<nvals; yi++)
n(yi, i) *= m1(yi, d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(i, k);
if(d==-2) continue;
for(int yi=0; yi<nvals; yi++)
n(yi, i) *= m2(yi, d);
}
// n.col(i) = n.col(i).array().pow(rho);
if(rho==1){
//nothing
} else if(rho==.5){
n.col(i) = n.col(i).array().sqrt();
} else
n.col(i) = n.col(i).array().pow(rho);
// compute m(y_j)
for(int yj=0; yj<nvals; yj++){
m0(yj) = 0;
for(int yi=0; yi<nvals; yi++){
int index = yi + yj*nvals;
S(yi,yj) = psi_ij(index, c)*psi_i(yi, i)*n(yi, i)/m1(yi, c);
m0(yj) += S(yi,yj);
}
}
double k = S.sum();
MatrixXd dm0 = dm2.col(c)/k;
dm0.array() -= (dm2.col(c).array()*m0.array()).sum()/(k*k);
MatrixXd dn = 0*n.col(i);
//#pragma omp parallel for num_threads(2)
for(int yj=0; yj<nvals; yj++){
for(int yi=0; yi<nvals; yi++){
int index = yi + yj*nvals;
dpsi_ij(index,c) += dm0(yj)*S(yi,yj)/psi_ij(index,c);
dpsi_i(yi,i) += dm0(yj)*S(yi,yj)/psi_i(yi,i);
dn(yi) += dm0(yj)*S(yi,yj)/n(yi,i);
dm1(yi,c) -= dm0(yj)*S(yi,yj)/m1(yi,c);
}
}
for(int yi=0; yi<nvals; yi++){
for(int k=0; k<N1.cols(); k++){
int d = N1(i, k);
if(d==-2) continue;
dm1(yi,d) += rho*dn(yi)*n(yi,i)/m1(yi,d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(i, k);
if(d==-2) continue;
dm2(yi,d) += rho*dn(yi)*n(yi,i)/m2(yi,d);
}
}
dm2.col(c) *= 0.0;
if( dorec )
for(int yj=nvals-1; yj>=0; yj--){ m2(yj,c)=mstor(w); w--;}
} else{
for( int yj=0; yj<nvals; yj++)
n(yj, j) = 1;
for(int k=0; k<N1.cols(); k++){
int d = N1(j, k);
if(d==-2) continue;
for( int yj=0; yj<nvals; yj++)
n(yj, j) *= m1(yj, d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(j, k);
if(d==-2) continue;
for( int yj=0; yj<nvals; yj++)
n(yj, j) *= m2(yj, d);
}
// n.col(j) = n.col(j).array().pow(rho);
if(rho==1){
//nothing
} else if(rho==.5){
n.col(j) = n.col(j).array().sqrt();
} else
n.col(j) = n.col(j).array().pow(rho);
for(int yi=0; yi<nvals; yi++){
m0(yi) = 0;
for(int yj=0; yj<nvals; yj++){
int index = yi + yj*nvals;
S(yi,yj) = psi_ij(index, c)*psi_i(yj, j)*n(yj, j)/m2(yj, c);
m0(yi) += S(yi,yj);
}
}
double k = S.sum();
MatrixXd dm0 = dm1.col(c)/k;
dm0.array() -= (dm1.col(c).array()*m0.array()).sum()/(k*k);
MatrixXd dn = 0*n.col(i);
for(int yj=0; yj<nvals; yj++){
for(int yi=0; yi<nvals; yi++){
int index = yi + yj*nvals;
dpsi_ij(index,c) += dm0(yi)*S(yi,yj)/psi_ij(index,c);
dpsi_i(yj,j) += dm0(yi)*S(yi,yj)/psi_i(yj,j);
dn(yj) += dm0(yi)*S(yi,yj)/n(yj,j);
dm2(yj,c) -= dm0(yi)*S(yi,yj)/m2(yj,c);
}
}
for(int yj=0; yj<nvals; yj++){
for(int k=0; k<N1.cols(); k++){
int d = N1(j, k);
if(d==-2) continue;
dm1(yj, d) += rho*dn(yj)*n(yj, j)/m1(yj, d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(j, k);
if(d==-2) continue;
dm2(yj, d) += rho*dn(yj)*n(yj, j)/m2(yj, d);
}
}
dm1.col(c) *= 0.0;
if( dorec )
for(int yi=nvals-1; yi>=0; yi--){ m1(yi,c)=mstor(w); w--;}
}
}
}
}
void CreateDoubleWayInteraction::mousePressEvent(QMouseEvent* anEvent)
{
if (anEvent->buttons() & Qt::LeftButton)
{
if (!HaveFirst)
{
HaveFirst = true;
view()->setInteracting(true);
FirstPoint = XY_TO_COORD(anEvent->pos());
FirstDistance = DockData.RoadDistance->text().toDouble();
}
else if (R1)
{
int i1 = R1->size()-1;
int i2 = 1;
LineF P1(
COORD_TO_XY(R1->getNode(i1-1)),
COORD_TO_XY(R1->getNode(i1)));
LineF P2(
COORD_TO_XY(R2->getNode(i2-1)),
COORD_TO_XY(R2->getNode(i2)));
Coord PreviousPoint = PreviousPoints[R1->size()-1];
if (distance(COORD_TO_XY(PreviousPoint), LastCursor) > 1)
{
qreal rB = view()->pixelPerM()*DockData.RoadDistance->text().toDouble()/2;
qreal rA = FirstDistance * view()->pixelPerM()/2;
LineF FA1(COORD_TO_XY(PreviousPoint),LastCursor);
LineF FA2(FA1);
LineF FB1(FA1);
LineF FB2(FA1);
qreal Modifier = DockData.DriveRight->isChecked()?1:-1;
FA1.slide(rA*Modifier);
FA2.slide(-rA*Modifier);
FB1.slide(rB*Modifier);
FB2.slide(-rB*Modifier);
LineF N1(FA1.project(COORD_TO_XY(PreviousPoint)), FB1.project(LastCursor));
LineF N2(FA2.project(COORD_TO_XY(PreviousPoint)), FB2.project(LastCursor));
Node* A1;
Node* A2;
CommandList* L = new CommandList(MainWindow::tr("Add nodes to double-way Road %1").arg(R1->id().numId), R1);
A1 = R1->getNode(i1);
A2 = R2->getNode(i2-1);
L->add(new MoveNodeCommand(A1,XY_TO_COORD(
P1.intersectionWith(N1).toPoint())));
L->add(new MoveNodeCommand(A2,XY_TO_COORD(
P2.intersectionWith(N2).toPoint())));
Node* B1 = g_backend.allocNode(theMain->document()->getDirtyOrOriginLayer(), XY_TO_COORD(
FB1.project(LastCursor).toPoint()));
Node* B2 = g_backend.allocNode(theMain->document()->getDirtyOrOriginLayer(), XY_TO_COORD(
FB2.project(LastCursor).toPoint()));
L->add(new AddFeatureCommand(theMain->document()->getDirtyOrOriginLayer(),B1,true));
L->add(new AddFeatureCommand(theMain->document()->getDirtyOrOriginLayer(),B2,true));
L->add(new WayAddNodeCommand(R1,B1));
L->add(new WayAddNodeCommand(R2,B2,(int)0));
document()->addHistory(L);
view()->invalidate(true, true, false);
//FirstPoint = view()->projection().inverse(anEvent->pos());
PreviousPoints[R1->size()-1] = XY_TO_COORD(anEvent->pos());
FirstDistance = DockData.RoadDistance->text().toDouble();
}
}
else
{
Coord PreviousPoint = FirstPoint;
if (distance(COORD_TO_XY(PreviousPoint), LastCursor) > 1)
{
qreal rB = view()->pixelPerM()*DockData.RoadDistance->text().toDouble()/2;
qreal rA = FirstDistance * view()->pixelPerM()/2;
LineF FA1(COORD_TO_XY(PreviousPoint),LastCursor);
LineF FA2(FA1);
LineF FB1(FA1);
LineF FB2(FA1);
qreal Modifier = DockData.DriveRight->isChecked()?1:-1;
FA1.slide(rA*Modifier);
FA2.slide(-rA*Modifier);
FB1.slide(rB*Modifier);
FB2.slide(-rB*Modifier);
Node* A1 = g_backend.allocNode(theMain->document()->getDirtyOrOriginLayer(), XY_TO_COORD(
FA1.project(COORD_TO_XY(PreviousPoint)).toPoint()));
Node* A2 = g_backend.allocNode(theMain->document()->getDirtyOrOriginLayer(), XY_TO_COORD(
FA2.project(COORD_TO_XY(PreviousPoint)).toPoint()));
Node* B1 = g_backend.allocNode(theMain->document()->getDirtyOrOriginLayer(), XY_TO_COORD(
FB1.project(LastCursor).toPoint()));
Node* B2 = g_backend.allocNode(theMain->document()->getDirtyOrOriginLayer(), XY_TO_COORD(
FB2.project(LastCursor).toPoint()));
R1 = g_backend.allocWay(theMain->document()->getDirtyOrOriginLayer());
R2 = g_backend.allocWay(theMain->document()->getDirtyOrOriginLayer());
CommandList* L = new CommandList(MainWindow::tr("Create double-way Road %1").arg(R1->id().numId), R1);
L->add(new AddFeatureCommand(theMain->document()->getDirtyOrOriginLayer(),A1,true));
L->add(new AddFeatureCommand(theMain->document()->getDirtyOrOriginLayer(),A2,true));
L->add(new AddFeatureCommand(theMain->document()->getDirtyOrOriginLayer(),B1,true));
L->add(new AddFeatureCommand(theMain->document()->getDirtyOrOriginLayer(),B2,true));
L->add(new AddFeatureCommand(theMain->document()->getDirtyOrOriginLayer(),R1,true));
L->add(new AddFeatureCommand(theMain->document()->getDirtyOrOriginLayer(),R2,true));
if (M_PREFS->getAutoSourceTag()) {
QStringList sl = theMain->document()->getCurrentSourceTags();
if (sl.size()) {
R1->setTag("source", sl.join(";"));
R2->setTag("source", sl.join(";"));
}
}
R1->setTag("oneway","yes");
R2->setTag("oneway","yes");
L->add(new WayAddNodeCommand(R1,A1));
L->add(new WayAddNodeCommand(R1,B1));
L->add(new WayAddNodeCommand(R2,B2));
L->add(new WayAddNodeCommand(R2,A2));
document()->addHistory(L);
view()->invalidate(true, true, false);
//FirstPoint = view()->projection().inverse(anEvent->pos());
PreviousPoints[R1->size()-1] = XY_TO_COORD(anEvent->pos());
FirstDistance = DockData.RoadDistance->text().toDouble();
}
}
}
else
Interaction::mousePressEvent(anEvent);
}
int main (int argc, char **argv) {
ImageDataset d;
d.loadImagesFromFile("train-images-idx3-ubyte");
d.loadLabelsFromFile("train-labels-idx1-ubyte");
std::shared_ptr<clneural::ActivationFunction> act(new clneural::SigmoidActivationFunction());
std::shared_ptr<clneural::ActivationFunction> act2(new clneural::LinearActivationFunction());
std::vector<std::list<unsigned int>> C1_connections(6, std::list<unsigned int>({0}));
clneural::ConvolutionalLayer::Dimension C1_input;
clneural::ConvolutionalLayer::Dimension C1_filter;
float training_speed = 0.7f;
C1_input.width = 32;
C1_input.height = 32;
C1_filter.width = 5;
C1_filter.height = 5;
std::shared_ptr<clneural::NeuralNetworkLayer> C1(new clneural::ConvolutionalLayer(C1_input, C1_filter, C1_connections, act, training_speed));
clneural::SubsamplingLayer::Dimension S2_input;
clneural::SubsamplingLayer::Dimension S2_filter;
S2_input.width = 28;
S2_input.height = 28;
S2_filter.width = 2;
S2_filter.height = 2;
std::shared_ptr<clneural::NeuralNetworkLayer> S2(new clneural::SubsamplingLayer(S2_input, S2_filter, 6, act2, training_speed));
std::vector<std::list<unsigned int>> C3_connections(16);
C3_connections[0] = std::list<unsigned int>({0,1,2});
C3_connections[1] = std::list<unsigned int>({1,2,3});
C3_connections[2] = std::list<unsigned int>({2,3,4});
C3_connections[3] = std::list<unsigned int>({3,4,5});
C3_connections[4] = std::list<unsigned int>({4,5,0});
C3_connections[5] = std::list<unsigned int>({5,0,1});
C3_connections[6] = std::list<unsigned int>({0,1,2,3});
C3_connections[7] = std::list<unsigned int>({1,2,3,4});
C3_connections[8] = std::list<unsigned int>({2,3,4,5});
C3_connections[9] = std::list<unsigned int>({3,4,5,0});
C3_connections[10] = std::list<unsigned int>({4,5,0,1});
C3_connections[11] = std::list<unsigned int>({5,0,1,2});
C3_connections[12] = std::list<unsigned int>({0,1,3,4});
C3_connections[13] = std::list<unsigned int>({1,2,4,5});
C3_connections[14] = std::list<unsigned int>({0,2,3,5});
C3_connections[15] = std::list<unsigned int>({0,1,2,3,4,5});
clneural::ConvolutionalLayer::Dimension C3_input;
clneural::ConvolutionalLayer::Dimension C3_filter;
C3_input.width = 14;
C3_input.height = 14;
C3_filter.width = 5;
C3_filter.height = 5;
std::shared_ptr<clneural::NeuralNetworkLayer> C3(new clneural::ConvolutionalLayer(C3_input, C3_filter, C3_connections, act, training_speed));
clneural::SubsamplingLayer::Dimension S4_input;
clneural::SubsamplingLayer::Dimension S4_filter;
S4_input.width = 10;
S4_input.height = 10;
S4_filter.width = 2;
S4_filter.height = 2;
std::shared_ptr<clneural::NeuralNetworkLayer> S4(new clneural::SubsamplingLayer(S4_input, S4_filter, 16, act2, training_speed));
std::shared_ptr<clneural::NeuralNetworkLayer> N1(new clneural::FullFeedforwardLayer(400, 84, act, training_speed));
std::shared_ptr<clneural::NeuralNetworkLayer> N2(new clneural::FullFeedforwardLayer(84, 10, act, training_speed));
clneural::NeuralNetwork n;
n.addLayer(C1);
n.addLayer(S2);
n.addLayer(C3);
n.addLayer(S4);
n.addLayer(N1);
n.addLayer(N2);
std::shared_ptr<OpenCLInterface> ocl = OpenCLInterface::getInstance();
ocl->initialize(CL_DEVICE_TYPE_CPU);
float dist = 0.0f;
for (unsigned int i = 0; i < 60000; i++) {
std::pair<std::vector<float>, uint8_t> trainelem = d.popRandomElementWithLabel();
std::vector<float> desired(10, 0.0f);
desired[trainelem.second] = 1.0f;
dist += n.trainNetwork(trainelem.first, desired);
std::vector<float> nout = n.getLastOutput();
if ((i % 1000) == 0) {
std::cout << "TIME: " << ((float) clock())/CLOCKS_PER_SEC << ", STEP:" << (i + 1) << ", MDIST: " << dist/1000.0f << ", OUT: (" << nout[0];
for (unsigned int j = 1; j < nout.size(); j++) std::cout << "," << nout[j];
std::cout << "), DESIRED: (" << desired[0];
for (unsigned int j = 1; j < desired.size(); j++) std::cout << "," << desired[j];
std::cout << ")" << std::endl;
dist = 0.0f;
}
}
n.saveToFile("conv_images1.net");
verifyNetwork(n);
return 0;
}
void ViewmdaModel::reset_selections() {
m_zoom_rect=QRect(-1,-1,1,1);
m_selected_rect=QRect(-1,-1,1,1);
m_selected_points=QImage(N1(),N2(),QImage::Format_Mono);
}
N (THREAD_CPUTIME),
N (THREAD_PRIO_INHERIT),
N (THREAD_PRIO_PROTECT),
N (THREAD_PRIORITY_SCHEDULING),
N (THREAD_PROCESS_SHARED),
N (THREAD_SAFE_FUNCTIONS),
N (THREAD_SPORADIC_SERVER),
N (THREADS),
N (TIMEOUTS),
N (TIMERS),
N (TRACE),
N (TRACE_EVENT_FILTER),
N (TRACE_INHERIT),
N (TRACE_LOG),
N (TYPED_MEMORY_OBJECTS),
N2 (C_BIND),
N2 (C_DEV),
N2 (CHAR_TERM)
};
#define nposix_options (sizeof (posix_options) / sizeof (posix_options[0]))
static int
do_test (void)
{
int result = 0;
for (int i = 0; i < nposix_options; ++i)
{
long int scret = sysconf (posix_options[i]._SC_val);
if (scret == 0)
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
// trw_bprop_helper2(model, psi_ij_rho, psi_i, rho, maxiter, n, m1, m2, b_i,...
// dm1, dm2, dn, dpsi_i, dpsi_ij);
int i=0;
int nnodes = mapdouble(mxGetField(prhs[i],0,"nnodes"))(0);
int ncliques = mapdouble(mxGetField(prhs[i],0,"ncliques"))(0);
int nvals = mapdouble(mxGetField(prhs[i],0,"nvals"))(0);
MatrixXi pairs = mapdouble(mxGetField(prhs[i],0,"pairs")).cast<int>();
pairs.array() -= 1;
MatrixXi N1 = mapdouble(mxGetField(prhs[i],0,"N1")).cast<int>();
N1.array() -= 1;
MatrixXi N2 = mapdouble(mxGetField(prhs[i],0,"N2")).cast<int>();
N2.array() -= 1;
MatrixXi tree2clique = mapdouble(mxGetField(prhs[i],0,"tree2clique")).cast<int>();
tree2clique.array() -= 1;
MatrixXi treeschedule = mapdouble(mxGetField(prhs[i],0,"treeschedule")).cast<int>();
treeschedule.array() -= 1;
i++;
MatrixMd psi_ij = mapdouble(prhs[i++]);
MatrixMd psi_i = mapdouble(prhs[i++]);
double rho = mapdouble(prhs[i++])(0);
int maxiter = mapdouble(prhs[i++])(0);
MatrixMd n = mapdouble(prhs[i++]);
MatrixMd m1 = mapdouble(prhs[i++]);
MatrixMd m2 = mapdouble(prhs[i++]);
MatrixMd b_i = mapdouble(prhs[i++]);
MatrixMd mstor = mapdouble(prhs[i++]);
MatrixMd dm1 = mapdouble(prhs[i++]);
MatrixMd dm2 = mapdouble(prhs[i++]);
MatrixMd dn = mapdouble(prhs[i++]);
MatrixMd dpsi_i = mapdouble(prhs[i++]);
MatrixMd dpsi_ij = mapdouble(prhs[i++]);
MatrixMd b_ij0 = mapdouble(prhs[i++]);
MatrixMd db_ij0 = mapdouble(prhs[i++]);
int dorec = mapdouble(prhs[i++]).cast<int>()(0);
MatrixMi w = mapint32(prhs[i++]);
#pragma omp parallel for num_threads(NTHREAD)
for(int c=0; c<ncliques; c++) {
int i = pairs(c, 0);
int j = pairs(c, 1);
for(int yi=0; yi<nvals; yi++) {
for(int yj=0; yj<nvals; yj++) {
int index = yi + yj*nvals;
//b_ij0(index,c) = b_ij0(index,c)*psi_i(yi,i)*psi_i(yj,j)*n(yi,i)*n(yj,j)/m1(yi,c)/m2(yj,c);
dpsi_i(yi, i) = dpsi_i(yi, i) + db_ij0(index, c)*b_ij0(index, c)/psi_i(yi, i);
dpsi_i(yj, j) = dpsi_i(yj, j) + db_ij0(index, c)*b_ij0(index, c)/psi_i(yj, j);
dn(yi, i) = dn(yi, i) + db_ij0(index, c)*b_ij0(index, c)/n(yi, i);
dn(yj, j) = dn(yj, j) + db_ij0(index, c)*b_ij0(index, c)/n(yj, j);
dm1(yi, c) = dm1(yi, c) - db_ij0(index, c)*b_ij0(index, c)/m1(yi, c);
dm2(yj, c) = dm2(yj, c) - db_ij0(index, c)*b_ij0(index, c)/m2(yj, c);
}
}
}
#pragma omp parallel for num_threads(NTHREAD)
for(int i=0; i<b_i.cols(); i++) {
for(int yi=0; yi<nvals; yi++) {
for(int k=0; k<N1.cols(); k++) {
int d = N1(i, k);
if(d==-2) continue;
//n(yi, i) *= m1(yi, d);
dm1(yi,d) += rho * dn(yi,i)*n(yi,i)/m1(yi,d);
}
for(int k=0; k<N2.cols(); k++) {
int d = N2(i, k);
if(d==-2) continue;
//n(yi, i) *= m2(yi, d);
dm2(yi,d) += rho * dn(yi,i)*n(yi,i)/m2(yi,d);
}
}
}
//tree_ncliques = sum(double(model.tree2clique>0),1);
//ntree = length(tree_ncliques);
int ntree = tree2clique.cols();
MatrixXi tree_ncliques = MatrixXi::Zero(ntree,1);
for(int tree=0; tree<tree2clique.cols(); tree++)
for(i=0; i<tree2clique.rows(); i++)
if(tree2clique(i,tree) != -1)
tree_ncliques(tree)++;
int reps;
double conv;
for(reps=0; reps<maxiter; reps++) {
// must re-order blocks
//for(int block=0; block<treeschedule.cols(); block++){
for(int block=treeschedule.cols()-1; block>=0; block--) {
// need not re-order trees, but why not...
// helps if someone specifies not parallel trees
// for(int treenum=treeschedule.rows()-1; treenum>=0; treenum--){
#pragma omp parallel for schedule(dynamic) num_threads(NTHREAD)
for(int treenum=0; treenum<treeschedule.rows(); treenum++) {
MatrixXd S (nvals,nvals);
MatrixXd m0(nvals,1);
int tree = treeschedule(treenum,block);
if(tree==-1)
continue;
int ncliques = tree_ncliques(tree);
for(int c0=2*ncliques-1; c0>=0; c0--) {
int c, mode;
if(c0<ncliques) {
c = tree2clique(c0,tree);
mode = 1;
} else {
c = tree2clique(ncliques - 1 - (c0-ncliques),tree);
mode = 2;
}
//cout << "BW c " << c << " mode " << mode << endl;
int i = pairs(c,0);
int j = pairs(c,1);
if( mode==1 ) {
for(int yi=0; yi<nvals; yi++)
n(yi, i) = 1;
for(int k=0; k<N1.cols(); k++) {
int d = N1(i, k);
if(d==-2) continue;
for(int yi=0; yi<nvals; yi++)
n(yi, i) *= m1(yi, d);
}
for(int k=0; k<N2.cols(); k++) {
int d = N2(i, k);
if(d==-2) continue;
for(int yi=0; yi<nvals; yi++)
n(yi, i) *= m2(yi, d);
}
// n.col(i) = n.col(i).array().pow(rho);
if(rho==1) {
//nothing
} else if(rho==.5) {
n.col(i) = n.col(i).array().sqrt();
} else
n.col(i) = n.col(i).array().pow(rho);
// compute m(y_j)
for(int yj=0; yj<nvals; yj++) {
m0(yj) = 0;
for(int yi=0; yi<nvals; yi++) {
int index = yi + yj*nvals;
S(yi,yj) = psi_ij(index, c)*psi_i(yi, i)*n(yi, i)/m1(yi, c);
m0(yj) += S(yi,yj);
}
}
double k = S.sum();
MatrixXd dm0 = dm2.col(c)/k;
dm0.array() -= (dm2.col(c).array()*m0.array()).sum()/(k*k);
MatrixXd dn = 0*n.col(i);
for(int yj=0; yj<nvals; yj++) {
for(int yi=0; yi<nvals; yi++) {
int index = yi + yj*nvals;
dpsi_ij(index,c) += dm0(yj)*S(yi,yj)/psi_ij(index,c);
dpsi_i(yi,i) += dm0(yj)*S(yi,yj)/psi_i(yi,i);
dn(yi) += dm0(yj)*S(yi,yj)/n(yi,i);
dm1(yi,c) -= dm0(yj)*S(yi,yj)/m1(yi,c);
}
}
for(int yi=0; yi<nvals; yi++) {
for(int k=0; k<N1.cols(); k++) {
int d = N1(i, k);
if(d==-2) continue;
dm1(yi,d) += rho*dn(yi)*n(yi,i)/m1(yi,d);
}
for(int k=0; k<N2.cols(); k++) {
int d = N2(i, k);
if(d==-2) continue;
dm2(yi,d) += rho*dn(yi)*n(yi,i)/m2(yi,d);
}
}
dm2.col(c) *= 0.0;
if( dorec )
for(int yj=nvals-1; yj>=0; yj--) {
w(tree)=w(tree)-1;
m2(yj,c)=mstor(w(tree),tree);
}
} else if( mode==2 ) {
for( int yj=0; yj<nvals; yj++)
n(yj, j) = 1;
for(int k=0; k<N1.cols(); k++) {
int d = N1(j, k);
if(d==-2) continue;
for( int yj=0; yj<nvals; yj++)
n(yj, j) *= m1(yj, d);
}
for(int k=0; k<N2.cols(); k++) {
int d = N2(j, k);
if(d==-2) continue;
for( int yj=0; yj<nvals; yj++)
n(yj, j) *= m2(yj, d);
}
// n.col(j) = n.col(j).array().pow(rho);
if(rho==1) {
//nothing
} else if(rho==.5) {
n.col(j) = n.col(j).array().sqrt();
} else
n.col(j) = n.col(j).array().pow(rho);
for(int yi=0; yi<nvals; yi++) {
m0(yi) = 0;
for(int yj=0; yj<nvals; yj++) {
int index = yi + yj*nvals;
S(yi,yj) = psi_ij(index, c)*psi_i(yj, j)*n(yj, j)/m2(yj, c);
m0(yi) += S(yi,yj);
}
}
double k = S.sum();
MatrixXd dm0 = dm1.col(c)/k;
dm0.array() -= (dm1.col(c).array()*m0.array()).sum()/(k*k);
MatrixXd dn = 0*n.col(i);
for(int yj=0; yj<nvals; yj++) {
for(int yi=0; yi<nvals; yi++) {
int index = yi + yj*nvals;
dpsi_ij(index,c) += dm0(yi)*S(yi,yj)/psi_ij(index,c);
dpsi_i(yj,j) += dm0(yi)*S(yi,yj)/psi_i(yj,j);
dn(yj) += dm0(yi)*S(yi,yj)/n(yj,j);
dm2(yj,c) -= dm0(yi)*S(yi,yj)/m2(yj,c);
}
}
for(int yj=0; yj<nvals; yj++) {
for(int k=0; k<N1.cols(); k++) {
int d = N1(j, k);
if(d==-2) continue;
dm1(yj, d) += rho*dn(yj)*n(yj, j)/m1(yj, d);
}
for(int k=0; k<N2.cols(); k++) {
int d = N2(j, k);
if(d==-2) continue;
dm2(yj, d) += rho*dn(yj)*n(yj, j)/m2(yj, d);
}
}
dm1.col(c) *= 0.0;
if( dorec )
for(int yi=nvals-1; yi>=0; yi--) {
w(tree)=w(tree)-1;
m1(yi,c)=mstor(w(tree),tree);
}
}
}
}
}
}
}
void AnalyticCompoundOptionEngine::calculate() const {
QL_REQUIRE(strikeDaughter()>0.0,
"Daughter strike must be positive");
QL_REQUIRE(strikeMother()>0.0,
"Mother strike must be positive");
QL_REQUIRE(spot() >= 0.0, "negative or null underlying given");
/* Solver Setup ***************************************************/
Date helpDate(process_->riskFreeRate()->referenceDate());
Date helpMaturity=helpDate+(maturityDaughter()-maturityMother())*Days;
Real vol =process_->blackVolatility()->blackVol(helpMaturity,
strikeDaughter());
Time helpTimeToMat=process_->time(helpMaturity);
vol=vol*std::sqrt(helpTimeToMat);
DiscountFactor dividendDiscount =
process_->dividendYield()->discount(helpMaturity);
DiscountFactor riskFreeDiscount =
process_->riskFreeRate()->discount(helpMaturity);
boost::shared_ptr<ImpliedSpotHelper> f(
new ImpliedSpotHelper(dividendDiscount, riskFreeDiscount,
vol, payoffDaughter(), strikeMother()));
Brent solver;
solver.setMaxEvaluations(1000);
Real accuracy = 1.0e-6;
Real X=0.0;
Real sSolved=0.0;
sSolved=solver.solve(*f, accuracy, strikeDaughter(), 1.0e-6, strikeDaughter()*1000.0);
X=transformX(sSolved); // transform stock to return as in Wystup's book
/* Solver Setup Finished*****************************************/
Real phi=typeDaughter(); // -1 or 1
Real w=typeMother(); // -1 or 1
Real rho=std::sqrt(residualTimeMother()/residualTimeDaughter());
BivariateCumulativeNormalDistributionDr78 N2(w*rho) ;
DiscountFactor ddD=dividendDiscountDaughter();
DiscountFactor rdD=riskFreeDiscountDaughter();
//DiscountFactor ddM=dividendDiscountMother();
DiscountFactor rdM=riskFreeDiscountMother();
Real XmSM=X-stdDeviationMother();
Real S=spot();
Real dP=dPlus();
Real dPT12=dPlusTau12(sSolved);
Real vD=volatilityDaughter();
Real dM=dMinus();
Real strD=strikeDaughter();
Real strM=strikeMother();
Real rTM=residualTimeMother();
Real rTD=residualTimeDaughter();
Real rD=riskFreeRateDaughter();
Real dD=dividendRateDaughter();
Real tempRes=0.0;
Real tempDelta=0.0;
Real tempGamma=0.0;
Real tempVega=0.0;
Real tempTheta=0.0;
Real N2XmSM=N2(-phi*w*XmSM,phi*dP);
Real N2X=N2(-phi*w*X,phi*dM);
Real NeX=N_(-phi*w*e(X));
Real NX=N_(-phi*w*X);
Real NT12=N_(phi*dPT12);
Real ndP=n_(dP);
Real nXm=n_(XmSM);
Real invMTime=1/std::sqrt(rTM);
Real invDTime=1/std::sqrt(rTD);
tempRes=phi*w*S*ddD*N2XmSM-phi*w*strD*rdD*N2X-w*strM*rdM*NX;
tempDelta=phi*w*ddD*N2XmSM;
tempGamma=(ddD/(vD*S))*(invMTime*nXm*NT12+w*invDTime*ndP*NeX);
tempVega=ddD*S*((1/invMTime)*nXm*NT12+w*(1/invDTime)*ndP*NeX);
tempTheta+=phi*w*dD*S*ddD*N2XmSM-phi*w*rD*strD*rdD*N2X-w*rD*strM*rdM*NX;
tempTheta-=0.5*vD*S*ddD*(invMTime*nXm*NT12+w*invDTime*ndP*NeX);
results_.value=tempRes;
results_.delta=tempDelta;
results_.gamma=tempGamma;
results_.vega=tempVega;
results_.theta=tempTheta;
}
int main(int argc, char *argv[])
{
# include "addTimeOptions.H"
# include "setRootCase.H"
# include "createTime.H"
// Get times list
instantList Times = runTime.times();
// set startTime and endTime depending on -time and -latestTime options
# include "checkTimeOptions.H"
runTime.setTime(Times[startTime], startTime);
# include "createMesh.H"
for (label i=startTime; i<endTime; i++)
{
runTime.setTime(Times[i], i);
Info<< "Time = " << runTime.timeName() << endl;
mesh.readUpdate();
IOobject tauHeader
(
"tau",
runTime.timeName(),
mesh,
IOobject::MUST_READ
);
// Check tau exists
if (tauHeader.headerOk())
{
mesh.readUpdate();
Info<< " Reading tau" << endl;
volSymmTensorField tau(tauHeader, mesh);
Info<< " Calculating N1"<< endl;
volScalarField N1
(
IOobject
(
"N1",
runTime.timeName(),
mesh,
IOobject::NO_READ
),
tau.component(symmTensor::XX) - tau.component(symmTensor::YY)
);
N1.write();
Info<< " Calculating N2"<< endl;
volScalarField N2
(
IOobject
(
"N2",
runTime.timeName(),
mesh,
IOobject::NO_READ
),
tau.component(symmTensor::YY) - tau.component(symmTensor::ZZ)
);
N2.write();
}
else
{
Info<< " No tau" << endl;
}
Info<< endl;
} //for time
Info<< endl;
Info<< " End"<< nl << endl;
return(0);
}
dtype32* Mda32::dataPtr(bigint i1, bigint i2, bigint i3)
{
return d->data() + (i1 + N1() * i2 + N1() * N2() * i3);
}
void distribution(Particle& p, State& st)
{
static const double Gamma = GlobalConfig.get<double>("Gamma");
show_message(msgStart, Module_electronDistribution);
const ParamSpace& ps{ p.ps };
const double rmin = ps[DIM_R].first();
const double rmax = ps[DIM_R].last();
const int nR = ps[DIM_R].size()-1;
ParamSpaceValues N2(p.ps);
N2.fill([&](const SpaceIterator& i){
return 0.0;
});
ParamSpaceValues N12(p.ps);
N12.fill([&](const SpaceIterator& i){
return 0.0;
});
static const std::string injector = GlobalConfig.get<std::string>("injector");
bool multiple = (injector == "multiple");
bool single = (injector == "single");
int superior;
superior = p.ps[1].size(); //este es el caso de multiple
for (int t_ix = 0; t_ix < p.ps[2].size(); t_ix++) {
//for (int z_ix = 0; z_ix < p.ps[1].size(); z_ix++) { //emisores para todo z
//for (int z_ix = 0; z_ix <= t_ix; z_ix++) { //unico emisor en z=0
if (single) { superior = t_ix + 1; }
//el +1 es para que el for incluya el t_ix
for (int z_ix = 0; z_ix < superior; z_ix++) {
N2.ps.iterate([&](const SpaceIterator& i){
N2.set(i, p.distribution.get(i)); //copia del N
}); //ver si quizas lo puedo copiar en t-1 que es donde lo necesito
p.ps.iterate([&](const SpaceIterator& i){
const double E = i.val(DIM_E);
const double r = i.val(DIM_R);
const double t = i.val(DIM_T);
const double magf = st.magf.get(i);
//
// equivale a:
// i.its[0].val();
// i.its[0].peek(DIM_E);
//
// equivalia a:
// ex i.par.T;
double Emax = eEmax(r, magf);
double tp = t / Gamma; //time in the FF
if (E < Emax){
double Eeff = effectiveE(E, Emax, tp, r, p, st, i);
double dist1(0.0), dist2(0.0);
dist1 = timeDistribution(E, r, tp, p, st, Eeff, i);
if (t_ix != 0)
{ //estos son los puntos donde Q=0, y las particulas vienen de ti-1
//if (i.its[2].canPeek(-1))
double dist = N2.interpolate({ { DIM_E, Eeff }, { DIM_R, r }, { DIM_T, i.its[2].peek(-1) } });
double ratioLosses = losses(Eeff, r, p, st, i) / losses(E, r, p, st, i);
dist2 = dist*ratioLosses;
}
N12.set(i, dist1 + dist2); //lo cargo en N12 mientras interpolo de N2
//N12.set(i, dist1); //lo cargo en N12 mientras interpolo de N2
}
else
{
N12.set(i, 0.0); //si E>Emax entonces anulo la N
}
}, { -1, z_ix, t_ix });
p.ps.iterate([&](const SpaceIterator& i){
double ni = N12.get(i); //el ni es que que obtengo con N12
double dist3;
double ri = i.its[1].peek(0);
double rip1;
if (i.its[1].canPeek(-1)) //if (z_ix != 0)
{
SpaceCoord coord = i.moved({ 0, -1, 0 }); //N(ri-1)
double ni_1 = p.distribution.get(coord); //este lo calculo con el p.dist porque ya esta en r-1
double ri_1 = i.its[1].peek(-1);
if (i.its[1].canPeek(1)) { rip1 = i.its[1].peek(+1); }
else{
double r_int = pow((rmax / rmin), (1.0 / nR));
rip1 = ri*r_int;
}
dist3 = ni*(1.0 - ri / rip1) + ni_1*(ri_1 / ri);
}
else //z_ix=0
{
rip1 = i.its[1].peek(+1);
dist3 = ni*(1.0 - ri / rip1);
}
p.distribution.set(i,dist3); // lleno p.distribution e interpolo en N12
//p.distribution.set(i, ni); // prueba
} , { -1, z_ix, t_ix });
}//for sobre r
}//for sobre t
show_message(msgEnd, Module_electronDistribution);
}
double* Mda::dataPtr(bigint i1, bigint i2, bigint i3)
{
return d->data() + (i1 + N1() * i2 + N1() * N2() * i3);
}
double
vpHomography::computeDisplacement(unsigned int nbpoint,
vpPoint *c1P,
vpPoint *c2P,
vpPlane *oN,
vpHomogeneousMatrix &c2Mc1,
vpHomogeneousMatrix &c1Mo,
int userobust
)
{
vpColVector e(2) ;
double r_1 = -1 ;
vpColVector p2(3) ;
vpColVector p1(3) ;
vpColVector Hp2(3) ;
vpColVector Hp1(3) ;
vpMatrix H2(2,6) ;
vpColVector e2(2) ;
vpMatrix H1(2,6) ;
vpColVector e1(2) ;
int only_1 = 1 ;
int only_2 = 0 ;
int iter = 0 ;
unsigned int i ;
unsigned int n=0 ;
n = nbpoint ;
if ((only_1==1) || (only_2==1)) ; else n *=2 ;
vpRobust robust(n);
vpColVector res(n) ;
vpColVector w(n) ;
w =1 ;
robust.setThreshold(0.0000) ;
vpMatrix W(2*n,2*n) ;
W = 0 ;
vpColVector N1(3), N2(3) ;
double d1, d2 ;
double r =1e10 ;
iter =0 ;
while (vpMath::equal(r_1,r,threshold_displacement) == false )
{
r_1 =r ;
// compute current position
//Change frame (current)
vpHomogeneousMatrix c1Mc2, c2Mo ;
vpRotationMatrix c1Rc2, c2Rc1 ;
vpTranslationVector c1Tc2, c2Tc1 ;
c1Mc2 = c2Mc1.inverse() ;
c2Mc1.extract(c2Rc1) ;
c2Mc1.extract(c2Tc1) ;
c2Mc1.extract(c1Rc2) ;
c1Mc2.extract(c1Tc2) ;
c2Mo = c2Mc1*c1Mo ;
vpMatrix L(2,3), Lp ;
int k =0 ;
for (i=0 ; i < nbpoint ; i++)
{
getPlaneInfo(oN[i], c1Mo, N1, d1) ;
getPlaneInfo(oN[i], c2Mo, N2, d2) ;
p2[0] = c2P[i].get_x() ;
p2[1] = c2P[i].get_y() ;
p2[2] = 1.0 ;
p1[0] = c1P[i].get_x() ;
p1[1] = c1P[i].get_y() ;
p1[2] = 1.0 ;
vpMatrix H(3,3) ;
Hp2 = ((vpMatrix)c1Rc2 + ((vpMatrix)c1Tc2*N2.t())/d2)*p2 ; // p2 = Hp1
Hp1 = ((vpMatrix)c2Rc1 + ((vpMatrix)c2Tc1*N1.t())/d1)*p1 ; // p1 = Hp2
Hp2 /= Hp2[2] ; // normalisation
Hp1 /= Hp1[2] ;
// set up the interaction matrix
double x = Hp2[0] ;
double y = Hp2[1] ;
double Z1 ;
Z1 = (N1[0]*x+N1[1]*y+N1[2])/d1 ; // 1/z
H2[0][0] = -Z1 ; H2[0][1] = 0 ; H2[0][2] = x*Z1 ;
H2[1][0] = 0 ; H2[1][1] = -Z1 ; H2[1][2] = y*Z1 ;
H2[0][3] = x*y ; H2[0][4] = -(1+x*x) ; H2[0][5] = y ;
H2[1][3] = 1+y*y ; H2[1][4] = -x*y ; H2[1][5] = -x ;
H2 *=-1 ;
vpMatrix c1CFc2(6,6) ;
{
vpMatrix sTR = c1Tc2.skew()*(vpMatrix)c1Rc2 ;
for (unsigned int k=0 ; k < 3 ; k++)
for (unsigned int l=0 ; l<3 ; l++)
{
c1CFc2[k][l] = c1Rc2[k][l] ;
c1CFc2[k+3][l+3] = c1Rc2[k][l] ;
c1CFc2[k][l+3] = sTR[k][l] ;
}
}
H2 = H2*c1CFc2 ;
// Set up the error vector
e2[0] = Hp2[0] - c1P[i].get_x() ;
e2[1] = Hp2[1] - c1P[i].get_y() ;
x = Hp1[0] ;
y = Hp1[1] ;
Z1 = (N2[0]*x+N2[1]*y+N2[2])/d2 ; // 1/z
H1[0][0] = -Z1 ; H1[0][1] = 0 ; H1[0][2] = x*Z1 ;
H1[1][0] = 0 ; H1[1][1] = -Z1 ; H1[1][2] = y*Z1;
H1[0][3] = x*y ; H1[0][4] = -(1+x*x) ; H1[0][5] = y ;
H1[1][3] = 1+y*y ; H1[1][4] = -x*y ; H1[1][5] = -x ;
// Set up the error vector
e1[0] = Hp1[0] - c2P[i].get_x() ;
e1[1] = Hp1[1] - c2P[i].get_y() ;
if (only_2==1)
{
if (k == 0) { L = H2 ; e = e2 ; }
else
{
L = vpMatrix::stackMatrices(L,H2) ;
e = vpMatrix::stackMatrices(e,e2) ;
}
}
else
if (only_1==1)
{
if (k == 0) { L = H1 ; e= e1 ; }
else
{
L = vpMatrix::stackMatrices(L,H1) ;
e = vpMatrix::stackMatrices(e,e1) ;
}
}
else
{
if (k == 0) {L = H2 ; e = e2 ; }
else
{
L = vpMatrix::stackMatrices(L,H2) ;
e = vpMatrix::stackMatrices(e,e2) ;
}
L = vpMatrix::stackMatrices(L,H1) ;
e = vpMatrix::stackMatrices(e,e1) ;
}
k++ ;
}
if (userobust)
{
robust.setIteration(0);
for (unsigned int k=0 ; k < n ; k++)
{
res[k] = vpMath::sqr(e[2*k]) + vpMath::sqr(e[2*k+1]) ;
}
robust.MEstimator(vpRobust::TUKEY, res, w);
// compute the pseudo inverse of the interaction matrix
for (unsigned int k=0 ; k < n ; k++)
{
W[2*k][2*k] = w[k] ;
W[2*k+1][2*k+1] = w[k] ;
}
}
else
{
for (unsigned int k=0 ; k < 2*n ; k++) W[k][k] = 1 ;
}
(W*L).pseudoInverse(Lp, 1e-16) ;
// Compute the camera velocity
vpColVector c2Tcc1 ;
c2Tcc1 = -1*Lp*W*e ;
// only for simulation
c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse()*c2Mc1 ; ;
// UpdatePose2(c2Tcc1, c2Mc1) ;
r =(W*e).sumSquare() ;
if (r < 1e-15) {break ; }
if (iter>1000){break ; }
if (r>r_1) { break ; }
iter++ ;
}
return (W*e).sumSquare() ;
}
// mcmc_helper(im,im_prev,buf,int(xwho),int(xwho2));
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
// trw_simple_helper(model,c_ij, psi_i, rho, n, m1, m2);
// need from model struct:
//model =
//
// nnodes: 10000
//ncliques: 20000
// nvals: 2
// pairs: [20000x2 double]
// N1: {10000x1 cell}
// N2: {10000x1 cell}
int i=0;
int nnodes = mapdouble(mxGetField(prhs[i],0,"nnodes"))(0);
int ncliques = mapdouble(mxGetField(prhs[i],0,"ncliques"))(0);
int nvals = mapdouble(mxGetField(prhs[i],0,"nvals"))(0);
MatrixXi pairs = mapdouble(mxGetField(prhs[i],0,"pairs")).cast<int>();
pairs.array() -= 1;
MatrixXi N1 = mapdouble(mxGetField(prhs[i],0,"N1")).cast<int>();
N1.array() -= 1;
MatrixXi N2 = mapdouble(mxGetField(prhs[i],0,"N2")).cast<int>();
N2.array() -= 1;
//vector<MatrixXi> N1;
//vector<MatrixXi> N2;
//mxArray *mxN1 = mxGetField(prhs[i],0,"N1");
//mxArray *mxN2 = mxGetField(prhs[i],0,"N2");
//for(int node=0; node<nnodes; node++){
// N1.push_back( mapdouble(mxGetCell(mxN1,node)).cast<int>() );
// N2.push_back( mapdouble(mxGetCell(mxN2,node)).cast<int>() );
// N1[node].cwise() -= 1;
// N2[node].cwise() -= 1;
//}
i++;
MatrixMd psi_ij = mapdouble(prhs[i++]);
MatrixMd psi_i = mapdouble(prhs[i++]);
double rho = mapdouble(prhs[i++])(0);
int maxiter = mapdouble(prhs[i++])(0);
double damp = mapdouble(prhs[i++])(0);
double convthresh = mapdouble(prhs[i++])(0);
MatrixMd n = mapdouble(prhs[i++]);
MatrixMd m1 = mapdouble(prhs[i++]);
MatrixMd m2 = mapdouble(prhs[i++]);
MatrixMd b_i = mapdouble(prhs[i++]);
MatrixMd b_ij = mapdouble(prhs[i++]);
MatrixMd Z_ij = mapdouble(prhs[i++]);
MatrixXd b_i_save = b_i;
MatrixXd S (nvals,nvals);
MatrixXd m0(nvals,1);
int reps;
double conv;
for(reps=0; reps<maxiter; reps++){
for(int c0=0; c0<ncliques*2; c0++){
int c, mode;
if(c0<ncliques){
c = c0;
mode = 1;
} else{
c = ncliques - 1 - (c0-ncliques);
mode = 2;
}
int i = pairs(c,0);
int j = pairs(c,1);
if( mode==1 ){
for(int yi=0; yi<nvals; yi++)
n(yi, i) = 1;
for(int k=0; k<N1.cols(); k++){
int d = N1(i, k);
if(d==-2) continue;
for(int yi=0; yi<nvals; yi++)
n(yi, i) *= m1(yi, d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(i, k);
if(d==-2) continue;
for(int yi=0; yi<nvals; yi++)
n(yi, i) *= m2(yi, d);
}
for(int yi=0; yi<nvals; yi++)
n(yi, i) = pow(n(yi, i), rho);
// compute m(y_j)
for(int yj=0; yj<nvals; yj++){
m0(yj) = 0;
for(int yi=0; yi<nvals; yi++){
int index = yi + yj*nvals;
S(yi,yj) = psi_ij(index, c)*psi_i(yi, i)*n(yi, i)/m1(yi, c);
m0(yj) += S(yi,yj);
}
}
double k = S.sum();
m2.col(c) = m0/k;
} else{
for( int yj=0; yj<nvals; yj++)
n(yj, j) = 1;
for(int k=0; k<N1.cols(); k++){
int d = N1(j, k);
if(d==-2) continue;
for( int yj=0; yj<nvals; yj++)
n(yj, j) *= m1(yj, d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(j, k);
if(d==-2) continue;
for( int yj=0; yj<nvals; yj++)
n(yj, j) *= m2(yj, d);
}
for( int yj=0; yj<nvals; yj++)
n(yj, j) = pow(n(yj, j), rho);
for(int yi=0; yi<nvals; yi++){
m0(yi) = 0;
for(int yj=0; yj<nvals; yj++){
int index = yi + yj*nvals;
S(yi,yj) = psi_ij(index, c)*psi_i(yj, j)*n(yj, j)/m2(yj, c);
m0(yi) += S(yi,yj);
}
}
double k = S.sum();
m1.col(c) = m0/k;
}
}
b_i = n.array()*psi_i.array();
norm_cols(b_i);
conv = (b_i-b_i_save).array().abs().maxCoeff();
//cout << "reps: " << reps << " conv: " << conv << endl;
if(conv < convthresh)
break;
b_i_save = b_i;
}
//cout << "reps: " << reps << " conv: " << conv << endl;
b_ij = psi_ij;
for(int c=0; c<ncliques; c++){
int i = pairs(c,0);
int j = pairs(c,1);
for(int yi=0; yi<nvals; yi++){
for(int yj=0; yj<nvals; yj++){
int index = yi + yj*nvals;
b_ij(index,c) = b_ij(index,c)*psi_i(yi,i)*psi_i(yj,j)*n(yi,i)*n(yj,j)/m1(yi,c)/m2(yj,c);
}
}
}
Z_ij = b_ij.colwise().sum();
for(int c=0; c<ncliques; c++)
b_ij.col(c) = b_ij.col(c)/Z_ij(c);
plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL);
double *outArray;
outArray = mxGetPr(plhs[0]);
outArray[0] = reps;
}
