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====================="; }