AppController::AppController() :
lastTick_(0), frameCounter_(0), frameRate_(0) {
model_ = new AppModel();
gui_ = new MainWindow(model_, this);
// register data update functions to model
boost::function<void()> data_cb = boost::bind(&AppController::dataUpdated,
this);
update_con_ = model_->registerCallback(data_cb);
// register pointpick callback of model to gui
boost::function<void(const pcl::visualization::PointPickingEvent &)> pointpick_cb =
boost::bind(&AppModel::pickPoint, model_, _1);
gui_->registerPointPickCallback(pointpick_cb);
// connect signal / slots to gui
connect(this, SIGNAL(newCloud()), gui_, SLOT(updatePointCloud()));
connect(this, SIGNAL(newSmallCloud(void)), gui_,
SLOT(updateSmallPointCloud(void)));
qRegisterMetaType<QImage>("QImage");
connect(this, SIGNAL(newRgbImage(QImage)), gui_,
SLOT(updateRgbImage(QImage)));
qRegisterMetaType<QString>("QString");
connect(this, SIGNAL(newStatusMessage(QString)), gui_,
SLOT(updateStatusMessage(QString)));
gui_->show();
}
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudXYZtoRGBA(
pcl::PointCloud<pcl::PointXYZ>::ConstPtr inCloud)
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr newCloud
(new pcl::PointCloud<pcl::PointXYZRGBA>);
int numPoints = inCloud->points.size();
newCloud->resize(numPoints);
for (int i; i<numPoints; i++) {
pcl::PointXYZRGBA &np = newCloud->at(i);
const pcl::PointXYZ &op = inCloud->at(i);
np.x = op.x;
np.y = op.y;
np.z = op.z;
np.r = 1.0;
np.g = 1.0;
np.b = 1.0;
np.a = 0.0;
}
return newCloud;
}
PCPtr getCloudFromIndices(const PCPtr& cloud, const pcl::PointIndices& pi)
{
PCPtr newCloud(new PC());
for (vector<int>::const_iterator pit = pi.indices.begin(); pit != pi.indices.end(); pit++)
newCloud->points.push_back(cloud->points[*pit]);
return newCloud;
}
void KITTI::getPointCloud(pcl::PointCloud<pcl::PointXYZ> & msg, int i)
{
getVeloPC(msg,velpoints[i]);
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr source (new pcl::PointCloud<pcl::PointXYZ>);
*source = msg;
pcl::VoxelGrid<pcl::PointXYZ> sor;
sor.setInputCloud (source);
sor.setLeafSize (0.4f, 0.4f, 0.4f);
sor.filter (*newCloud);
msg = *newCloud;
}
Video randomGaussJittered(const Video & set, double meanx, double stdx, double meany, double stdy){
std::random_device rd;
std::mt19937 gen(rd());
std::normal_distribution<> dx(meanx,stdx);
std::normal_distribution<> dy(meany,stdy);
Video res;
res.reserve(set.size());
for(auto && cloud: set){
PointCloud<66> newCloud(cloud);
PointArray<66> & points = newCloud.points();
for (cv::Point2f & p: points){
p.x += dx(gen);
p.y += dy(gen);
}
res.emplace_back(std::move(newCloud));
}
return res;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudRGBAtoXYZ(
pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr inCloud)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud
(new pcl::PointCloud<pcl::PointXYZ>);
int numPoints = inCloud->points.size();
newCloud->resize(numPoints);
for (int i; i<numPoints; i++) {
pcl::PointXYZ &np = newCloud->at(i);
const pcl::PointXYZRGBA &op = inCloud->at(i);
np.x = op.x;
np.y = op.y;
np.z = op.z;
}
return newCloud;
}
void AppController::dataUpdated() {
++frameCounter_;
if (frameCounter_ == 10) {
double current_tick = cv::getTickCount();
frameRate_ = frameCounter_
/ ((current_tick - lastTick_) / cv::getTickFrequency());
lastTick_ = current_tick;
frameCounter_ = 0;
}
emit newSmallCloud();
emit newCloud();
QImage * image = model_->getLastImage();
if (!image->isNull()) {
QImage sImage = image->scaled(400, 300);
emit newRgbImage(sImage);
}
QString status = QString("Final fps = %1 Grabber Time = %2 ms").arg(frameRate_,
0, 'f', 1).arg(model_->getGrabberTime(), 0, 'f', 1);
emit newStatusMessage(status);
}
CloudMapper::Clouds CloudMapper::AlignAll(CloudMapper::Clouds &clouds, CloudMapper::LandmarksVector &landmarksVector, TransformationsPtr transformations)
{
TransformationsPtr allTransformations(new Transformations);
if (transformations != nullptr)
{
allTransformations = transformations;
}
if (clouds.size() != landmarksVector.size())
{
throw HPEException("CloudMapper::AlignAll - clouds.size() != landmarksVector.size()");
}
std::vector<Eigen::Matrix4f> stepTransformations;
pcl::PointCloud<CloudMapper::PointType>::Ptr sourceCloud = clouds[0];
for (int cloudIndex = 1; cloudIndex < clouds.size() - 1; cloudIndex++)
{
int sourceIndex = cloudIndex;
int targetIndex = sourceIndex + 1;
pcl::PointCloud<CloudMapper::PointType>::Ptr targetCloud = clouds[targetIndex];
auto totalTransform = GetTransformHavingLandmarks(sourceCloud, landmarksVector[sourceIndex],
targetCloud, landmarksVector[targetIndex]);
Eigen::Matrix3f rotationMatrix = totalTransform.block<3, 3>(0, 0);
Eigen::Vector3f translationVector = totalTransform.block<3, 1>(0, 3);
Eigen::Quaternionf quaternion(rotationMatrix);
Eigen::Matrix3f rotation2(quaternion);
pcl::transformPointCloud(*sourceCloud, *sourceCloud, totalTransform);
pcl::PointCloud<CloudMapper::PointType>::Ptr newCloud(new pcl::PointCloud<CloudMapper::PointType>);
*sourceCloud += *targetCloud;
sourceCloud = Voxelize<CloudMapper::PointType>(sourceCloud, 0.006f);
//ShowCloud<CloudMapper::PointType>(sourceCloud);
if (IsNotNAN(totalTransform) == false)
{
int q = 42;
}
std::cout << cloudIndex << std::endl;
stepTransformations.push_back(totalTransform);
}
ShowCloud<CloudMapper::PointType>(sourceCloud);
CloudMapper::Clouds result;
auto targetCloud = clouds[clouds.size() - 1];
for (int cloudIndex = 0; cloudIndex < clouds.size() - 1; cloudIndex++)
{
Eigen::Matrix4f accumulatedMatrix = stepTransformations[cloudIndex];
for (int matrixIndex = cloudIndex + 1; matrixIndex < stepTransformations.size(); matrixIndex++)
{
accumulatedMatrix = accumulatedMatrix * stepTransformations[matrixIndex];
}
CloudMapper::Cloud::Ptr transformed(new CloudMapper::Cloud);
pcl::transformPointCloud(*clouds[cloudIndex], *transformed, accumulatedMatrix);
allTransformations->push_back(accumulatedMatrix);
result.push_back(transformed);
}
result.push_back(targetCloud);
return result;
}