void
extractLibmvReconstructionData(InputOutputArray K,
OutputArray Rs,
OutputArray Ts,
OutputArray points3d)
{
getCameras(Rs, Ts);
getPoints(points3d);
getIntrinsics().copyTo(K.getMat());
}
void Scene::render(Renderer* renderer)
{
auto director = Director::getInstance();
Camera* defaultCamera = nullptr;
const auto& transform = getNodeToParentTransform();
for (const auto& camera : getCameras())
{
if (!camera->isVisible())
continue;
Camera::_visitingCamera = camera;
if (Camera::_visitingCamera->getCameraFlag() == CameraFlag::DEFAULT)
{
defaultCamera = Camera::_visitingCamera;
}
director->pushMatrix(MATRIX_STACK_TYPE::MATRIX_STACK_PROJECTION);
director->loadMatrix(MATRIX_STACK_TYPE::MATRIX_STACK_PROJECTION, Camera::_visitingCamera->getViewProjectionMatrix());
camera->apply();
//clear background with max depth
camera->clearBackground();
//visit the scene
visit(renderer, transform, 0);
#if CC_USE_NAVMESH
if (_navMesh && _navMeshDebugCamera == camera)
{
_navMesh->debugDraw(renderer);
}
#endif
renderer->render();
director->popMatrix(MATRIX_STACK_TYPE::MATRIX_STACK_PROJECTION);
}
#if CC_USE_3D_PHYSICS && CC_ENABLE_BULLET_INTEGRATION
if (_physics3DWorld && _physics3DWorld->isDebugDrawEnabled())
{
director->pushMatrix(MATRIX_STACK_TYPE::MATRIX_STACK_PROJECTION);
director->loadMatrix(MATRIX_STACK_TYPE::MATRIX_STACK_PROJECTION, _physics3dDebugCamera != nullptr ? _physics3dDebugCamera->getViewProjectionMatrix() : defaultCamera->getViewProjectionMatrix());
_physics3DWorld->debugDraw(renderer);
renderer->render();
director->popMatrix(MATRIX_STACK_TYPE::MATRIX_STACK_PROJECTION);
}
#endif
Camera::_visitingCamera = nullptr;
experimental::FrameBuffer::applyDefaultFBO();
}
AScene3D *ASceneDecoder::createNewSceneObject()
{
DEBUG_OUT<<"ASceneDecoder::createNewSceneObject() starting...\n";
AScene3D *s=new AScene3D();
DEBUG_OUT<<"Meshes...\n";
s->setMeshes((AMesh3D **)allocArrayBigEnoughForList(getMeshes()));
s->setNumMeshes(buildArrayFromList((void **)s->getMeshes(),getMeshes()));
DEBUG_OUT<<"Lights...\n";
s->setLights((ALight3D **)allocArrayBigEnoughForList(getLights()));
s->setNumLights(buildArrayFromList((void **)s->getLights(),getLights()));
DEBUG_OUT<<"Cameras...\n";
s->setCameras((ACamera3D **)allocArrayBigEnoughForList(getCameras()));
s->setNumCameras(buildArrayFromList((void **)s->getCameras(),getCameras()));
//
//DEBUG_OUT<<"Sky...\n";
//theSky.surf=readFile->getTheSky()->surf;
//theSky.fRed=readFile->getTheSky()->fRed;
//theSky.fBlue=readFile->getTheSky()->fBlue;
//theSky.fGreen=readFile->getTheSky()->fGreen;
//DEBUG_OUT<<"Ground...\n";
//createGroundMesh(readFile->getTheGround());
DEBUG_OUT<<"Done.\n";
return s;
}
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();
}
void EventDispatcher::dispatchTouchEventToListeners(EventListenerVector* listeners, const std::function<bool(EventListener*)>& onEvent)
{
bool shouldStopPropagation = false;
auto fixedPriorityListeners = listeners->getFixedPriorityListeners();
auto sceneGraphPriorityListeners = listeners->getSceneGraphPriorityListeners();
ssize_t i = 0;
// priority < 0
if (fixedPriorityListeners)
{
CCASSERT(listeners->getGt0Index() <= static_cast<ssize_t>(fixedPriorityListeners->size()), "Out of range exception!");
if (!fixedPriorityListeners->empty())
{
for (; i < listeners->getGt0Index(); ++i)
{
auto l = fixedPriorityListeners->at(i);
if (l->isEnabled() && !l->isPaused() && l->isRegistered() && onEvent(l))
{
shouldStopPropagation = true;
break;
}
}
}
}
auto scene = Director::getInstance()->getRunningScene();
if (scene && sceneGraphPriorityListeners)
{
if (!shouldStopPropagation)
{
// priority == 0, scene graph priority
// first, get all enabled, unPaused and registered listeners
std::vector<EventListener*> sceneListeners;
for (auto& l : *sceneGraphPriorityListeners)
{
if (l->isEnabled() && !l->isPaused() && l->isRegistered())
{
sceneListeners.push_back(l);
}
}
// second, for all camera call all listeners
// get a copy of cameras, prevent it's been modified in listener callback
// if camera's depth is greater, process it earlier
auto cameras = scene->getCameras();
Camera* camera;
for (int j = int(cameras.size()) - 1; j >= 0; --j)
{
camera = cameras[j];
if (camera->isVisible() == false)
{
continue;
}
Camera::_visitingCamera = camera;
auto cameraFlag = (unsigned short)camera->getCameraFlag();
for (auto& l : sceneListeners)
{
if (nullptr == l->getAssociatedNode() || 0 == (l->getAssociatedNode()->getCameraMask() & cameraFlag))
{
continue;
}
if (onEvent(l))
{
shouldStopPropagation = true;
break;
}
}
if (shouldStopPropagation)
{
break;
}
}
Camera::_visitingCamera = nullptr;
}
}
if (fixedPriorityListeners)
{
if (!shouldStopPropagation)
{
// priority > 0
ssize_t size = fixedPriorityListeners->size();
for (; i < size; ++i)
{
auto l = fixedPriorityListeners->at(i);
if (l->isEnabled() && !l->isPaused() && l->isRegistered() && onEvent(l))
{
shouldStopPropagation = true;
break;
}
}
}
}
}
/* ------------------------------------------------------------------------- */
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;
}
/* ------------------------------------------------------------------------- */
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());
}
