void ImageNV21::toYCRCB(Image& dst) const throw(FormatMismatch){
if (dst.format() != ImageYCRCB::FORMAT_YCRCB)
throw Image::FormatMismatch("FORMAT_YCRCB required for dst");
ImageYCRCB rgbImg(dst.height,dst.width);
toRGB8(rgbImg);
cv::cvtColor(rgbImg,dst,CV_RGB2YCrCb);
}
int main(int argc, char * argv[])
{
int width = 640;
int height = 480;
parse_argument(argc, argv, "-w", width);
parse_argument(argc, argv, "-h", height);
Resolution::getInstance(width, height);
Bytef * decompressionBuffer = new Bytef[Resolution::getInstance().numPixels() * 2];
IplImage * deCompImage = 0;
std::string logFile;
assert(parse_argument(argc, argv, "-l", logFile) > 0 && "Please provide a log file");
RawLogReader logReader(decompressionBuffer,
deCompImage,
logFile,
find_argument(argc, argv, "-f") != -1);
cv::Mat1b tmp(height, width);
cv::Mat3b depthImg(height, width);
while(logReader.hasMore())
{
logReader.getNext();
cv::Mat3b rgbImg(height, width, (cv::Vec<unsigned char, 3> *)logReader.deCompImage->imageData);
cv::Mat1w depth(height, width, (unsigned short *)&decompressionBuffer[0]);
cv::normalize(depth, tmp, 0, 255, cv::NORM_MINMAX, 0);
cv::cvtColor(tmp, depthImg, CV_GRAY2RGB);
cv::imshow("RGB", rgbImg);
cv::imshow("Depth", depthImg);
char key = cv::waitKey(1);
if(key == 'q')
break;
else if(key == ' ')
key = cv::waitKey(0);
if(key == 'q')
break;
}
delete [] decompressionBuffer;
if(deCompImage)
{
cvReleaseImage(&deCompImage);
}
return 0;
}
void USBCamReader::readCurFrame(unsigned char* rgbdata,
unsigned char* graydata) {
assert(videoCap);
IplImage* img = cvRetrieveFrame(videoCap);
cv::Mat rawFrame(img);
cv::Mat rgbImg(_h, _w, CV_8UC3, rgbdata);
cv::cvtColor(rawFrame, rgbImg, CV_BGR2RGB);
cv::Mat videoFrame(_h, _w, CV_8UC1, graydata);
cv::cvtColor(rawFrame, videoFrame, CV_RGB2GRAY);
}
int main(int argc, char * argv[])
{
int width = 640;
int height = 480;
Resolution::getInstance(width, height);
Intrinsics::getInstance(528, 528, 320, 240);
cv::Mat intrinsicMatrix = cv::Mat(3,3,CV_64F);
intrinsicMatrix.at<double>(0,0) = Intrinsics::getInstance().fx();
intrinsicMatrix.at<double>(1,1) = Intrinsics::getInstance().fy();
intrinsicMatrix.at<double>(0,2) = Intrinsics::getInstance().cx();
intrinsicMatrix.at<double>(1,2) = Intrinsics::getInstance().cy();
intrinsicMatrix.at<double>(0,1) =0;
intrinsicMatrix.at<double>(1,0) =0;
intrinsicMatrix.at<double>(2,0) =0;
intrinsicMatrix.at<double>(2,1) =0;
intrinsicMatrix.at<double>(2,2) =1;
Bytef * decompressionBuffer = new Bytef[Resolution::getInstance().numPixels() * 2];
IplImage * deCompImage = 0;
std::string logFile="/home/lili/Kinect_Logs/2015-11-05.00.klg";
// assert(pcl::console::parse_argument(argc, argv, "-l", logFile) > 0 && "Please provide a log file");
RawLogReader logReader(decompressionBuffer,
deCompImage,
logFile,
true);
cv::Mat1b tmp(height, width);
cv::Mat3b depthImg(height, width);
PlaceRecognition placeRecognition(&intrinsicMatrix);
iSAMInterface iSAM;
//Keyframes
KeyframeMap map(true);
Eigen::Vector3f lastPlaceRecognitionTrans = Eigen::Vector3f::Zero();
Eigen::Matrix3f lastPlaceRecognitionRot = Eigen::Matrix3f::Identity();
int64_t lastTime = 0;
OdometryProvider * odom = 0;
//int frame_index = 0;
// uint64_t timestamp;
/*if(true)
{
odom = new FOVISOdometry;
if(logReader.hasMore())
{
logReader.getNext();
Eigen::Matrix3f Rcurr = Eigen::Matrix3f::Identity();
Eigen::Vector3f tcurr = Eigen::Vector3f::Zero();
odom->getIncrementalTransformation(tcurr,
Rcurr,
logReader.timestamp,
(unsigned char *)logReader.deCompImage->imageData,
(unsigned short *)&decompressionBuffer[0]);
}
}*/
//else
// {
odom = new DVOdometry;
if(logReader.hasMore())
{
logReader.getNext();
DVOdometry * dvo = static_cast<DVOdometry *>(odom);
dvo->firstRun((unsigned char *)logReader.deCompImage->imageData,
(unsigned short *)&decompressionBuffer[0]);
}
//}
ofstream fout1("camera_pose_DVOMarch28.txt");
ofstream fout2("camera_pose_KeyframeMotionMetric0.1March28.txt");
ofstream fout3("loop_closure_transformationMarch28.txt");
ofstream fout4("camera_pose_after_optimizationMarch28.txt");
ofstream fout5("camera_pose_after_optimizationMarch28DVOCov.txt");
ofstream fout6("camera_pose_after_optimizationMarch28DVOLoopTransCov.txt");
/*
pcl::visualization::PCLVisualizer cloudViewer;
cloudViewer.setBackgroundColor(1, 1, 1);
cloudViewer.initCameraParameters();
cloudViewer.addCoordinateSystem(0.1, 0, 0, 0);
*/
//pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> color(cloud->makeShared());
//cloudViewer.addPointCloud<pcl::PointXYZRGB>(cloud->makeShared(), color, "Cloud Viewer");
int loopClosureCount=0;
while(logReader.hasMore())
{
logReader.getNext();
cv::Mat3b rgbImg(height, width, (cv::Vec<unsigned char, 3> *)logReader.deCompImage->imageData);
cv::Mat1w depth(height, width, (unsigned short *)&decompressionBuffer[0]);
cv::normalize(depth, tmp, 0, 255, cv::NORM_MINMAX, 0);
cv::cvtColor(tmp, depthImg, CV_GRAY2RGB);
cv::imshow("RGB", rgbImg);
cv::imshow("Depth", depthImg);
char key = cv::waitKey(1);
if(key == 'q')
{
break;
}
else if(key == ' ')
{
key = cv::waitKey(0);
}
if(key == 'q')
{
break;
}
Eigen::Matrix3f Rcurr = Eigen::Matrix3f::Identity();
Eigen::Vector3f tcurr = Eigen::Vector3f::Zero();
// #1
odom->getIncrementalTransformation(tcurr,
Rcurr,
logReader.timestamp,
(unsigned char *)logReader.deCompImage->imageData,
(unsigned short *)&decompressionBuffer[0]);
fout1<<tcurr[0]<<" "<<tcurr[1]<<" "<<tcurr[2]<<" "<<Rcurr(0,0)<<" "<<Rcurr(0,1)<<" "<<Rcurr(0,2)<<" "<<Rcurr(1,0)<<" "<<Rcurr(1,1)<<" "<<Rcurr(1,2)<<" "<<Rcurr(2,0)<<" "<<Rcurr(2,1)<<" "<<Rcurr(2,2)<<endl;
Eigen::Matrix3f Rdelta = Rcurr.inverse() * lastPlaceRecognitionRot;
Eigen::Vector3f tdelta = tcurr - lastPlaceRecognitionTrans;
//Eigen::MatrixXd covariance = odom->getCovariance();
//Eigen::MatrixXd covariance=Eigen::Matrix<double, 6, 6>::Identity()* 1e-3;
if((Projection::rodrigues2(Rdelta).norm() + tdelta.norm()) >= 0.1)
{
Eigen::MatrixXd covariance = odom->getCovariance();
iSAM.addCameraCameraConstraint(lastTime,
logReader.timestamp,
lastPlaceRecognitionRot,
lastPlaceRecognitionTrans,
Rcurr,
tcurr);
//covariance);
printCovariance(fout5, covariance);
lastTime = logReader.timestamp;
lastPlaceRecognitionRot = Rcurr;
lastPlaceRecognitionTrans = tcurr;
cout<<"before add keyframe"<<endl;
// #2
map.addKeyframe((unsigned char *)logReader.deCompImage->imageData,
(unsigned short *)&decompressionBuffer[0],
Rcurr,
tcurr,
logReader.timestamp);
fout2<<tcurr[0]<<" "<<tcurr[1]<<" "<<tcurr[2]<<" "<<Rcurr(0,0)<<" "<<Rcurr(0,1)<<" "<<Rcurr(0,2)<<" "<<Rcurr(1,0)<<" "<<Rcurr(1,1)<<" "<<Rcurr(1,2)<<" "<<Rcurr(2,0)<<" "<<Rcurr(2,1)<<" "<<Rcurr(2,2)<<endl;
/*
//Save keyframe
{
cv::Mat3b rgbImgKeyframe(height, width, (cv::Vec<unsigned char, 3> *)logReader.deCompImage->imageData);
cv::Mat1w depthImgKeyframe(height, width, (unsigned short *)&decompressionBuffer[0]);
//save keyframe depth
char fileName[1024] = {NULL};
sprintf(fileName, "keyframe_depth_%06d.png", frame_index);
cv::imwrite(fileName, depthImgKeyframe);
//save keyframe rgb
sprintf(fileName, "keyframe_rgb_%06d.png", frame_index);
cv::imwrite(fileName, rgbImgKeyframe);
frame_index ++;
}
*/
int64_t matchTime;
Eigen::Matrix4d transformation;
// Eigen::MatrixXd cov(6,6);
//isam::Covariance(0.001 * Eigen::Matrix<double, 6, 6>::Identity()))
Eigen::MatrixXd cov=0.001 * Eigen::Matrix<double, 6, 6>::Identity();
cout<<"map.addKeyframe is OK"<<endl;
// #3
if(placeRecognition.detectLoop((unsigned char *)logReader.deCompImage->imageData,
(unsigned short *)&decompressionBuffer[0],
logReader.timestamp,
matchTime,
transformation,
cov,
loopClosureCount))
{
//printCovariance(fout6, cov);
cout<<"logReader.timestamp "<<logReader.timestamp<<endl;
cout<<"matchTime "<<matchTime<<endl;
/*
transformation << -0.2913457145219732, 0.228056050293173, -0.9290361201559172, 2.799184934345601,
0.6790194052589797, 0.7333821627861707, -0.03291277242681545, 1.310438143604587,
0.673832562222562, -0.6404225489719699, -0.3685222338703895, 6.988973505496276,
0, 0, 0, 0.999999999999998;
*/
/*
transformation << 0.9998996846969838, 0.003948215234314986, -0.01360265192291004, 0.05847011404293689,
-0.004032877285312574, 0.9999726343121815, -0.006202138950136233, 0.04528938486109094,
0.01357779229749574, 0.006256374606648019, 0.9998882444218992, 0.02203456132723125,
0, 0, 0, 1;
*/
iSAM.addLoopConstraint(logReader.timestamp, matchTime, transformation);//, cov);
fout3<<transformation(0,0)<<" "<<transformation(0,1)<<" "<<transformation(0,2)<<" "<<transformation(0,3)<<" "<<transformation(1,0)<<" "<<transformation(1,1)<<" "<<transformation(1,2)<<" "<<transformation(1,3)<<" "<<transformation(2,0)<<" "<<transformation(2,1)<<" "<<transformation(2,2)<<" "<<transformation(2,3)<<" "<<transformation(3,0)<<" "<<transformation(3,1)<<" "<<transformation(3,2)<<" "<<transformation(3,3)<<endl;
loopClosureCount++;
}
}
if(loopClosureCount>=1)
{
break;
}
}
/*
for(int i=0; i<loopClosureCount;i++)
{
iSAM.addLoopConstraint(placeRecognition.loopClosureConstraints.at(i)->time1,
placeRecognition.loopClosureConstraints.at(i)->time2,
placeRecognition.loopClosureConstraints.at(i)->constraint);
}*/
std::vector<std::pair<uint64_t, Eigen::Matrix4f> > posesBefore;
iSAM.getCameraPoses(posesBefore);
cout<<"It works good before optimization"<<endl;
// #4
double residual =iSAM.optimise();
cout<<"It works good after optimize and before map.applyPoses"<<endl;
// map.applyPoses(isam);
//cout<<"It works good before *cloud=map.getMap and after map.applyPoses(isam)"<<endl;
/*
pcl::PointCloud<pcl::PointXYZRGB> *cloud = map.getMap();
// Write it back to disk under a different name.
// Another possibility would be "savePCDFileBinary()".
cout<<"before storing the point cloud map"<<endl;
pcl::io::savePCDFileASCII ("outputCloudMap03DVODensity005.pcd", *cloud);
cout << "Saved data points to outputMap.pcd." << std::endl;
cout<<"copy data into octomap..."<<endl;
octomap::ColorOcTree tree( 0.05 );
for (size_t i=0; i<(*cloud).points.size(); i++)
{
// 将点云里的点插入到octomap中
tree.updateNode( octomap::point3d((*cloud).points[i].x, (*cloud).points[i].y, (*cloud).points[i].z), true );
}
for (size_t i=0; i<(*cloud).points.size(); i++)
{
tree.integrateNodeColor( (*cloud).points[i].x, (*cloud).points[i].y, (*cloud).points[i].z, (*cloud).points[i].r, (*cloud).points[i].g, (*cloud).points[i].b);
}
tree.updateInnerOccupancy();
tree.write("OctomapColorLab03DVODensity005.ot");
cout<<"please see the done."<<endl;
*/
//pcl::visualization::PCLVisualizer cloudViewer;
// cloudViewer.setBackgroundColor(1, 1, 1);
//cloudViewer.initCameraParameters();
// cloudViewer.addCoordinateSystem(0.1, 0, 0, 0);
//pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> color(cloud->makeShared());
//cloudViewer.addPointCloud<pcl::PointXYZRGB>(cloud->makeShared(), color, "Cloud Viewer");
std::vector<std::pair<uint64_t, Eigen::Matrix4f> > newPoseGraph;
iSAM.getCameraPoses(newPoseGraph);
/*
for(unsigned int i = 0; i < newPoseGraph.size(); i++)
{
// file << std::setprecision(6) << std::fixed << (double)newPoseGraph.at(i).first / 1000000.0 << " ";
Eigen::Vector3f trans = newPoseGraph.at(i).second.topRightCorner(3, 1);
Eigen::Matrix3f rot = newPoseGraph.at(i).second.topLeftCorner(3, 3);
fout4 << trans(0) << " " << trans(1) << " " << trans(2) << " ";
Eigen::Quaternionf currentCameraRotation(rot);
//file << currentCameraRotation.x() << " " << currentCameraRotation.y() << " " << currentCameraRotation.z() << " " << currentCameraRotation.w() << "\n";
}*/
for(std::vector<std::pair<uint64_t, Eigen::Matrix4f> >::iterator ite=newPoseGraph.begin(); ite!=newPoseGraph.end(); ite++)
{
Eigen::Matrix3f Roptimized;
Roptimized<<ite->second(0,0), ite->second(0,1), ite->second(0,2),
ite->second(1,0), ite->second(1,1), ite->second(1,2),
ite->second(2,0), ite->second(2,1), ite->second(2,2);
Eigen::Quaternionf quatOptimized(Roptimized);
fout4<<ite->second(0,3)<<" "<<ite->second(1,3)<<" "<<ite->second(2,3)<<" "<<quatOptimized.w()<<" "<<quatOptimized.x()<<" "<<quatOptimized.y()<<" "<<quatOptimized.z()<<endl;
}
cout<<"The number of optimized poses"<<newPoseGraph.size()<<endl;
// drawPoses(poses, cloudViewer, 1.0, 0, 0);
//drawPoses(posesBefore, cloudViewer, 0, 0, 1.0);
//cloudViewer.spin();
delete [] decompressionBuffer;
return 0;
}
void identifyClip(QString activeFolder, int clipNum){
qDebug()<<"Clip"<<clipNum<<":"<<activeFolder;
qDebug()<<"===========KNN Ant Identification Program===========\n( Starting with k ="<<K<<"bgSimilarity ="<<bgSimilarity<<"binSize ="<<binSize<<")";
qDebug()<<"Counting ants...";
QString header = "../data/KNNtraining/";
QDir preDir(header);
QStringList fileList = preDir.entryList(QStringList("????"));
int numKnownAnts = fileList.length();
int maxSamplesPerAnt = 0;
QStringList::const_iterator iterator;
for (iterator = fileList.constBegin(); iterator != fileList.constEnd(); ++iterator){
QDir innerDir(header+(*iterator)+"/");
QStringList innerFileList = innerDir.entryList(QStringList("*.png"));
if(innerFileList.length()>maxSamplesPerAnt) maxSamplesPerAnt = innerFileList.length();
}
qDebug()<<"Initializing data structures...";
RgbImage **RGBsamples = new RgbImage*[numKnownAnts];
for(int i=0 ; i<numKnownAnts ; i++){ RGBsamples[i] = new RgbImage[maxSamplesPerAnt]; }
const int numUnknownImages = maxFrames * numParkingSpots;
bool *nullPtr = new bool[numUnknownImages];
UVHistogram *H = new UVHistogram[numKnownAnts*maxSamplesPerAnt];
UVHistogram *unknownH = new UVHistogram[numUnknownImages];
YuvPixel ****YUVsamples = new YuvPixel***[numKnownAnts];
for(int i=0 ; i<numKnownAnts ; i++){
YUVsamples[i] = new YuvPixel**[maxSamplesPerAnt];
for(int j=0 ; j<maxSamplesPerAnt ; j++){
YUVsamples[i][j] = new YuvPixel*[maxImageRows];
for(int k=0; k<maxImageRows; k++){
YUVsamples[i][j][k] = new YuvPixel[maxImageCols];
}
}
}
int *samplesPerAnt = new int[numKnownAnts];
qDebug()<<"Reading training images...";
header = "../data/KNNtraining/";
QDir trainingDir(header);
fileList = trainingDir.entryList(QStringList("????"));
qDebug()<<fileList;
QStringList::const_iterator tIterator;
int antIndex = 0;
for (tIterator = fileList.constBegin(); tIterator != fileList.constEnd(); ++tIterator){
QDir innerDir(header+(*tIterator)+"/");
QStringList innerFileList = innerDir.entryList(QStringList("*.png"));
QStringList::const_iterator innerIterator;
int imageIndex = 0;
for (innerIterator = innerFileList.constBegin(); innerIterator != innerFileList.constEnd(); ++innerIterator){
IplImage* img = 0;
img = cvLoadImage((header+(*tIterator)+"/"+(*innerIterator)).toUtf8().constData());
if(!img){
qDebug()<<"Could not load image file"<<(header+(*tIterator)+"/"+(*innerIterator));
}
else{
RgbImage rgbImg(img);
RGBsamples[antIndex][imageIndex] = rgbImg;
samplesPerAnt[antIndex] = imageIndex+1;
}
imageIndex++;
}
antIndex++;
}
qDebug()<<"Converting to YUV...";
for(int i=1; i<=numKnownAnts; i++){
for(int j=1; j<=samplesPerAnt[i-1]; j++){
for(int r=0; r<RGBsamples[i-1][j-1].height(); r++){
for(int c=0; c<RGBsamples[i-1][j-1].width(); c++){
double Y = 0.299*RGBsamples[i-1][j-1][r][c].r + 0.587*RGBsamples[i-1][j-1][r][c].g + 0.114*RGBsamples[i-1][j-1][r][c].b;
double U = (RGBsamples[i-1][j-1][r][c].b - Y)*0.565;
double V = (RGBsamples[i-1][j-1][r][c].r - Y)*0.713;
YUVsamples[i-1][j-1][r][c].y = Y;
YUVsamples[i-1][j-1][r][c].u = U;
YUVsamples[i-1][j-1][r][c].v = V;
}
}
}
}
qDebug()<<"Building histograms...";
for(int i=1; i<=numKnownAnts; i++){
for(int j=1; j<=samplesPerAnt[i-1]; j++){
H[(i-1)*maxSamplesPerAnt+j-1].agentId = i;
for(int x=0; x<256; x++){
H[(i-1)*maxSamplesPerAnt+j-1].UValues[x] = 0;
H[(i-1)*maxSamplesPerAnt+j-1].VValues[x] = 0;
}
for(int r=0; r<RGBsamples[i-1][j-1].height(); r++){
for(int c=0; c<RGBsamples[i-1][j-1].width(); c++){
if(!(similar(0, YUVsamples[i-1][j-1][r][c].u, bgSimilarity) && similar(0, YUVsamples[i-1][j-1][r][c].v, bgSimilarity))){
H[(i-1)*maxSamplesPerAnt+j-1].UValues[(YUVsamples[i-1][j-1][r][c].u + 128)/binSize]++;
H[(i-1)*maxSamplesPerAnt+j-1].VValues[(YUVsamples[i-1][j-1][r][c].v + 128)/binSize]++;
}
}
}
H[(i-1)*maxSamplesPerAnt+j-1].normalize();
}
for(int j=samplesPerAnt[i-1]+1; j<=maxSamplesPerAnt; j++){
for(int x=0; x<256; x++){
H[(i-1)*maxSamplesPerAnt+j-1].UValues[x] = 0;
H[(i-1)*maxSamplesPerAnt+j-1].VValues[x] = 0;
}
}
}
delete [] RGBsamples;
delete [] YUVsamples;
qDebug()<<"Processing unidentified images...";
header = "/media/8865399a-a349-43cd-8cc0-2b719505efaf/"+activeFolder;
for(int i=0; i<maxFrames; i++){
for(int j=0; j<numParkingSpots; j++){
nullPtr[(i)*numParkingSpots + j ] = true;
unknownH[(i)*numParkingSpots + j ].agentId = -1;
for(int x=0; x<256; x++){
unknownH[(i)*numParkingSpots + j ].UValues[x] = 0;
unknownH[(i)*numParkingSpots + j ].VValues[x] = 0;
}
}
}
QDir unknownDir(header);
fileList = unknownDir.entryList(QStringList("pSpot*"));
QStringList::const_iterator uIterator;
for (uIterator = fileList.constBegin(); uIterator != fileList.constEnd(); ++uIterator){
qDebug()<<" Beginning images in"<<(*uIterator);
QDir innerDir(header+(*uIterator)+"/");
QStringList innerFileList = innerDir.entryList(QStringList("*.png"));
QStringList::const_iterator innerIterator;
for (innerIterator = innerFileList.constBegin(); innerIterator != innerFileList.constEnd(); ++innerIterator){
IplImage* img = 0;
img = cvLoadImage((header+(*uIterator)+"/"+(*innerIterator)).toUtf8().constData());
if(!img){
qDebug()<<"Could not load image file"<<(header+(*uIterator)+"/"+(*innerIterator));
}
else{
RgbImage rgbImg(img);
QString name = (*innerIterator);
name.remove(QString("framenum_"));
name.remove(QString(".png"));
//QStringList parts = name.split("_");
//int i = parts[3].toInt();//frame
//int j = parts[0].toInt();//spot
int i = name.toInt();
QString spotName = (*uIterator);
int j = spotName.remove("pSpot").toInt();
nullPtr[(i)*numParkingSpots + j ] = false;
for(int r=0; r<rgbImg.height(); r++){
for(int c=0; c<rgbImg.width(); c++){
double Y = 0.299*rgbImg[r][c].r + 0.587*rgbImg[r][c].g + 0.114*rgbImg[r][c].b;
double U = (rgbImg[r][c].b - Y)*0.565;
double V = (rgbImg[r][c].r - Y)*0.713;
if(!(similar(0, ((int)U), bgSimilarity) && similar(0, ((int)V), bgSimilarity))){
unknownH[(i)*numParkingSpots + j ].UValues[(((int)U) + 128)/binSize]++;
unknownH[(i)*numParkingSpots + j ].VValues[(((int)V) + 128)/binSize]++;
}
}
}
unknownH[(i)*numParkingSpots + j ].normalize();
}
cvReleaseImage(&img);
}
}
// for(int i=1; i<=maxFrames; i++){
// for(int j=1; j<=numParkingSpots; j++){
// QString name, fileName;
// IplImage* img=0;
// //name = "clipnum_"+QString::number(3)+"_framenum_"+QString::number(i)+"_spotnum_"+QString::number(j)+".png";
// name = parkingSpotNames[j-1]+QString::number(i)+".png";
// fileName = header+name;
// img=cvLoadImage(fileName.toUtf8().constData());
// if(!img){
// nullPtr[(i-1)*numParkingSpots + j - 1] = true;
// }
// else{
// RgbImage rgbImg(img);
// unknowns[(i-1)*numParkingSpots + j - 1] = rgbImg;
// nullPtr[(i-1)*numParkingSpots + j - 1] = false;
// }
// unknownH[(i-1)*numParkingSpots + j - 1].agentId = -1;
// for(int x=0; x<256; x++){
// unknownH[(i-1)*numParkingSpots + j - 1].UValues[x] = 0;
// unknownH[(i-1)*numParkingSpots + j - 1].VValues[x] = 0;
// }
// if(nullPtr[(i-1)*numParkingSpots + j - 1]){
// continue;
// }
// for(int r=0; r<unknowns[(i-1)*numParkingSpots + j - 1].height(); r++){
// for(int c=0; c<unknowns[(i-1)*numParkingSpots + j - 1].width(); c++){
// double Y = 0.299*unknowns[(i-1)*numParkingSpots + j - 1][r][c].r + 0.587*unknowns[(i-1)*numParkingSpots + j - 1][r][c].g + 0.114*unknowns[(i-1)*numParkingSpots + j - 1][r][c].b;
// double U = (unknowns[(i-1)*numParkingSpots + j - 1][r][c].b - Y)*0.565;
// double V = (unknowns[(i-1)*numParkingSpots + j - 1][r][c].r - Y)*0.713;
// if(!(similar(0, ((int)U), bgSimilarity) && similar(0, ((int)V), bgSimilarity))){
// unknownH[(i-1)*numParkingSpots + j - 1].UValues[(((int)U) + 128)/binSize]++;
// unknownH[(i-1)*numParkingSpots + j - 1].VValues[(((int)V) + 128)/binSize]++;
// }
// }
// }
// unknownH[(i-1)*numParkingSpots + j - 1].normalize();
//
// cvReleaseImage(&img);
//
// }
// if(i%1000==0)
// qDebug()<<"( Frame"<<i<<")";
// }
// delete [] unknowns;
header = "../data/"+activeFolder;
QDir dir(header); if (!dir.exists()) dir.mkpath(".");
qDebug()<<"Computing confusion matrix...";
int confHeight = 480, confWidth = 2*confHeight;//, buffer = 2, unknownWidth = (double)(confWidth/(numKnownAnts*maxSamplesPerAnt))*numUnknownImages;
QString name = header+"confusionmat"+QString::number(clipNum)+".png";
//cvNamedWindow("ConfusionMatrix", CV_WINDOW_AUTOSIZE);
IplImage* confImg = cvCreateImage(cvSize(confWidth,confHeight), IPL_DEPTH_8U, 1);
BwImage confMat(confImg);
int totalUnknownSamples = 0;
for(int i=1; i<=numUnknownImages; i++){
if(nullPtr[i-1]){
continue;
}
totalUnknownSamples++;
}
int totalKnownSamples = 0;
for(int i=0; i<numKnownAnts;i++)
totalKnownSamples += samplesPerAnt[i];
int vertStep = max(confHeight/totalKnownSamples, 1);
int horzStep = max((confWidth/2)/totalKnownSamples, 1);
int stepRow = 0;
for(int i=1; i<=numKnownAnts; i++){
for(int j=1; j<=samplesPerAnt[i-1]; j++){
int rowIndex = (i-1)*maxSamplesPerAnt+j-1;
int stepCol = 0;
for(int ii=1; ii<=numKnownAnts; ii++){
for(int jj=1; jj<=samplesPerAnt[ii-1]; jj++){
int colIndex = (ii-1)*maxSamplesPerAnt+jj-1;
for(int k=0; k<=vertStep; k++){
for(int kk=0; kk<=horzStep; kk++){
confMat[min(confHeight,(int)(((double)stepRow/totalKnownSamples)*confHeight+k))]
[min(confWidth/2, (int)(((double)stepCol/totalKnownSamples)*(confWidth/2)+kk))] = 255 * H[rowIndex].intersectionWith(H[colIndex]);
}
}
stepCol++;
}
}
stepCol = 0;
for(int ii=1; ii<=maxFrames; ii++){
for(int jj=1; jj<=numParkingSpots; jj++){
int colIndex = (ii-1)*numParkingSpots + jj - 1;
if(!nullPtr[colIndex]){
for(int k=0; k<=vertStep; k++)
confMat[min(confHeight,(int)(((double)stepRow/totalKnownSamples)*confHeight+k))]
[confWidth/2+(int)(((double)stepCol/totalUnknownSamples)*(confWidth/2))] = 255 * H[rowIndex].intersectionWith(unknownH[colIndex]);
stepCol++;
}
}
}
stepRow++;
}
}
//cvShowImage("ConfusionMatrix", confImg);
cvSaveImage(name.toUtf8().constData(),confImg);
qDebug()<<"Assigning IDs...";
double **hypotheses = new double*[numUnknownImages]; //id and confidence
for(int i=0; i<numUnknownImages; i++) hypotheses[i] = new double[2];
double *averageDistance = new double[numUnknownImages];
for(int i=0; i<numUnknownImages; i++){
if(nullPtr[i]){
continue;
}
//find k nearest neighbors
double nearestK[K][2];//id and confidence
for(int k=0; k<K; k++){
nearestK[k][0] = -1;
nearestK[k][1] = 0;
}
for(int j=0; j<numKnownAnts*maxSamplesPerAnt; j++){
double similarity = unknownH[i].intersectionWith(H[j]);
int furthestNeighbor = 0;
for(int k=1; k<K; k++){
if(nearestK[k][1]<nearestK[furthestNeighbor][1])
furthestNeighbor = k;
}
if(similarity > nearestK[furthestNeighbor][1]){
nearestK[furthestNeighbor][1] = similarity;
nearestK[furthestNeighbor][0] = H[j].agentId;
}
}
//poll the neighbors
int agentVotes[numKnownAnts];
for(int j=0; j<numKnownAnts; j++){agentVotes[j] = 0;}
for(int k=0; k<K; k++){agentVotes[(int)(nearestK[k][0]-1)]++;}
int majorityVote = 0;
//qDebug()<<agentVotes[0];
for(int j=1; j<numKnownAnts; j++){
if(agentVotes[j]>agentVotes[majorityVote])
majorityVote = j;
//qDebug()<<agentVotes[j];
}
//qDebug()<<"--";
hypotheses[i][0] = majorityVote+1;//this 'sometimes zero-indexed, sometimes one-indexed' business is going to bite us later
hypotheses[i][1] = ((double)agentVotes[majorityVote])/K;
averageDistance[i] = 0;
for(int k=0; k<K; k++){
//if((int)(nearestK[k][0]) == majorityVote)
averageDistance[i]+=nearestK[k][1];
}
averageDistance[i] /= K;//((double)agentVotes[majorityVote]);
}
ofstream myFile;
name = header+"results"+QString::number(clipNum)+".csv";
myFile.open(name.toUtf8().constData());
myFile << "Frame Number, Spot Number, ID, Confidence, Similarity, \n";
for(int i=0; i<numUnknownImages; i++){
if(nullPtr[i]){
continue;
}
//qDebug()<<"Image"<<i+1<<"is of agent"<<hypotheses[i][0]<<"("<<hypotheses[i][1]*100<<"% agree at"<<averageDistance[i]<<")";
//if(averageDistance[i]>=0.9){
myFile << ((i/numParkingSpots) + 1) << "," << ((i%numParkingSpots) + 1) << "," << hypotheses[i][0] << "," << hypotheses[i][1] << "," << averageDistance[i] << ", \n";
//}
}
myFile.close();
qDebug()<<"Output saved to"<<name;
delete [] averageDistance;
delete [] samplesPerAnt;
delete [] hypotheses;
delete [] unknownH;
delete [] nullPtr;
delete [] H;
qDebug()<<"=====================Clean Exit=====================";
}
