void MultiClientWindow::on_pushButton_RegisterLocally_clicked()
{
int i_target = ui->spinBox_PC_Target->value();
int i_input = ui->spinBox_PC_Input->value();
PointCloudT::Ptr PC_target(new PointCloudT);
PointCloudT::Ptr PC_input(new PointCloudT);
PC_target = getPointCloud(i_target);
PC_input = getPointCloud(i_input);
PointCloudT::Ptr PCnew(new PointCloudT);
PCnew = getPointCloud(i_input);
Transform currentPose = Transform(PCnew->sensor_origin_, PCnew->sensor_orientation_);
Transform T = utils::getTransformation(PC_target, PC_input);
T.print();
T = T.postmultiplyby(currentPose);
T.print();
setPointCloudPose(i_input,T);
}
template<typename PointT> typename pcl::registration::LUM<PointT>::PointCloudPtr
pcl::registration::LUM<PointT>::getTransformedCloud (Vertex vertex)
{
PointCloudPtr out (new PointCloud);
pcl::transformPointCloud (*getPointCloud (vertex), *out, getTransformation (vertex));
return (out);
}
void DepthFrame::getRegisteredPointCloud(pcl::PointCloud<pcl::PointXYZ>& cloud)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudTmp (new pcl::PointCloud<pcl::PointXYZ>);
getPointCloud(*pCloudTmp);
pcl::transformPointCloud(*pCloudTmp, cloud, m_T);
}
void PointsFileSystem::updatePointCloud(int frame)
{
std::string filename = getPointsFilename(frame);
if (point_cloud_cache_map_.find(filename) == point_cloud_cache_map_.end())
return;
getPointCloud(frame)->reload();
return;
}
void OctreeGenerator::buildOctree() {
// double voxelSize = 0.01f;
// octree = new OctreeWithSIFT(voxelSize);
pcl::PointCloud<PointXYZSIFT>::Ptr cloud = getPointCloud();
octree = new Octree(cloud);
octree->init();
// octree -> setInputCloud(cloud);
// octree -> defineBoundingBox();
// octree -> addPointsFromInputCloud();
}
void PointsFileSystem::hideAndShowPointCloud(int hide_frame, int show_frame)
{
bool to_hide = true;
bool to_show = true;
osg::ref_ptr<PointCloud> show_cloud = getPointCloud(show_frame);
osg::ref_ptr<PointCloud> hide_cloud = getPointCloud(hide_frame);
QModelIndex show_index = this->index(QString(getPointsFilename(show_frame).c_str()));
checked_indexes_.insert(show_index);
PointCloudMap::iterator point_cloud_map_it = point_cloud_map_.find(show_index);
if (point_cloud_map_it == point_cloud_map_.end())
{
if (show_cloud != NULL)
point_cloud_map_[show_index] = show_cloud;
else
to_show = false;
}
else
to_show = false;
QModelIndex hide_index = this->index(QString(getPointsFilename(hide_frame).c_str()));
checked_indexes_.remove(hide_index);
point_cloud_map_it = point_cloud_map_.find(hide_index);
if (point_cloud_map_it != point_cloud_map_.end())
point_cloud_map_.erase(point_cloud_map_it);
else
to_hide = false;
SceneWidget* scene_widget = MainWindow::getInstance()->getSceneWidget();
if (to_hide && to_show)
scene_widget->replaceSceneChild(hide_cloud, show_cloud);
else if (to_hide)
scene_widget->removeSceneChild(getPointCloud(hide_frame));
else if (to_show)
scene_widget->addSceneChild(getPointCloud(show_frame));
return;
}
template<typename PointT> typename pcl::registration::LUM<PointT>::PointCloudPtr
pcl::registration::LUM<PointT>::getConcatenatedCloud ()
{
PointCloudPtr out (new PointCloud);
typename SLAMGraph::vertex_iterator v, v_end;
for (boost::tuples::tie (v, v_end) = vertices (*slam_graph_); v != v_end; ++v)
{
PointCloud temp;
pcl::transformPointCloud (*getPointCloud (*v), temp, getTransformation (*v));
*out += temp;
}
return (out);
}
void PointsFileSystem::savePointCloudAsPly(int frame)
{
std::string folder = getPointsFolder(frame);
if (folder.empty())
return;
PointCloud* point_cloud = getPointCloud(frame);
//point_cloud->reEstimateNormal();
std::string save_ply = folder + "/points.ply";
pcl::io::savePLYFileASCII (save_ply, *point_cloud);
}
void ModelMaker::readData(int fileIndex, bool isSource){
char dFilePath[30], iFilePath[30];
sprintf(dFilePath, "data\\depthraw\\%d_raw.txt", fileIndex);
sprintf(iFilePath, "data\\color\\%d_color.png", fileIndex);
if(isSource){
cout << "Reading Source Image "<< fileIndex << "..." ;
readDepthImage(srcD, dFilePath);
readColorImage(srcI, iFilePath);
cout << " Source Point Cloud..." ;
srcPointCloud.clear();
srcColorCloud.clear();
getPointCloud(srcD, srcPointCloud, srcI, srcColorCloud, srcCenter);
}else{
cout << "Reading Destination Image "<< fileIndex << "..." ;
dstPointCloud.clear();
dstColorCloud.clear();
readDepthImage(dstD, dFilePath);
readColorImage(dstI, iFilePath);
cout << " Point Cloud...";
getPointCloud(dstD, dstPointCloud, dstI, dstColorCloud, dstCenter);
}
}
void DepthFrame::getRegisteredAndReferencedPointCloud(pcl::PointCloud<pcl::PointXYZ>& cloud)
{
pcl::PointXYZ regReferencePoint = getRegisteredReferencePoint();
// Build a translation matrix to the registered reference the cloud after its own registration
Eigen::Matrix4f E = Eigen::Matrix4f::Identity();
E.col(3) = regReferencePoint.getVector4fMap(); // set translation column
// Apply registration first and then referenciation (right to left order in matrix product)
pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudTmp (new pcl::PointCloud<pcl::PointXYZ>);
getPointCloud(*pCloudTmp);
pcl::transformPointCloud(*pCloudTmp, cloud, E.inverse() * m_T);
}
void PointsFileSystem::showPointCloud(const QPersistentModelIndex& index)
{
checked_indexes_.insert(index);
osg::ref_ptr<PointCloud> point_cloud(getPointCloud(filePath(index).toStdString()));
if (!point_cloud.valid())
return;
PointCloudMap::iterator point_cloud_map_it = point_cloud_map_.find(index);
if (point_cloud_map_it != point_cloud_map_.end())
return;
MainWindow::getInstance()->getSceneWidget()->addSceneChild(point_cloud);
point_cloud_map_[index] = point_cloud;
return;
}
int capKinect::saveDepthAsPointCloud(int index, std::string prefix)
{
cv::Mat depth,mask;
load_depth(index,prefix,depth,mask);
int pcs=0;
if(!depth.empty())
{
std::string paths[] = { "firstViewDepth", "secondViewDepth", "thirdViewDepth",
"forthViewDepth", "fifthViewDepth", "sixthViewDepth", "seventhViewDepth" };
std::string curpath=prefix;
curpath.append(paths[index]);
curpath.append(".txt");
pcs = getPointCloud(depth,world);
world.save(curpath);
printf("PointCloud saved in %s!\n",curpath.c_str());
}
return pcs;
}
QModelIndex PointsFileSystem::setRootPath(const QString & root_path)
{
point_cloud_cache_map_.clear();
point_cloud_map_.clear();
checked_indexes_.clear();
QModelIndex index = FileSystemModel::setRootPath(root_path);
computeFrameRange();
if (start_frame_ != -1)
{
if (getPointCloud(start_frame_) != NULL)
showPointCloud(start_frame_);
}
MainWindow::getInstance()->getSceneWidget()->centerScene();
return index;
}
void MainWindow::on_pb_savecloud_clicked()
{
std::vector<cv::Point3d> save_point = getPointCloud();
std::vector<cv::Vec3b> save_rgb = getPointCloudRGB();
std::vector<cv::Matx34d> save_camera = getCameras();
cv::Matx34d tmp;
cv::FileStorage fs("/home/sy/set/data/savingcoud.yml", cv::FileStorage::WRITE);
for(int i = 0; i < filelist.size(); i++)
{
write( fs , "frame" + QString::number(i).toStdString(), Mat(save_camera.at(i)));
}
cout << "saving camera finished " << endl;
cv::write(fs,"points",save_point);
cout << "saving cloud finished " << endl;
cv::write(fs,"rgb",save_rgb);
cout << "saving rgb finished " << endl;
fs.release();
}
PointCloud* PointsFileSystem::getDisplayFirstFrame(void)
{
if (point_cloud_map_.empty())
return NULL;
osg::ref_ptr<PointCloud> first_cloud = *point_cloud_map_.begin();
int frame = first_cloud->getFrame();
for (PointCloudMap::const_iterator it = point_cloud_map_.begin(); it != point_cloud_map_.end(); ++ it)
{
osg::ref_ptr<PointCloud> point_cloud = *it;
int this_frame = point_cloud->getFrame();
if (this_frame < frame)
{
frame = this_frame;
}
}
return getPointCloud(frame);
}
void MultiClientWindow::on_pushButton_SavePointCloud_clicked()
{
for (int i = 1; i < 5; i++)
{
PointCloudT::Ptr PC(new PointCloudT);
PC = getPointCloud(i);
QString Path = QString("%1/PointClouds/%2/%4m_%3.pcd")
.arg(QDir::homePath())
.arg(ui->lineEdit_CampataPath->text())
.arg(QString::fromStdString(PC->header.frame_id))
.arg(QString::number(ui->doubleSpinBox_OffsetCampata->value(),'f',1));
QDir().mkpath(QFileInfo(Path).absolutePath());
qDebug() << Path;
pcl::io::savePCDFileBinary(Path.toStdString(), *PC);
}
}
void pFluid::updateCloud()
{
CK3dPointCloud *cloud = getPointCloud();
if (!cloud)
return;
CKMesh *mesh = getParticleObject()->GetCurrentMesh();
int count = mesh->GetVertexCount();
VxVector *points = new VxVector[count];
if (mParticleBuffer)
{
for (int i = 0 ; i < mesh->GetVertexCount() ; i++)
{
pParticle *p = &mParticleBuffer[i];
if (p)
{
points[i]= getFrom(p->position);
}
}
}
VxVector prec(0.5,0.5,0.5);
VxVector a[10];
int b = cloud->CreateFromPointList(2,a,NULL,NULL,NULL,prec);
if (b)
{
int op2=2;
op2++;
}
}
void MultiClientWindow::on_pushButton_Clean_clicked()
{
pcl::RadiusOutlierRemoval<PointT> outrem;
// build the filter
outrem.setRadiusSearch(0.1);
outrem.setMinNeighborsInRadius(5);
outrem.setNegative(false);
for (int i = 1; i < 5; i++)
{
PointCloudT::Ptr PC_input(new PointCloudT);
PC_input = getPointCloud(i);
PointCloudT::Ptr PC_output(new PointCloudT);
pcl::copyPointCloud(*PC_input, *PC_output);
outrem.setInputCloud(PC_input);
outrem.setKeepOrganized(PC_input->isOrganized());
// apply filter
outrem.filter (*PC_output);
// update PC
setPointCloud(i, PC_output);
ui->widget->replacePC(PC_output);
}
}
/* ------------------------------------------------------------------------- */
void MainWindow::on_PB_Reconstruction_clicked()
{
on_PB_Sift_clicked();
cv::Mat_<double> rvec(1,3);
vector<KeyPoint> imgpts1_tmp;
vector<KeyPoint> imgpts2_tmp;
imgpts.resize(filelist.size());
reconstruct_first_two_view();
cv::Mat_<double> t = (cv::Mat_<double>(1,3) << Pmats[1](0,3), Pmats[1](1,3), Pmats[1](2,3));
cv::Mat_<double> R = (cv::Mat_<double>(3,3) << Pmats[1](0,0), Pmats[1](0,1), Pmats[1](0,2),
Pmats[1](1,0), Pmats[1](1,1), Pmats[1](1,2),
Pmats[1](2,0), Pmats[1](2,1), Pmats[1](2,2));
cv::Matx34d P_0;
cv::Matx34d P_1;
P_0 = cv::Matx34d(1,0,0,0,
0,1,0,0,
0,0,1,0);
int img_prev;
for( int img_now = 2; img_now<filelist.size(); img_now++)
{
cout << endl << endl << endl <<endl;
cout << "dealing with " << filelist.at(img_now).fileName().toStdString() << endl;
cout << endl;
img_prev = img_now - 1;
descriptors1.release();
descriptors1 = descriptors2;
QString ymlFile;
ymlFile = ymlFileDir;
ymlFile.append(filelist.at(img_now).fileName()).append(".yml");
restore_descriptors_from_file(ymlFile.toStdString(),imgpts[img_now],descriptors2);
matches_prev.clear();
matches_prev = matches_new;
matches_new.clear();
//matching
matching_fb_matcher(descriptors1,descriptors2,matches_new);
matching_good_matching_filter(matches_new);
outCloud_prev = outCloud_new;
outCloud_new.clear();
vector<cv::Point3f> tmp3d; vector<cv::Point2f> tmp2d;
for (unsigned int i=0; i < matches_new.size(); i++) {
int idx_in_prev_img = matches_new[i].queryIdx;
for (unsigned int pcldp=0; pcldp<outCloud_prev.size(); pcldp++)
{
if(idx_in_prev_img == outCloud_prev[pcldp].imgpt_for_img[img_prev])
{
tmp3d.push_back(outCloud_prev[pcldp].pt);
tmp2d.push_back(imgpts[img_now][matches_new[i].trainIdx].pt);
break;
}
}
}
bool pose_estimated = FindPoseEstimation(rvec,t,R,tmp3d,tmp2d);
if(!pose_estimated)
{
cout << "error"<<endl;
}
//store estimated pose
Pmats[img_now] = cv::Matx34d (R(0,0),R(0,1),R(0,2),t(0),
R(1,0),R(1,1),R(1,2),t(1),
R(2,0),R(2,1),R(2,2),t(2));
cout << "Pmats:" << endl << Pmats[img_now] << endl;
imgpts1_tmp.clear();
imgpts2_tmp.clear();
GetAlignedPointsFromMatch(imgpts[img_prev], imgpts[img_now], matches_new, imgpts1_tmp, imgpts2_tmp);
std::vector<cv::KeyPoint> correspImg1Pt;
double mean_proj_err = TriangulatePoints(imgpts1_tmp, imgpts2_tmp, K, Kinv,distcoeff, Pmats[img_prev], Pmats[img_now], outCloud, correspImg1Pt);
std::vector<CloudPoint> outCloud_tmp;
outCloud_tmp.clear();
for (unsigned int i=0; i<outCloud.size(); i++)
{
if(outCloud[i].reprojection_error <= 5){
//cout << "surving" << endl;
outCloud[i].imgpt_for_img.resize(filelist.size());
for(int j = 0; j<filelist.size();j++)
{
outCloud[i].imgpt_for_img[j] = -1;
}
outCloud[i].imgpt_for_img[img_now] = matches_new[i].trainIdx;
outCloud_tmp.push_back(outCloud[i]);
}
}
outCloud.clear();
outCloud= outCloud_tmp;
for(unsigned int i=0;i<outCloud.size();i++)
{
outCloud_all.push_back(outCloud[i]);
}
outCloud_new = outCloud;
}
GetRGBForPointCloud(outCloud_all,pointCloudRGB);
ui->widget->update(getPointCloud(),
getPointCloudRGB(),
getCameras());
}
void Camera::getPointCloudUndistorted(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud)
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_distorted(new pcl::PointCloud<pcl::PointXYZRGBA>);
getPointCloud(cloud_distorted);
undistort(cloud_distorted,cloud);
}
/* ------------------------------------------------------------------------- */
void MainWindow::on_method2_clicked()
{
int index_prev;
int index_now;
QString ymlFile;
vector<KeyPoint> imgpts1_tmp;
vector<KeyPoint> imgpts2_tmp;
imgpts.resize(filelist.size());
on_PB_Sift_clicked();
cout << endl << endl << endl << "Using Method 2:" <<endl;
vector<DMatch> matches;
cv::Matx34d P_0;
cv::Matx34d P_1;
P_0 = cv::Matx34d(1,0,0,0,
0,1,0,0,
0,0,1,0);
cv::Mat_<double> t_prev = (cv::Mat_<double>(3,1) << 0, 0, 0);
cv::Mat_<double> R_prev = (cv::Mat_<double>(3,3) << 0, 0, 0,
0, 0, 0,
0, 0, 0);
cv::Mat_<double> R_prev_inv = (cv::Mat_<double>(3,3) << 0, 0, 0,
0, 0, 0,
0, 0, 0);
cv::Mat_<double> t_now = (cv::Mat_<double>(3,1) << 0, 0, 0);
cv::Mat_<double> R_now = (cv::Mat_<double>(3,3) << 0, 0, 0,
0, 0, 0,
0, 0, 0);
cv::Mat_<double> t_new = (cv::Mat_<double>(3,1) << 0, 0, 0);
cv::Mat_<double> R_new = (cv::Mat_<double>(3,3) << 0, 0, 0,
0, 0, 0,
0, 0, 0);
reconstruct_first_two_view();
std::cout << "Pmat[0] = " << endl << Pmats[0]<<endl;
std::cout << "Pmat[1] = " << endl << Pmats[1]<<endl;
for(index_now = 2; index_now<filelist.size(); index_now++)
{
cout << endl << endl << endl <<endl;
cout << "dealing with " << filelist.at(index_now).fileName().toStdString() << endl;
cout << endl;
index_prev = index_now - 1;
descriptors1.release();
descriptors1 = descriptors2;
descriptors2.release();
ymlFile = ymlFileDir;
ymlFile.append(filelist.at(index_now).fileName()).append(".yml");
restore_descriptors_from_file(ymlFile.toStdString(),imgpts[index_now],descriptors2);
matches.clear();
//matching
matching_fb_matcher(descriptors1,descriptors2,matches);
matching_good_matching_filter(matches);
imggoodpts1.clear();
imggoodpts2.clear();
P_0 = cv::Matx34d(1,0,0,0,
0,1,0,0,
0,0,1,0);
P_1 = cv::Matx34d(1,0,0,0,
0,1,0,0,
0,0,1,0);
outCloud.clear();
if(FindCameraMatrices(K,Kinv,distcoeff,
imgpts[index_prev],imgpts[index_now],
imggoodpts1,imggoodpts2,
P_0,P_1,
matches,
outCloud))
{//if can find camera matries
R_prev(0,0) = Pmats[index_prev](0,0);
R_prev(0,1) = Pmats[index_prev](0,1);
R_prev(0,2) = Pmats[index_prev](0,2);
R_prev(1,0) = Pmats[index_prev](1,0);
R_prev(1,1) = Pmats[index_prev](1,1);
R_prev(1,2) = Pmats[index_prev](1,2);
R_prev(2,0) = Pmats[index_prev](2,0);
R_prev(2,1) = Pmats[index_prev](2,1);
R_prev(2,2) = Pmats[index_prev](2,2);
t_prev(0) = Pmats[index_prev](0,3);
t_prev(1) = Pmats[index_prev](1,3);
t_prev(2) = Pmats[index_prev](2,3);
R_now(0,0) = P_1(0,0);
R_now(0,1) = P_1(0,1);
R_now(0,2) = P_1(0,2);
R_now(1,0) = P_1(1,0);
R_now(1,1) = P_1(1,1);
R_now(1,2) = P_1(1,2);
R_now(2,0) = P_1(2,0);
R_now(2,1) = P_1(2,1);
R_now(2,2) = P_1(2,2);
t_now(0) = P_1(0,3);
t_now(1) = P_1(1,3);
t_now(2) = P_1(2,3);
invert(R_prev, R_prev_inv);
t_new = t_prev + R_prev * t_now ;
R_new = R_now * R_prev;
// //store estimated pose
Pmats[index_now] = cv::Matx34d (R_new(0,0),R_new(0,1),R_new(0,2),t_new(0),
R_new(1,0),R_new(1,1),R_new(1,2),t_new(1),
R_new(2,0),R_new(2,1),R_new(2,2),t_new(2));
cout << "Pmats[index_now]:" << endl << Pmats[index_now] << endl;
}
else
{
break;
}
}
cout << endl;
cout << endl;
cout << endl;
//visualization
imgpts.clear();
imgpts.resize(filelist.size());
for( index_now = 1; index_now<filelist.size(); index_now++)
{
index_prev = index_now - 1;
descriptors1.release();
descriptors2.release();
ymlFile = ymlFileDir;
ymlFile.append(filelist.at(index_prev).fileName()).append(".yml");
cout << ymlFile.toStdString()<< endl;
restore_descriptors_from_file(ymlFile.toStdString(),imgpts[index_prev],descriptors1);
ymlFile = ymlFileDir;
ymlFile.append(filelist.at(index_now).fileName()).append(".yml");
cout << ymlFile.toStdString()<< endl;
restore_descriptors_from_file(ymlFile.toStdString(),imgpts[index_now],descriptors2);
matches.clear();
//matching
matching_fb_matcher(descriptors1,descriptors2,matches);
matching_good_matching_filter(matches);
imgpts1_tmp.clear();
imgpts2_tmp.clear();
GetAlignedPointsFromMatch(imgpts[index_prev], imgpts[index_now], matches, imgpts1_tmp, imgpts2_tmp);
cout << imgpts1_tmp.size() << endl;
cout << imgpts1_tmp.size() << endl;
outCloud.clear();
std::vector<cv::KeyPoint> correspImg1Pt;
double mean_proj_err = TriangulatePoints(imgpts1_tmp, imgpts2_tmp, K, Kinv,distcoeff, Pmats[index_prev], Pmats[index_now], outCloud, correspImg1Pt);
std::vector<CloudPoint> outCloud_tmp;
outCloud_tmp.clear();
for (unsigned int i=0; i<outCloud.size(); i++)
{
if(outCloud[i].reprojection_error <= 3){
//cout << "surving" << endl;
outCloud[i].imgpt_for_img.resize(filelist.size());
for(int j = 0; j<filelist.size();j++)
{
outCloud[i].imgpt_for_img[j] = -1;
}
outCloud[i].imgpt_for_img[index_now] = matches[i].trainIdx;
outCloud_tmp.push_back(outCloud[i]);
}
}
outCloud.clear();
outCloud= outCloud_tmp;
cout << outCloud_tmp.size() << endl;
for(unsigned int i=0;i<outCloud.size();i++)
{
outCloud_all.push_back(outCloud[i]);
}
outCloud_new = outCloud;
}
for( int i = 0; i<filelist.size(); i++)
Pmats[i](1,3) =0;
GetRGBForPointCloud(outCloud_all,pointCloudRGB);
ui->widget->update(getPointCloud(),
getPointCloudRGB(),
getCameras());
cout << "finished" <<endl <<endl;
}
osg::ref_ptr<PointCloud> PointsFileSystem::getPointCloud(int frame)
{
return getPointCloud(getPointsFilename(frame));
}
