void rectify(Mat& K1, Mat& R1, Mat& t1,
Mat& K2, Mat& R2, Mat& t2,
Size size,
Mat& T1, Mat& T2) {
Mat_<double> Kn1(3, 3), Kn2(3, 3);
Mat_<double> c1(3, 1), c2(3, 1),
v1(3, 1), v2(3, 1), v3(3, 1);
K1.copyTo(Kn1);
K2.copyTo(Kn2);
// optical centers
c1 = - R1.t() * t1;
c2 = - R2.t() * t2;
// x axis
v1 = c2 - c1;
// y axis
v2 = Mat(R1.row(2).t()).cross(v1);
// z axis
v3 = v1.cross(v2);
Mat_<double> m_R(3, 3);
v1 = v1 / norm(v1);
v2 = v2 / norm(v2);
v3 = v3 / norm(v3);
m_R(0, 0) = v1(0), m_R(0, 1) = v1(1), m_R(0, 2) = v1(2);
m_R(1, 0) = v2(0), m_R(1, 1) = v2(1), m_R(1, 2) = v2(2);
m_R(2, 0) = v3(0), m_R(2, 1) = v3(1), m_R(2, 2) = v3(2);
T1 = (Kn1 * m_R) * (R1.t() * K1.inv());
T2 = (Kn2 * m_R) * (R2.t() * K2.inv());
// Image center displacement
Mat_<double> o1(3, 1), o2(3, 1);
o1(0) = 0.5 * size.width, o1(1) = 0.5 * size.height, o1(2) = 1.0;
o2(0) = 0.5 * size.width, o2(1) = 0.5 * size.height, o2(2) = 1.0;
Mat_<double> on1(3, 1), on2(3, 1), d1(2, 1), d2(2, 1);
on1 = T1 * o1;
on2 = T2 * o2;
d1(0) = o1(0) - on1(0) / on1(2);
d1(1) = o1(1) - on1(1) / on1(2);
d2(0) = o2(0) - on2(0) / on1(2);
d2(1) = o2(1) - on2(1) / on1(2);
d1(1) = d2(1);
Kn1(0, 2) = Kn1(0, 2) + d1(0);
Kn1(1, 2) = Kn1(1, 2) + d1(1);
Kn2(0, 2) = Kn2(0, 2) + d2(0);
Kn2(1, 2) = Kn2(1, 2) + d2(1);
T1 = (Kn1 * m_R) * (R1.t() * K1.inv());
T2 = (Kn2 * m_R) * (R2.t() * K2.inv());
}
bool EKalman::findObservation(Detections& det, int frame, int /*detPos*/, int /*startTimeStemp*/)
{
Vector<double> succPoint;
if(m_Up)
frame = frame + 1;
else
frame = frame - 1;
Volume<double> obsCol;
FrameInlier inlier(frame);
Vector<int> inl;
Vector<double> weights;
double colScore = 1.0;
double weight;
Vector<int> allInlierInOneFrame;
Vector<double> weightOfAllInliersInOneFrame;
// Matrix<double> covCopy;
for(int i = 0; i < det.numberDetectionsAtFrame(frame); i++)
{
Matrix<double> devObs;
Vector<double> currBbox;
det.getPos3D(frame, i, succPoint);
det.getColorHist(frame, i, obsCol);
det.get3Dcovmatrix(frame, i, devObs);
det.getBBox(frame, i, currBbox);
colScore = Math::hist_bhatta(obsCol, m_colHist);
Matrix<double> covariance(2,2, 0.0);
covariance(0,0) = sqrt(devObs(0,0));
covariance(1,1) = sqrt(devObs(2,2));
covariance.inv();
// covCopy = covariance;
Vector<double> p1(2,0.0);
Vector<double> p2(2,0.0);
p1(0) = m_xprio(0);
p1(1) = m_xprio(1);
p2(0) = succPoint(0);
p2(1) = succPoint(2);
Vector<double> pDiff(2,0.0);
pDiff = p1;
pDiff -= p2;
covariance *= pDiff;
covariance.Transpose();
covariance *= pDiff;
weight = exp(-0.5*covariance(0,0));
// IMAGE BASED
Vector<double> rectInter;
AncillaryMethods::IntersetRect(m_bbox, currBbox, rectInter);
double iou = rectInter(2)*rectInter(3)/(m_bbox(2)*m_bbox(3)+currBbox(2)*currBbox(3)-rectInter(2)*rectInter(3));
// if(iou>0.2 && colScore > Globals::kalmanObsColorModelthresh)
// {
// allInlierInOneFrame.pushBack(i);
// weightOfAllInliersInOneFrame.pushBack(iou*colScore);
// }
// 3D POSITION BASED
if(weight > Globals::kalmanObsMotionModelthresh /*&& colScore > Globals::kalmanObsColorModelthresh*/)
{
allInlierInOneFrame.pushBack(i);
weightOfAllInliersInOneFrame.pushBack(weight*colScore);
}
}
if(allInlierInOneFrame.getSize()>0)
{
pair<double, int> maxPosValue = weightOfAllInliersInOneFrame.maxim();
int pos = maxPosValue.second;
inlier.addInlier(allInlierInOneFrame(pos));
inlier.addWeight(weightOfAllInliersInOneFrame(pos));
}
m_measurement_found = false;
if(inlier.getNumberInlier() > 0)
{
m_measurement_found = true;
Matrix<double> covMatrix;
inl = inlier.getInlier();
weights = inlier.getWeight();
// Update the color histogram
Volume<double> newColHist;
det.getColorHist(frame, inl(0), newColHist);
m_colHist *= 0.4;
newColHist *= 0.6;
m_colHist += newColHist;
m_height = det.getHeight(frame, inl(0));
det.get3Dcovmatrix(frame,inl(0), covMatrix);
Vector<double> inlierPos;
m_yPos.setSize(3,0.0);
for(int i = 0; i < allInlierInOneFrame.getSize(); i++)
{
det.getPos3D(frame, allInlierInOneFrame(i), inlierPos);
m_yPos += inlierPos;
}
m_yPos *= 1.0/(double) allInlierInOneFrame.getSize();
FrameInlier newInlier(frame);
newInlier.addInlier(inl(0));
newInlier.addWeight(weights(0)*det.getScore(frame, inl(0)));
m_Idx.pushBack(newInlier);
m_R.set_size(4,4, 0.0);
m_R(0,0) = sqrt(covMatrix(0,0));
m_R(1,1) = sqrt(covMatrix(2,2));
m_R(2,2) = 0.2*0.2;
m_R(3,3) = 0.2*0.2;
}
return m_measurement_found;
}
void EKalman::runKalmanDown(Detections& det, int frame, int pointPos, int t, Matrix<double>& allXnew, Vector<FrameInlier>& Idx,
Vector<double>& R, Vector<double>& Vel, Volume<double>& hMean, Vector<Matrix<double> >& stateCovMats,
Vector<Volume<double> >& colHists)
{
m_Up = false;
det.getColorHist(frame, pointPos, m_colHist);
det.getBBox(frame, pointPos, m_bbox);
Matrix<double> copyInitStateUnc = m_Ppost;
Vector<double> copyInitState = m_xpost;
//////////////////////////////////////////////////////////////////////
Matrix<double> covMatrix;
det.get3Dcovmatrix(frame, pointPos, covMatrix);
Vector<double> startingDet;
det.getPos3D(frame, pointPos, startingDet);
m_R.fill(0.0);
m_R(0,0) = covMatrix(0,0);
m_R(1,1) = covMatrix(2,2);
m_R(2,2) = 0.2*0.2;
m_R(3,3) = 0.2*0.2;
int tLastSupport = 0;
m_yPos.pushBack(m_xpost(0));
m_yPos.pushBack(startingDet(1));
m_yPos.pushBack(m_xpost(1));
int accepted_frames_without_det = 20;
for(int i = frame+1; i > t; i--)
{
if(tLastSupport > accepted_frames_without_det)
break;
predict();
m_measurement_found = findObservation(det, i, pointPos, frame);
if(m_measurement_found)
{
m_measurement = makeMeasurement();
update();
if(i == frame+1)
{
m_Ppost = copyInitStateUnc;
m_xpost = copyInitState;
}
saveData(i-1);
tLastSupport = 0;
pointPos = m_Idx(m_Idx.getSize()-1).getInlier()(0);
}
else
{
update();
m_xpost(3) *= 0.8;
saveData(i-1);
tLastSupport += 1;
}
}
if(m_Rot.getSize() > 1) {
// As this is kalman-DOWN, we start with an unassociated detection (InitState)
// The position is fine, but we fix the rotation by copying the one from the successor state
m_Rot(0) = m_Rot(1);
}
Vel = m_Vel;
turnRotations(R, m_Rot);
Idx = m_Idx;
allXnew = Matrix<double>(m_allXnew);
hMean = m_colHist;
colHists = m_colHists;
stateCovMats = m_CovMats;
}
void
MAST::ComplexAssemblyBase::
residual_and_jacobian_blocked (const libMesh::NumericVector<Real>& X,
libMesh::NumericVector<Real>& R,
libMesh::SparseMatrix<Real>& J,
MAST::Parameter* p) {
libmesh_assert(_system);
libmesh_assert(_discipline);
libmesh_assert(_elem_ops);
START_LOG("residual_and_jacobian()", "ComplexSolve");
MAST::NonlinearSystem& nonlin_sys = _system->system();
R.zero();
J.zero();
// iterate over each element, initialize it and get the relevant
// analysis quantities
RealVectorX
sol;
ComplexVectorX
delta_sol,
vec;
ComplexMatrixX
mat,
dummy;
// get the petsc vector and matrix objects
Mat
jac_bmat = dynamic_cast<libMesh::PetscMatrix<Real>&>(J).mat();
PetscInt ierr;
std::vector<libMesh::dof_id_type> dof_indices;
const libMesh::DofMap& dof_map = nonlin_sys.get_dof_map();
const std::vector<libMesh::dof_id_type>&
send_list = nonlin_sys.get_dof_map().get_send_list();
std::unique_ptr<libMesh::NumericVector<Real> >
localized_base_solution,
localized_complex_sol(libMesh::NumericVector<Real>::build(nonlin_sys.comm()).release());
// prepare a send list for localization of the complex solution
std::vector<libMesh::dof_id_type>
complex_send_list(2*send_list.size());
for (unsigned int i=0; i<send_list.size(); i++) {
complex_send_list[2*i ] = 2*send_list[i];
complex_send_list[2*i+1] = 2*send_list[i]+1;
}
localized_complex_sol->init(2*nonlin_sys.n_dofs(),
2*nonlin_sys.n_local_dofs(),
complex_send_list,
false,
libMesh::GHOSTED);
X.localize(*localized_complex_sol, complex_send_list);
// localize the base solution, if it was provided
if (_base_sol)
localized_base_solution.reset(build_localized_vector(nonlin_sys,
*_base_sol).release());
// if a solution function is attached, initialize it
//if (_sol_function)
// _sol_function->init( X);
libMesh::MeshBase::const_element_iterator el =
nonlin_sys.get_mesh().active_local_elements_begin();
const libMesh::MeshBase::const_element_iterator end_el =
nonlin_sys.get_mesh().active_local_elements_end();
MAST::ComplexAssemblyElemOperations&
ops = dynamic_cast<MAST::ComplexAssemblyElemOperations&>(*_elem_ops);
for ( ; el != end_el; ++el) {
const libMesh::Elem* elem = *el;
dof_map.dof_indices (elem, dof_indices);
ops.init(*elem);
// get the solution
unsigned int ndofs = (unsigned int)dof_indices.size();
sol.setZero(ndofs);
delta_sol.setZero(ndofs);
vec.setZero(ndofs);
mat.setZero(ndofs, ndofs);
// first set the velocity to be zero
ops.set_elem_velocity(sol);
// next, set the base solution, if provided
if (_base_sol)
for (unsigned int i=0; i<dof_indices.size(); i++)
sol(i) = (*localized_base_solution)(dof_indices[i]);
ops.set_elem_solution(sol);
// set the value of the small-disturbance solution
for (unsigned int i=0; i<dof_indices.size(); i++) {
// get the complex block for this dof
delta_sol(i) = Complex((*localized_complex_sol)(2*dof_indices[i]),
(*localized_complex_sol)(2*dof_indices[i]+1));
}
ops.set_elem_complex_solution(delta_sol);
// if (_sol_function)
// physics_elem->attach_active_solution_function(*_sol_function);
// perform the element level calculations
ops.elem_calculations(true, vec, mat);
// if sensitivity was requested, then ask the element for sensitivity
// of the residual
if (p) {
// set the sensitivity of complex sol to zero
delta_sol.setZero();
ops.set_elem_complex_solution_sensitivity(delta_sol);
vec.setZero();
ops.elem_sensitivity_calculations(*p, vec);
}
ops.clear_elem();
//physics_elem->detach_active_solution_function();
// extract the real or the imaginary part of the matrix/vector
// The complex system of equations
// (J_R + i J_I) (x_R + i x_I) + (r_R + i r_I) = 0
// is rewritten as
// [ J_R -J_I] {x_R} + {r_R} = {0}
// [ J_I J_R] {x_I} + {r_I} = {0}
//
DenseRealVector v_R, v_I;
DenseRealMatrix m_R, m_I1, m_I2;
std::vector<Real> vals(4);
// copy the real part of the residual and Jacobian
MAST::copy( m_R, mat.real());
MAST::copy(m_I1, mat.imag()); m_I1 *= -1.; // this is the -J_I component
MAST::copy(m_I2, mat.imag()); // this is the J_I component
MAST::copy( v_R, vec.real());
MAST::copy( v_I, vec.imag());
dof_map.constrain_element_matrix(m_R, dof_indices);
dof_map.constrain_element_matrix(m_I1, dof_indices);
dof_map.constrain_element_matrix(m_I2, dof_indices);
dof_map.constrain_element_vector(v_R, dof_indices);
dof_map.constrain_element_vector(v_I, dof_indices);
for (unsigned int i=0; i<dof_indices.size(); i++) {
R.add(2*dof_indices[i], v_R(i));
R.add(2*dof_indices[i]+1, v_I(i));
for (unsigned int j=0; j<dof_indices.size(); j++) {
vals[0] = m_R (i,j);
vals[1] = m_I1(i,j);
vals[2] = m_I2(i,j);
vals[3] = m_R (i,j);
ierr = MatSetValuesBlocked(jac_bmat,
1, (PetscInt*)&dof_indices[i],
1, (PetscInt*)&dof_indices[j],
&vals[0],
ADD_VALUES);
}
}
}
// if a solution function is attached, clear it
//if (_sol_function)
// _sol_function->clear();
R.close();
J.close();
libMesh::out << "R: " << R.l2_norm() << std::endl;
STOP_LOG("residual_and_jacobian()", "ComplexSolve");
}
