/* ************************************************************************* */
VectorValues GaussianConditional::solveOtherRHS(
const VectorValues& parents, const VectorValues& rhs) const
{
// Concatenate all vector values that correspond to parent variables
Vector xS = parents.vector(FastVector<Key>(beginParents(), endParents()));
// Instead of updating getb(), update the right-hand-side from the given rhs
const Vector rhsR = rhs.vector(FastVector<Key>(beginFrontals(), endFrontals()));
xS = rhsR - get_S() * xS;
// Solve Matrix
Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS);
// Scale by sigmas
if(model_)
soln.array() *= model_->sigmas().array();
// Insert solution into a VectorValues
VectorValues result;
DenseIndex vectorPosition = 0;
for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
result.insert(*frontal, soln.segment(vectorPosition, getDim(frontal)));
vectorPosition += getDim(frontal);
}
return result;
}
/* this function takes in the Y, Pb, Pr representation and outputs the
4 pixels in a UArray_T
*/
UArray_T CVC_to_rgb_pixels(CVC *YPbPr)
{
UArray_T r_array = UArray_new(PIX_PER_BLOCK, sizeof(struct Pnm_rgb));
assert(r_array != NULL);
Pnm_rgb cur_pix;
Pnm_rgb_float cur_pix_float = malloc(sizeof(struct Pnm_rgb_float));
for (int i = 0; i < PIX_PER_BLOCK; i++) {
cur_pix = (Pnm_rgb)UArray_at(r_array, i);
assert(cur_pix != NULL);
cur_pix_float->red = get_R(YPbPr, i);
cur_pix_float->green = get_G(YPbPr, i);
cur_pix_float->blue = get_B(YPbPr, i);
denormalize_pixel(cur_pix_float, DEFAULT_DENOMINATOR);
/* we could have rounded this properly before assigning
to integers, but we ran out of time. sorry! */
cur_pix->red = cur_pix_float->red;
cur_pix->blue = cur_pix_float->blue;
cur_pix->green = cur_pix_float->green;
}
free(cur_pix_float);
return r_array;
}
/* ************************************************************************* */
VectorValues GaussianConditional::solve(const VectorValues& x) const
{
// Concatenate all vector values that correspond to parent variables
const Vector xS = x.vector(FastVector<Key>(beginParents(), endParents()));
// Update right-hand-side
const Vector rhs = get_d() - get_S() * xS;
// Solve matrix
const Vector solution = get_R().triangularView<Eigen::Upper>().solve(rhs);
// Check for indeterminant solution
if (solution.hasNaN()) {
throw IndeterminantLinearSystemException(keys().front());
}
// Insert solution into a VectorValues
VectorValues result;
DenseIndex vectorPosition = 0;
for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
result.insert(*frontal, solution.segment(vectorPosition, getDim(frontal)));
vectorPosition += getDim(frontal);
}
return result;
}
//get inverse rotation matrix
Matrix get_Ri(Point vrp, Point vpn, Point vup) {
Matrix m = get_R(vrp, vpn, vup);
Row r1 = { m[0][0], m[1][0], m[2][0], m[3][0] };
Row r2 = { m[0][1], m[1][1], m[2][1], m[3][1] };
Row r3 = { m[0][2], m[1][2], m[2][2], m[3][2] };
Row r4 = { m[0][3], m[1][3], m[2][3], m[3][3] };
return {r1,r2,r3,r4};
}
static void
bestlift_init(long a, GEN nf, GEN pr, GEN C, nflift_t *L)
{
const long D = 100;
const double alpha = ((double)D-1) / D; /* LLL parameter */
const long d = degpol(nf[1]);
pari_sp av = avma;
GEN prk, PRK, B, GSmin, pk;
pari_timer ti;
TIMERstart(&ti);
if (!a) a = (long)bestlift_bound(C, d, alpha, pr_norm(pr));
for (;; avma = av, a<<=1)
{
if (DEBUGLEVEL>2) fprintferr("exponent: %ld\n",a);
PRK = prk = idealpows(nf, pr, a);
pk = gcoeff(prk,1,1);
/* reduce size first, "scramble" matrix */
PRK = lllintpartial_ip(PRK);
/* now floating point reduction is fast */
PRK = lllint_fp_ip(PRK, 4);
PRK = lllint_i(PRK, D, 0, NULL, NULL, &B);
if (!PRK) { PRK = prk; GSmin = pk; } /* nf = Q */
else
{
pari_sp av2 = avma;
GEN S = invmat( get_R(PRK) ), BB = GS_norms(B, DEFAULTPREC);
GEN smax = gen_0;
long i, j;
for (i=1; i<=d; i++)
{
GEN s = gen_0;
for (j=1; j<=d; j++)
s = gadd(s, gdiv( gsqr(gcoeff(S,i,j)), gel(BB,j)));
if (gcmp(s, smax) > 0) smax = s;
}
GSmin = gerepileupto(av2, ginv(gmul2n(smax, 2)));
}
if (gcmp(GSmin, C) >= 0) break;
}
if (DEBUGLEVEL>2)
fprintferr("for this exponent, GSmin = %Z\nTime reduction: %ld\n",
GSmin, TIMER(&ti));
L->k = a;
L->den = L->pk = pk;
L->prk = PRK;
L->iprk = ZM_inv(PRK, pk);
L->GSmin= GSmin;
L->prkHNF = prk;
init_proj(L, gel(nf,1), gel(pr,1));
}
// Modified to R and t instead of Rt and Rt*t
void Camera::getRay(Vector<double>& x, Vector<double>& ray1, Vector<double>& ray2)
{
Matrix<double> rot = get_R();
Matrix<double> Kinv = get_K();
// Vector<double> pos = get_t();
ray1 = t_;
Kinv.inv();
rot *= Kinv;
ray2 = rot * x;
ray2 += ray1;
}
/* ************************************************************************* */
void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const
{
cout << s << " Conditional density ";
for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
}
cout << endl;
cout << formatMatrixIndented(" R = ", get_R()) << endl;
for(const_iterator it = beginParents() ; it != endParents() ; ++it ) {
cout << formatMatrixIndented((boost::format(" S[%1%] = ")%(formatter(*it))).str(), getA(it))
<< endl;
}
cout << formatMatrixIndented(" d = ", getb(), true) << "\n";
if(model_)
model_->print(" Noise model: ");
else
cout << " No noise model" << endl;
}
/* ************************************************************************* */
bool GaussianConditional::equals(const GaussianFactor& f, double tol) const
{
if (const GaussianConditional* c = dynamic_cast<const GaussianConditional*>(&f))
{
// check if the size of the parents_ map is the same
if (parents().size() != c->parents().size())
return false;
// check if R_ and d_ are linear independent
for (DenseIndex i = 0; i < Ab_.rows(); i++) {
list<Vector> rows1; rows1.push_back(Vector(get_R().row(i)));
list<Vector> rows2; rows2.push_back(Vector(c->get_R().row(i)));
// check if the matrices are the same
// iterate over the parents_ map
for (const_iterator it = beginParents(); it != endParents(); ++it) {
const_iterator it2 = c->beginParents() + (it - beginParents());
if (*it != *(it2))
return false;
rows1.push_back(row(getA(it), i));
rows2.push_back(row(c->getA(it2), i));
}
Vector row1 = concatVectors(rows1);
Vector row2 = concatVectors(rows2);
if (!linear_dependent(row1, row2, tol))
return false;
}
// check if sigmas are equal
if ((model_ && !c->model_) || (!model_ && c->model_)
|| (model_ && c->model_ && !model_->equals(*c->model_, tol)))
return false;
return true;
}
else
{
return false;
}
}
/* ************************************************************************* */
void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const
{
Vector frontalVec = gy.vector(FastVector<Key>(beginFrontals(), endFrontals()));
frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R()));
// Check for indeterminant solution
if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front());
for (const_iterator it = beginParents(); it!= endParents(); it++)
gy[*it] += -1.0 * Matrix(getA(it)).transpose() * frontalVec;
// Scale by sigmas
if(model_)
frontalVec.array() *= model_->sigmas().array();
// Write frontal solution into a VectorValues
DenseIndex vectorPosition = 0;
for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
gy[*frontal] = frontalVec.segment(vectorPosition, getDim(frontal));
vectorPosition += getDim(frontal);
}
}
double xmax = -0.0175;
double ymax = 0.0175;
/* camera position */
Point VRP = {128.0, 64.0, 250.0};
Vector VPN = {-64.0, 0.0, -186.0};
Vector VUP = {0.0, 1.0, 0.0};
double focal = 0.05; /* focal length simulating 50 mm lens */
Vector Light = {0.577, -0.577, -0.577}; /* light direction */
double Ip = 255.0; /* intensity of the point light source */
/* Transformation from the world to the camera coordinates */
Matrix Mwc = get_M(VRP, VPN, VUP);
Matrix Rwc = get_R(VRP, VPN, VUP);
Matrix Twc = get_T(VRP);
/* Transformation from the camera to the world coordinates */
Matrix Mcw = get_Mi(VRP, VPN, VUP);
Matrix Rcw = get_Ri(VRP, VPN, VUP);
Matrix Tcw = get_Ti(VRP);
int main () {
//main program for volume rendering
Volume* ct = new Volume;
read_from_file("smallHead.den", ct);
//print_ct_volume(ct);
Volume* color = new Volume;
compute_shading_volume(ct, color);
//print_ct_volume(color);
//this is the world to view final matrix, which is Mwc, also Mwl
Matrix get_M(Point vrp, Point vpn, Point vup) {
return mul(get_R(vrp, vpn, vup), get_T(vrp));
}
