void SLTrackerDialog::on_startStopPushButton_clicked(){
if(!tracking){
// Prepare trackerWorker on separate thread
trackerWorker = new SLTrackerWorker();
trackerWorkerThread = new QThread(this);
trackerWorkerThread->setObjectName("trackerWorkerThread");
trackerWorker->moveToThread(trackerWorkerThread);
trackerWorkerThread->start(QThread::LowPriority);
QMetaObject::invokeMethod(trackerWorker, "setup");
qRegisterMetaType< Eigen::Affine3f >("Eigen::Affine3f");
connect(this, SIGNAL(newPointCloud(PointCloudConstPtr)), trackerWorker, SLOT(trackPointCloud(PointCloudConstPtr)));
// connect(trackerWorker, SIGNAL(newPoseEstimate(Eigen::Affine3f)), ui->poseWidget, SLOT(showPoseEstimate(Eigen::Affine3f)));
connect(trackerWorker, SIGNAL(newPoseEstimate(Eigen::Affine3f)), this, SLOT(showPoseEstimate(Eigen::Affine3f)));
connect(trackerWorkerThread, SIGNAL(finished()), trackerWorker, SLOT(deleteLater()));
tracking = true;
ui->startStopPushButton->setText("Stop Tracking");
} else {
// Terminate tracker worker thread
if(trackerWorkerThread && trackerWorkerThread->isRunning()){
trackerWorkerThread->quit();
trackerWorkerThread->wait();
}
disconnect(trackerWorker);
tracking = false;
ui->startStopPushButton->setText("Start Tracking");
}
}
void SLTriangulatorWorker::triangulatePointCloud(cv::Mat up, cv::Mat vp, cv::Mat mask, cv::Mat shading){
// Recursively call self until latest event is hit
busy = true;
QCoreApplication::sendPostedEvents(this, QEvent::MetaCall);
bool result = busy;
busy = false;
if(!result){
std::cerr << "SLTriangulatorWorker: dropped phase image!" << std::endl;
return;
}
time.restart();
// Reconstruct point cloud
cv::Mat pointCloud;
triangulator->triangulate(up, vp, mask, shading, pointCloud);
// Convert point cloud to PCL format
PointCloudPtr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGB>);
// Interprete as organized point cloud
pointCloudPCL->width = pointCloud.cols;
pointCloudPCL->height = pointCloud.rows;
pointCloudPCL->is_dense = false;
pointCloudPCL->points.resize(pointCloud.rows*pointCloud.cols);
for(int row=0; row<pointCloud.rows; row++){
int offset = row * pointCloudPCL->width;
for(int col=0; col<pointCloud.cols; col++){
const cv::Vec3f pnt = pointCloud.at<cv::Vec3f>(row,col);
unsigned char shade = shading.at<unsigned char>(row,col);
pcl::PointXYZRGB point;
point.x = pnt[0]; point.y = pnt[1]; point.z = pnt[2];
point.r = shade; point.g = shade; point.b = shade;
pointCloudPCL->points[offset + col] = point;
}
}
// std::vector<cv::Mat> xyz;
// cv::split(pointCloud, xyz);
// // stack xyz data
// std::vector<cv::Mat> pointCloudChannels;
// pointCloudChannels.push_back(xyz[0]);
// pointCloudChannels.push_back(xyz[1]);
// pointCloudChannels.push_back(xyz[2]);
// // 4 byte padding
// pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));
// // triple uchar color information
// std::vector<cv::Mat> rgb;
// rgb.push_back(shading);
// rgb.push_back(shading);
// rgb.push_back(shading);
// rgb.push_back(cv::Mat::zeros(shading.size(), CV_8U));
// cv::Mat rgb8UC4;
// cv::merge(rgb, rgb8UC4);
// cv::Mat rgb32F(rgb8UC4.size(), CV_32F, rgb8UC4.data);
// pointCloudChannels.push_back(rgb32F);
// // 12 bytes padding
// pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));
// pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));
// pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));
// // merge channels
// cv::Mat pointCloudPadded;
// cv::merge(pointCloudChannels, pointCloudPadded);
// // memcpy everything
// memcpy(&pointCloudPCL->points[0], pointCloudPadded.data, pointCloudPadded.rows*pointCloudPadded.cols*sizeof(pcl::PointXYZRGB));
// // filtering
// pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> filter;
// filter.setMeanK(5);
// filter.setStddevMulThresh(1.0);
// filter.setInputCloud(pointCloudPCL);
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudFiltered(new pcl::PointCloud<pcl::PointXYZRGB>);
// filter.filter(*pointCloudFiltered);
// Emit result
emit newPointCloud(pointCloudPCL, cNum);
std::cout << "Triangulator: " << time.elapsed() << "ms" << std::endl;
if(writeToDisk){
QString fileName = QString("acam_%1").arg(cNum,1);// = QDateTime::currentDateTime().toString("yyyyMMdd_HHmmsszzz");
fileName.append(".ply");
//pcl::io::savePCDFileBinary(fileName.toStdString(), *pointCloudPCL);
pcl::PLYWriter w;
// Write to ply in binary without camera
w.write<pcl::PointXYZRGB> (fileName.toStdString(), *pointCloudPCL, true, false);
std::cout << "Save PLY: " << fileName.toStdString() << std::endl;
}
emit finished();
}
void SLTrackerDialog::receiveNewPointCloud(PointCloudConstPtr pointCloud){
emit newPointCloud(pointCloud);
}
