FORCEINLINE void Stop()
{
if (m_pperf != NULL) {
m_pperf->Stop();
m_pperf = NULL;
}
}
shared_ptr<Frame> FrameLoggerTextBz2::readLog(
Pave_Libraries_Common::StateEstimationType *state)
{
shared_ptr<Frame> frm(new Frame());
// Clunky EOF checking but it works.
if(printDebug) cout << "Reading at: " << manifest.tellg() << endl;
manifest >> frm->framenum;
if (!manifest.good()) return shared_ptr<Frame>();
manifest >> frm->name;
manifest >> frm->focalLength;
int h, w;
manifest >> h >> w;
frm->setSize(w, h);
Pave_Libraries_Common::StateEstimationType temp;
if (!state) state = &temp;
manifest >> state->Northing;
manifest >> state->Easting;
manifest >> state->Heading;
manifest >> state->Speed;
string basename = getDirectory() + "/" + frm->name;
string colorname = basename + "-color.png";
cv::Mat_<cv::Vec3b> colorTemp = cv::imread(colorname.c_str(), 1);
frm->color = colorTemp;
ifstream valid((basename + "-valid").c_str());
boost::iostreams::filtering_istream tcStream;
tcStream.push(boost::iostreams::bzip2_decompressor());
tcStream.push(boost::iostreams::file_source(basename + "-transformed-txt.bz2", std::ios::binary));
valid.read((char *)frm->validArrPtr, frm->height * frm->width * sizeof(bool));
valid.close();
MatIterator_<Point3f> it = frm->transformedCloud.begin();
MatIterator_<Point3f> itEnd = frm->transformedCloud.end();
if(printDebug) cout << "Transformed read started." << endl;
CPerformanceTimer timer;
timer.Start();
for (; it != itEnd; it++)
{
Point3f &pt = *it;
tcStream >> pt.x >> pt.y >> pt.z;
}
timer.Stop();
if(printDebug) std::cerr << "decompressing read time (ms): " << timer.Interval_mS() << std::endl;
if(printDebug) cout << "Transformed read ended." << endl;
return frm;
}
void FrameLoggerTextBz2::log(shared_ptr<Pave_Libraries_Camera::Frame> frm,
Pave_Libraries_Common::StateEstimationType *state)
{
manifest << frm->framenum << "\t";
manifest << frm->name << "\t";
manifest << frm->focalLength << "\t";
manifest << frm->height << "\t";
manifest << frm->width << "\t";
if (state) {
manifest << state->Northing << "\t";
manifest << state->Easting << "\t";
manifest << state->Heading << "\t";
manifest << state->Speed << endl;
} else {
manifest << "0\t0\t0\t0" << endl;
}
string basename = getDirectory();
basename += "/";
basename += frm->name;
cv::imwrite(basename + "-color.png", frm->color);
ofstream valid((basename + "-valid").c_str());
valid.write((char *)frm->validArrPtr, frm->height * frm->width * sizeof(bool));
valid.close();
MatConstIterator_<Point3f> it = frm->transformedCloud.begin();
MatConstIterator_<Point3f> itEnd = frm->transformedCloud.end();
std::stringstream trText;
for (; it != itEnd; it++)
{
const Point3f &pt = *it;
trText << pt.x << "\t" << pt.y << "\t" << pt.z << "\n";
}
boost::iostreams::filtering_ostream tcStream;
tcStream.push(boost::iostreams::bzip2_compressor());
tcStream.push(boost::iostreams::file_sink(basename + "-transformed-txt.bz2", std::ios::binary));
CPerformanceTimer timer;
timer.Start();
tcStream << trText.rdbuf();
timer.Stop();
if(printDebug) std::cerr << "compressed write time (ms): " << timer.Interval_mS() << std::endl;
}
bool Manduchi::process(shared_ptr<Frame> frm)
{
if (frm->width != IMAGE_WIDTH || frm->height != IMAGE_HEIGHT) {
cout << "Manduchi not configured for "
<< frm->width << "x" << frm->height
<< " frames! No obstacles detected!" << endl;
frm->color.copyTo(overlay_); // otherwise it'd be uninitialized
return false;
}
#ifdef DO_TIMING
CPerformanceTimer timer;
timer.Start();
#endif
// copy OpenCV matrix to array of floats for faster access
float *cloud = new float[frm->width*frm->height*3];
cv::MatIterator_<cv::Point3f> it = frm->transformedCloud.begin();
cv::MatIterator_<cv::Point3f> itEnd = frm->transformedCloud.end();
for (int i = 0; it != itEnd; it++, i += 3) {
float *pt = cloud + i;
pt[0] = (*it).x;
pt[1] = (*it).y;
pt[2] = (*it).z;
}
// Setup the overlay image
frm->color.copyTo(overlay_);
focalLength = (float)frm->focalLength;
// create tiles of type (0,0)
for (int i = 0; i < 16; ++i) {
for (int j = 0; j < 6; ++j) {
int r = j * (TILE_HEIGHT + 1);
float *tile = depthTileA[i*6+j];
for (int yi = 0; yi < TILE_HEIGHT; ++yi, ++r) {
int c = i * TILE_WIDTH;
for (int xi = 0; xi < TILE_WIDTH; ++xi, ++c) {
int cloudIdx = (r * IMAGE_WIDTH + c)*3;
int tileIdx = (yi * TILE_WIDTH + xi)*4;
tile[tileIdx] = cloud[cloudIdx];
tile[tileIdx+1] = cloud[cloudIdx+1];
tile[tileIdx+2] = cloud[cloudIdx+2];
if (frm->validArr[r][c])
tile[tileIdx+3] = 0.5f; //arbitrary indicator
else
tile[tileIdx+3] = -1.0f; //invalid
}
}
}
}
// create tiles of type (1, 0)
for (int i = 0; i < 15; ++i) {
for (int j = 0; j < 6; ++j) {
float *tile = depthTileB[i*6+j];
int r = j * (TILE_HEIGHT + 1);
for (int yi = 0; yi < TILE_HEIGHT; ++yi, ++r) {
int c = i * TILE_WIDTH + (TILE_WIDTH / 2);
for (int xi = 0; xi < TILE_WIDTH; ++xi, ++c) {
int cloudIdx = (r * IMAGE_WIDTH + c) * 3;
int tileIdx = (yi * TILE_WIDTH + xi) * 4; //the last float stores isValid
tile[tileIdx] = cloud[cloudIdx];
tile[tileIdx+1] = cloud[cloudIdx+1];
tile[tileIdx+2] = cloud[cloudIdx+2];
if (frm->validArr[r][c])
tile[tileIdx+3] = 0.5f; //arbitrary indicator
else
tile[tileIdx+3] = -1.0f; //invalid
}
}
}
}
// create tiles of type (0,1)
for (int i = 0; i < 16; ++i) {
for (int j = 0; j < 5; ++j) {
float *tile = depthTileC[i*5+j];
int r = j * (TILE_HEIGHT + 1) + (TILE_HEIGHT + 1) / 2;
for (int yi = 0; yi < TILE_HEIGHT; ++yi, ++r) {
int c = i * TILE_WIDTH;
for (int xi = 0; xi < TILE_WIDTH; ++xi, ++c) {
int cloudIdx = (r * IMAGE_WIDTH + c) * 3;
int tileIdx = (yi * TILE_WIDTH + xi) * 4;
tile[tileIdx] = cloud[cloudIdx];
tile[tileIdx+1] = cloud[cloudIdx+1];
tile[tileIdx+2] = cloud[cloudIdx+2];
if (frm->validArr[r][c])
tile[tileIdx+3] = 0.5f; //arbitrary indicator
else
tile[tileIdx+3] = -1.0f; //invalid
}
}
}
}
// create tiles of type (1, 1)
for (int i = 0; i < 15; ++i) {
for (int j = 0; j < 5; ++j) {
int r = j * (TILE_HEIGHT + 1) + (TILE_HEIGHT + 1) / 2;
float *tile = depthTileD[i*5+j];
for (int yi = 0; yi < TILE_HEIGHT; ++yi, ++r) {
int c = i * TILE_WIDTH + (TILE_WIDTH / 2);
for (int xi = 0; xi < TILE_WIDTH; ++xi, ++c) {
int cloudIdx = (r * IMAGE_WIDTH + c)*3;
int tileIdx = (yi*TILE_WIDTH+xi)*4;
tile[tileIdx] = cloud[cloudIdx];
tile[tileIdx+1] = cloud[cloudIdx+1];
tile[tileIdx+2] = cloud[cloudIdx+2];
if (frm->validArr[r][c])
tile[tileIdx+3] = 0.5f; //arbitrary indicator
else
tile[tileIdx+3] = -1.0f; //invalid
}
}
}
}
#ifdef DO_TIMING
timer.Stop();
cout << "Time (pre-processing): " << timer.Interval_mS() << "ms" << endl;
timer.Reset();
timer.Start();
#endif
#pragma omp parallel num_threads(NUMBER_OF_THREADS)
{
#pragma omp for nowait schedule(dynamic, 2)
for (int i = 0; i < 16*6; i+=1) {
blockACProcess(depthTileA[i]);
}
#pragma omp for nowait schedule(dynamic, 2)
for (int i = 0; i < 15*6; ++i) {
blockBDProcess(depthTileB[i]);
}
#pragma omp for nowait schedule(dynamic, 2)
for (int i = 0; i < 16*5; ++i) {
blockACProcess(depthTileC[i]);
}
#pragma omp for nowait schedule(dynamic, 2)
for (int i = 0; i < 15*5; ++i) {
blockBDProcess(depthTileD[i]);
}
}
#ifdef DO_TIMING
timer.Stop();
cout << "Time (actual algorithm): " << timer.Interval_mS() << "ms" << endl;
timer.Reset();
timer.Start();
#endif
// type (0,0)
for (int i = 0; i < 16; ++i) {
for (int j = 0; j < 6; ++j) {
float *tile = depthTileA[i*6+j];
int r = j * (TILE_HEIGHT + 1);
for (int yi = 0; yi < TILE_HEIGHT; ++yi, ++r) {
int c = i * TILE_WIDTH;
for (int xi = 0; xi < TILE_WIDTH; ++xi, ++c) {
if (tile[(yi*TILE_WIDTH+xi)*4+3] > 1.0f + MIN_COMPATIBLE_POINTS_COUNT) { //1.0 comes from the fact that 0.5 means no compatible points found
frm->obstacle[r][c] = UCHAR_SAT;
// overlay_[r][c] = cv::Vec3b(255,0,0);
} else if (tile[(yi*TILE_WIDTH+xi)*4+3] < 0) {
//overlay_[r][c] = cv::Vec3b(128, 128, 128);
}
}
}
}
}
// type (1, 0)
for (int i = 0; i < 15; ++i) {
for (int j = 0; j < 6; ++j) {
float *tile = depthTileB[i*6+j];
int r = j * (TILE_HEIGHT + 1);
for (int yi = 0; yi < TILE_HEIGHT; ++yi, ++r) {
int c = i * TILE_WIDTH + (TILE_WIDTH / 2);
for (int xi = 0; xi < TILE_WIDTH; ++xi, ++c) {
if (tile[(yi*TILE_WIDTH+xi)*4+3] > 1.0f + MIN_COMPATIBLE_POINTS_COUNT) { //1.0 comes from the fact that 0.5 means no compatible points found
frm->obstacle[r][c] = UCHAR_SAT;
// overlay_[r][c] = cv::Vec3b(255,0,0);
} else if (tile[(yi*TILE_WIDTH+xi)*4+3] < 0) {
//overlay_[r][c] = cv::Vec3b(128, 128, 128);
}
}
}
}
}
// type (0,1)
for (int i = 0; i < 16; ++i) {
for (int j = 0; j < 5; ++j) {
float *tile = depthTileC[i*5+j];
int r = j * (TILE_HEIGHT + 1) + (TILE_HEIGHT + 1) / 2;
for (int yi = 0; yi < TILE_HEIGHT; ++yi, ++r) {
int c = i * TILE_WIDTH;
for (int xi = 0; xi < TILE_WIDTH; ++xi, ++c) {
if (tile[(yi*TILE_WIDTH+xi)*4+3] > 1.0f + MIN_COMPATIBLE_POINTS_COUNT) { //1.0 comes from the fact that 0.5 means no compatible points found
frm->obstacle[r][c] = UCHAR_SAT;
// overlay_[r][c] = cv::Vec3b(255,0,0);
} else if (tile[(yi*TILE_WIDTH+xi)*4+3] < 0) {
//overlay_[r][c] = cv::Vec3b(128, 128, 128);
}
}
}
}
}
// type (1, 1)
for (int i = 0; i < 15; ++i) {
for (int j = 0; j < 5; ++j) {
float *tile = depthTileD[i*5+j];
int r = j * (TILE_HEIGHT + 1) + (TILE_HEIGHT + 1) / 2;
for (int yi = 0; yi < TILE_HEIGHT; ++yi, ++r) {
int c = i * TILE_WIDTH + (TILE_WIDTH / 2);
for (int xi = 0; xi < TILE_WIDTH; ++xi, ++c) {
if (tile[(yi*TILE_WIDTH+xi)*4+3] > 1.0f + MIN_COMPATIBLE_POINTS_COUNT) { //1.0 comes from the fact that 0.5 means no compatible points found
frm->obstacle[r][c] = UCHAR_SAT;
// overlay_[r][c] = cv::Vec3b(255,0,0);
} else if (tile[(yi*TILE_WIDTH+xi)*4+3] < 0) {
//overlay_[r][c] = cv::Vec3b(128, 128, 128);
}
}
}
}
}
//cv::imshow("wtf", overlay_);
//cv::waitKey();
////for testing the simple bar detector
//bool result[IMAGE_WIDTH*IMAGE_HEIGHT];
//for (int i = 0; i < IMAGE_WIDTH*IMAGE_HEIGHT; ++i)
// result[i] = false;
//simpleRailingDetector(frm->transformedCloud, frm->validArr[0], result);
//for (int r = 0; r < IMAGE_HEIGHT; ++r) {
// for (int c = 0; c < IMAGE_WIDTH; ++c) {
// if (result[r*IMAGE_WIDTH+c]) {
// frm->obstacle[r][c] = UCHAR_SAT;
// overlay_[r][c] = cv::Vec3b(255,0,0);
// }
// }
//}
#ifdef DO_TIMING
timer.Stop();
cout << "Time (assign overlay): " << timer.Interval_mS() << "ms" << endl;
timer.Reset();
timer.Start();
#endif
// Obstacle clustering
// clustering using connected components
findConnectedComponent(frm->obstacle[0], labelMap, cloud);
// Process the clusters
clusteredObstacles.resize(0);
int indexCount = 0;
// draw obstacles with different colors
// THE LOOP ORDER HERE IS IMPORTANT! It may not look right but it is
// necessary to push the points into the cluster vectors in column order,
// not row order (to efficiently compute average slope)
for (int c = 0; c < IMAGE_WIDTH; ++c) {
for (int r = 0; r < IMAGE_HEIGHT; ++r) {
int label = labelMap[r*IMAGE_WIDTH+c];
if (label == 0 && frm->validArr[r][c] == false) {
//Draw non-valid pixels as black
//overlay_[r][c] = cv::Vec3b(0, 0, 0);
} else if (label > 0) {
//the indices of relabelList are the non-consecutive labels outputted by the connect-components
//labeling algorithm. it's values are new, consecutive labels. This is so we don't need to use
//std::map, which is a lot slower
int newLabel = relabelList[label];
if (newLabel < 0) {
newLabel = indexCount++;
relabelList[label] = newLabel;
clusteredObstacles.resize(newLabel+1);
}
clusteredObstacles[newLabel].push_back(cv::Point2i(r, c));
//switch (label % 6) {
// case 0: overlay_[r][c] = cv::Vec3b(255, 0, 0); break;
// case 1: overlay_[r][c] = cv::Vec3b(255, 255, 0); break;
// case 2: overlay_[r][c] = cv::Vec3b(255, 0, 255); break;
// case 3: overlay_[r][c] = cv::Vec3b(0, 255, 0); break;
// case 4: overlay_[r][c] = cv::Vec3b(0, 255, 255); break;
// case 5: overlay_[r][c] = cv::Vec3b(0, 0, 255); break;
// default: break;
//}
}
//else
// overlay_[r][c] = cv::Vec3b(0, 0, 0);
}
}
std::fill(relabelList.begin(), relabelList.end(), -1);
frm->setObstacle(0);
// Threshold on average slope and size
vector<vector<cv::Point2i>>::iterator clusterIt;
for (clusterIt = clusteredObstacles.begin(); clusterIt != clusteredObstacles.end(); ++clusterIt) {
float totalDistance = 0;
float netHeightDifference = 0;
int topPixelRow = -1;
int bottomPixelRow = IMAGE_HEIGHT;
vector<cv::Point2i>::iterator it = clusterIt->begin();
int colCount = 1;
int currentCol = it->y;
for (; it != clusterIt->end(); ++it) {
if (currentCol != it->y) {
netHeightDifference +=
(cloud[(topPixelRow*IMAGE_WIDTH+currentCol)*3+2] - cloud[(bottomPixelRow*IMAGE_WIDTH+currentCol)*3+2]);
currentCol = it->y;
colCount++;
topPixelRow = -1;
bottomPixelRow = IMAGE_HEIGHT;
}
if (it->x > topPixelRow)
topPixelRow = it->x;
if (it->x < bottomPixelRow)
bottomPixelRow = it->x;
totalDistance += cloud[((it->x)*IMAGE_WIDTH+currentCol)*3+1];
}
float avgSlope = netHeightDifference/float(colCount);
float avgDistance = totalDistance / float(clusterIt->size());
//cout << "Cluster Size: " << setw(5) << clusterIt->size()
// << " Avg height: " << setw(10) << avgSlope << " Avg dist: " << avgDistance << endl;
// now do the actual thresholding on slope
if ((avgDistance > 18.0 && fabs(avgSlope) < 0.4) ||
(avgDistance > 12.0 && fabs(avgSlope) < 0.3) ||
(avgDistance > 8.0 && fabs(avgSlope) < 0.2) ||
fabs(avgSlope) < 0.1) {
continue;
}
// do thresholding on cluster size
if (clusterIt->size() < CLUSTER_THRESHOLD_SIZE)
continue;
for (it = clusterIt->begin(); it != clusterIt->end(); ++it) {
overlay_[(*it).x][(*it).y] = cv::Vec3b(255, 0, 0);
frm->obstacle[(*it).x][(*it).y] = UCHAR_SAT;
}
}
//cv::imshow("wtf", overlay_);
//cv::waitKey();
#ifdef DO_TIMING
timer.Stop();
cout << "Time (clustering): " << timer.Interval_mS() << "ms" << endl;
#endif
delete[] cloud;
return true;
}