void AncillaryMethods::intersectIdx(Vector< FrameInlier >& idx1, Vector< FrameInlier >& idx2, Vector< FrameInlier >& intersection) {
int j = 0;
int k = 0;
intersection.clearContent();
Vector<int> inter;
Vector<int> inlier1;
Vector<int> inlier2;
Vector<double> weight1;
Vector<double> weight2;
if (idx1.getSize() > 0 && idx2.getSize() > 0) {
for (int i = 0; i < idx1.getSize(); i++) {
while (idx1(i).getFrame() > idx2(j).getFrame() && j < idx2.getSize() - 1) {
j++;
}
if (j == idx2.getSize())
break;
if (idx1(i).getFrame() == idx2(j).getFrame()) {
inlier1 = idx1(i).getInlier();
inlier2 = idx2(j).getInlier();
weight1 = idx1(i).getWeight();
weight2 = idx2(j).getWeight();
inlier1.intersection(inlier2, inter);
if (inter.getSize() > 0) {
FrameInlier inlier(idx1(i).getFrame());
for ( int l = 0; l < inlier1.getSize(); l++) {
while (inlier1(l) > inlier2(k) && k < inlier2.getSize() - 1) {
k++;
}
if (k == inlier2.getSize())
break;
if (inlier1(l) == inlier2(k)) {
inlier.addInlier(inlier1(l));
inlier.addWeight(weight1(l));
inlier.addWeight(weight2(k));
}
}
k = 0;
intersection.pushBack(inlier);
}
}
}
}
}
pcl::PointIndices::Ptr
RansacMethod(pcl::PointCloud<pcl::PointXYZ>::Ptr cloudp, pcl::ModelCoefficients::Ptr coeff)
{
//Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inlier (new pcl::PointIndices ());
// Optional
seg.setOptimizeCoefficients(true);
// Mandatory
seg.setModelType(pcl::SACMODEL_SPHERE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(1000);
seg.setDistanceThreshold(0.01);
seg.setRadiusLimits(0.20315, 0.20325);
seg.setInputCloud(cloudp);
seg.segment(*inlier, *coeff);
return inlier;
}
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;
}
