i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-08 12:30:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

Database: added "opt_****" fields in Admin table. Tango: optimized mesh saved in database for quick open, open menu shows preview images. util3d::mergeTextures() return all textures in same cv::Mat.

Browse Source
This commit is contained in:
matlabbe
2017-06-20 17:29:06 -04:00
parent 539d500528
commit ee8d48a915
25 changed files with 1376 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/android/jni/RTABMapApp.cpp
+291 -28
View File
@@ -50,6 +50,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/VWDictionary.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/GainCompensator.h>
#include <rtabmap/core/DBDriver.h>
#include <pcl/common/common.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/ply_io.h>
@@ -174,6 +175,7 @@ RTABMapApp::RTABMapApp() :
filterPolygonsOnNextRender_(false),
gainCompensationOnNextRender_(0),
bilateralFilteringOnNextRender_(false),
takeScreenshotOnNextRender_(false),
cameraJustInitialized_(false),
meshDecimation_(1),
totalPoints_(0),
@@ -271,14 +273,14 @@ void RTABMapApp::setScreenRotation(int displayRotation, int cameraRotation)
camera_->setScreenRotation(rotation);
}
int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize)
int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, const std::string & databaseSource)
{
LOGI("Opening database %s (inMemory=%d, optimize=%d)", databasePath.c_str(), databaseInMemory?1:0, optimize?1:0);
this->unregisterFromEventsManager(); // to ignore published init events when closing rtabmap
status_.first = rtabmap::RtabmapEventInit::kInitializing;
openingDatabase_ = true;
rtabmapMutex_.lock();
rtabmapEvents_.clear();
openingDatabase_ = true;
if(rtabmapThread_)
{
rtabmapThread_->close(false);
@@ -287,6 +289,165 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
rtabmap_ = 0;
}
int status = 0;
// Open visualization while we load (if there is an optimized mesh saved in database)
exportedMesh_.reset(new pcl::TextureMesh);
exportedTexture_ = cv::Mat();
cv::Mat cloudMat;
std::vector<std::vector<std::vector<unsigned int> > > polygons;
cv::Mat textures;
std::map<int, rtabmap::Transform> optPoses;
if(!databaseSource.empty())
{
UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading optimized mesh..."));
rtabmap::DBDriver * driver = rtabmap::DBDriver::create();
if(driver->openConnection(databaseSource))
{
cloudMat = driver->loadOptimizedMesh(&optPoses, &polygons, &exportedMesh_->tex_coordinates, &textures);
if(!cloudMat.empty())
{
LOGI("Open: Found optimized mesh! Visualizing it.");
if(cloudMat.channels() <= 3)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(cloudMat);
pcl::toPCLPointCloud2(*cloud, exportedMesh_->cloud);
}
else if(cloudMat.channels() == 4)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGB(cloudMat);
pcl::toPCLPointCloud2(*cloud, exportedMesh_->cloud);
}
else if(cloudMat.channels() == 6)
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudNormal(cloudMat);
pcl::toPCLPointCloud2(*cloud, exportedMesh_->cloud);
}
else if(cloudMat.channels() == 7)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGBNormal(cloudMat);
pcl::toPCLPointCloud2(*cloud, exportedMesh_->cloud);
}
if(exportedMesh_->cloud.data.size())
{
status = 1;
}
if(exportedMesh_->cloud.data.size() && polygons.size())
{
status = 2;
exportedMesh_->tex_polygons.resize(polygons.size());
for(unsigned int t=0; t<polygons.size(); ++t)
{
exportedMesh_->tex_polygons[t].resize(polygons[t].size());
for(unsigned int p=0; p<polygons[t].size(); ++p)
{
exportedMesh_->tex_polygons[t][p].vertices = polygons[t][p];
}
}
if(!exportedMesh_->tex_coordinates.empty())
{
status = 3;
UASSERT(!textures.empty() && textures.cols % textures.rows == 0 && textures.cols/textures.rows == (int)exportedMesh_->tex_coordinates.size());
if(textures.cols/textures.rows == 1)
{
exportedTexture_ = textures;
}
else if(textures.cols/textures.rows > 1)
{
// Visualization doesn't support more than one material, so concatenate to 1
std::vector<bool> materialsKept;
float scale = 0.0f;
cv::Size imageSize(textures.rows, textures.rows);
int imageType = CV_8UC3;
rtabmap::util3d::concatenateTextureMaterials(*exportedMesh_, imageSize, textures.rows, 1, scale, &materialsKept);
if(scale && exportedMesh_->tex_materials.size() == 1)
{
int cols = float(textures.rows)/(scale*imageSize.width);
int rows = float(textures.rows)/(scale*imageSize.height);
exportedTexture_ = cv::Mat(textures.rows, textures.rows, imageType, cv::Scalar::all(255));
// make a blank texture
cv::Size resizedImageSize(int(imageSize.width*scale), int(imageSize.height*scale));
int oi=0;
for(int i=0; i<(int)materialsKept.size(); ++i)
{
if(materialsKept.at(i))
{
int u = oi%cols * resizedImageSize.width;
int v = ((oi/cols) % rows ) * resizedImageSize.height;
UASSERT(u < textures.rows-resizedImageSize.width);
UASSERT(v < textures.rows-resizedImageSize.height);
cv::Mat resizedImage;
cv::resize(textures(cv::Range::all(), cv::Range(i*textures.rows, (i+1)*textures.rows)), resizedImage, resizedImageSize, 0.0f, 0.0f, cv::INTER_AREA);
UASSERT(resizedImage.type() == exportedTexture_.type());
resizedImage.copyTo(exportedTexture_(cv::Rect(u, v, resizedImage.cols, resizedImage.rows)));
++oi;
}
}
}
}
exportedMesh_->tex_materials.resize (exportedMesh_->tex_coordinates.size () + 1);
for(unsigned int i = 0 ; i <= exportedMesh_->tex_coordinates.size() ; ++i)
{
pcl::TexMaterial mesh_material;
mesh_material.tex_Ka.r = 0.2f;
mesh_material.tex_Ka.g = 0.2f;
mesh_material.tex_Ka.b = 0.2f;
mesh_material.tex_Kd.r = 0.8f;
mesh_material.tex_Kd.g = 0.8f;
mesh_material.tex_Kd.b = 0.8f;
mesh_material.tex_Ks.r = 1.0f;
mesh_material.tex_Ks.g = 1.0f;
mesh_material.tex_Ks.b = 1.0f;
mesh_material.tex_d = 1.0f;
mesh_material.tex_Ns = 75.0f;
mesh_material.tex_illum = 2;
std::stringstream tex_name;
tex_name << "material_" << i;
tex_name >> mesh_material.tex_name;
mesh_material.tex_file = uFormat("%d", i);
exportedMesh_->tex_materials[i] = mesh_material;
}
}
}
}
else
{
LOGI("Open: No optimized mesh found.");
}
delete driver;
}
}
if(status > 0)
{
boost::mutex::scoped_lock lockRender(renderingMutex_);
visualizingMesh_ = true;
exportedMeshUpdated_ = true;
}
LOGI("Erasing database \"%s\"...", databasePath.c_str());
UFile::erase(databasePath);
if(!databaseSource.empty())
{
LOGI("Copying database source \"%s\" to \"%s\"...", databaseSource.c_str(), databasePath.c_str());
UFile::copy(databaseSource, databasePath);
}
this->registerToEventsManager();
//Rtabmap
@@ -316,7 +477,6 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
true,
true);
int status = 0;
if(signatures.size() && poses.empty())
{
LOGE("Failed to optimize the graph!");
@@ -329,7 +489,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
createdMeshes_.clear();
int i=0;
UTimer addTime;
for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end() && status==0; ++iter)
for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end() && status>=0; ++iter)
{
try
{
@@ -386,6 +546,14 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
}
LOGI("Created cloud %d (%fs)", id, timer.ticks());
}
else
{
LOGI("Cloud %d not added to created meshes", id);
}
}
else
{
UWARN("Cloud %d is empty", id);
}
}
else
@@ -404,6 +572,14 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
processMemoryUsedBytes +=s.getWordsDescriptors().size()*(4+s.getWordsDescriptors().begin()->second.total());
}
}
else
{
UWARN("Data for node %d not found", id);
}
}
else
{
UWARN("Pose %d is null !?", id);
}
++i;
if(addTime.elapsed() >= 4.0f)
@@ -428,14 +604,19 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
status = -2;
}
}
if(status < 0)
{
createdMeshes_.clear();
}
else
{
LOGI("Created %d meshes...", (int)createdMeshes_.size());
}
}
if(status < 0)
{
createdMeshes_.clear();
}
if(optimize && status==0)
if(optimize && status>=0)
{
UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Visual optimization..."));
gainCompensation();
@@ -477,11 +658,12 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
rtabmapMutex_.unlock();
boost::mutex::scoped_lock lockRender(renderingMutex_);
if(poses.empty())
if(poses.empty() || status>0)
{
openingDatabase_ = false;
}
clearSceneOnNextRender_ = true;
clearSceneOnNextRender_ = status<=0;
return status;
}
@@ -868,7 +1050,7 @@ int RTABMapApp::Render()
std::list<rtabmap::RtabmapEvent*> rtabmapEvents;
try
{
UASSERT(camera_!=0 && rtabmap_!=0);
UASSERT(camera_!=0);
UTimer fpsTime;
#ifdef DEBUG_RENDERING_PERFORMANCE
@@ -876,14 +1058,14 @@ int RTABMapApp::Render()
#endif
boost::mutex::scoped_lock lock(renderingMutex_);
bool notifyDataLoaded = false;
bool notifyCameraStarted = false;
if(clearSceneOnNextRender_)
{
visualizingMesh_ = false;
}
bool notifyDataLoaded = false;
bool notifyCameraStarted = false;
// process only pose events in visualization mode
rtabmap::Transform pose;
{
@@ -939,6 +1121,7 @@ int RTABMapApp::Render()
}
if(!main_scene_.hasCloud(g_exportedMeshId))
{
LOGI("Adding optimized mesh to opengl...");
if(exportedMesh_->tex_polygons.size() && exportedMesh_->tex_polygons[0].size())
{
Mesh mesh;
@@ -1020,6 +1203,7 @@ int RTABMapApp::Render()
if(clearSceneOnNextRender_)
{
LOGI("Clearing all rendering data...");
odomMutex_.lock();
odomEvents_.clear();
odomMutex_.unlock();
@@ -1033,6 +1217,7 @@ int RTABMapApp::Render()
if(!openingDatabase_)
{
boost::mutex::scoped_lock lock(meshesMutex_);
LOGI("Clearing meshes...");
createdMeshes_.clear();
}
else
@@ -1061,6 +1246,8 @@ int RTABMapApp::Render()
{
LOGI("added (%d) != meshes (%d)", (int)added.size(), meshes);
processGPUMemoryUsedBytes = 0;
boost::mutex::scoped_lock lockRtabmap(rtabmapMutex_);
UASSERT(rtabmap_!=0);
for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
{
if(!main_scene_.hasCloud(iter->first) && !iter->second.pose.isNull())
@@ -1475,7 +1662,34 @@ int RTABMapApp::Render()
}
}
if(openingDatabase_ || exporting_ || postProcessing_)
if(takeScreenshotOnNextRender_)
{
takeScreenshotOnNextRender_ = false;
int w = main_scene_.getViewPortWidth();
int h = main_scene_.getViewPortHeight();
cv::Mat image(h, w, CV_8UC4);
glReadPixels(0, 0, w, h, GL_RGBA, GL_UNSIGNED_BYTE, image.data);
cv::flip(image, image, 0);
cv::cvtColor(image, image, CV_RGBA2BGRA);
cv::Mat roi;
if(w>h)
{
int offset = (w-h)/2;
roi = image(cv::Range::all(), cv::Range(offset,offset+h));
}
else
{
int offset = (h-w)/2;
roi = image(cv::Range(offset,offset+w), cv::Range::all());
}
rtabmapMutex_.lock();
LOGI("Saving screenshot %dx%d...", roi.cols, roi.rows);
rtabmap_->getMemory()->savePreviewImage(roi);
rtabmapMutex_.unlock();
screenshotReady_.release();
}
if((openingDatabase_ && !visualizingMesh_) || exporting_ || postProcessing_)
{
// throttle rendering max 5Hz if we are doing some processing
double renderTime = fpsTime.elapsed();
@@ -1846,10 +2060,17 @@ void RTABMapApp::resetMapping()
void RTABMapApp::save(const std::string & databasePath)
{
LOGI("Saving database to %s", databasePath.c_str());
rtabmapThread_->join(true);
// save mapping parameters in the database
LOGI("Taking screenshot...");
takeScreenshotOnNextRender_ = true;
if(!screenshotReady_.acquire(1, 2000))
{
UERROR("Failed to take a screenshot after 2 sec!");
}
// save mapping parameters in the database
bool appendModeBackup = appendMode_;
if(appendMode_)
{
@@ -1962,7 +2183,7 @@ bool RTABMapApp::exportMesh(
pcl::PolygonMesh::Ptr polygonMesh(new pcl::PolygonMesh);
pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
std::vector<std::map<int, pcl::PointXY> > vertexToPixels;
std::vector<cv::Mat> globalTextures;
cv::Mat globalTextures;
int totalPolygons = 0;
{
if(optimized)
@@ -2420,10 +2641,11 @@ bool RTABMapApp::exportMesh(
return false;
}
LOGD("Saving texture(s) (%d)", (int)globalTextures.size());
LOGD("Saving texture(s) (%d)", globalTextures.empty()?0:globalTextures.cols/globalTextures.rows);
std::string baseName = uSplit(UFile::getName(filePath), '.').front();
std::string textureDirectory = UDirectory::getDir(filePath);
UASSERT(textureMesh->tex_materials.size() == globalTextures.size());
UASSERT(globalTextures.empty() || globalTextures.cols % globalTextures.rows == 0);
UASSERT((int)textureMesh->tex_materials.size() == globalTextures.cols/globalTextures.rows);
for(unsigned int i=0; i<textureMesh->tex_materials.size(); ++i)
{
std::string baseNameNum = baseName;
@@ -2434,13 +2656,13 @@ bool RTABMapApp::exportMesh(
std::string fullPath = textureDirectory+UDirectory::separator()+baseNameNum+".jpg";
textureMesh->tex_materials[i].tex_file = baseNameNum+".jpg";
LOGI("Saving texture to %s.", fullPath.c_str());
if(!cv::imwrite(fullPath, globalTextures[i]))
if(!cv::imwrite(fullPath, globalTextures(cv::Range::all(), cv::Range(i*globalTextures.rows, (i+1)*globalTextures.rows))))
{
LOGI("Failed saving %s!", fullPath.c_str());
}
else
{
LOGI("Saved %s (%d bytes).", fullPath.c_str(), globalTextures[i].total()*globalTextures[i].channels());
LOGI("Saved %s.", fullPath.c_str());
}
}
}
@@ -2471,6 +2693,22 @@ bool RTABMapApp::exportMesh(
exportedMesh_.reset(new pcl::TextureMesh);
exportedMesh_->cloud = polygonMesh->cloud;
exportedMesh_->tex_polygons.push_back(polygonMesh->polygons);
// save in database
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::fromPCLPointCloud2(polygonMesh->cloud, *cloud);
cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud)); // for database
std::vector<std::vector<std::vector<unsigned int> > > polygons(exportedMesh_->tex_polygons.size());
for(unsigned int t=0; t<exportedMesh_->tex_polygons.size(); ++t)
{
polygons[t].resize(exportedMesh_->tex_polygons[t].size());
for(unsigned int p=0; p<exportedMesh_->tex_polygons[t].size(); ++p)
{
polygons[t][p] = exportedMesh_->tex_polygons[t][p].vertices;
}
}
boost::mutex::scoped_lock lock(rtabmapMutex_);
rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses, polygons);
}
else
{
@@ -2483,6 +2721,7 @@ bool RTABMapApp::exportMesh(
// With Sketchfab, the OBJ models are rotated 90 degrees on x axis, so rotate -90 to have model in right position
pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud)); // for database
pcl::PCLPointCloud2 tmp = textureMesh->cloud;
cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::Transform(1,0,0,0, 0,0,1,0, 0,-1,0,0));
pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
@@ -2493,16 +2732,32 @@ bool RTABMapApp::exportMesh(
{
LOGI("Saved obj to %s!", filePath.c_str());
exportedMesh_ = textureMesh;
if(globalTextures.size() == 1)
// save in database
{
exportedTexture_ = globalTextures[0];
std::vector<std::vector<std::vector<unsigned int> > > polygons(exportedMesh_->tex_polygons.size());
for(unsigned int t=0; t<exportedMesh_->tex_polygons.size(); ++t)
{
polygons[t].resize(exportedMesh_->tex_polygons[t].size());
for(unsigned int p=0; p<exportedMesh_->tex_polygons[t].size(); ++p)
{
polygons[t][p] = exportedMesh_->tex_polygons[t][p].vertices;
}
}
boost::mutex::scoped_lock lock(rtabmapMutex_);
rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses, polygons, exportedMesh_->tex_coordinates, globalTextures);
}
else if(globalTextures.size() > 1)
if(globalTextures.cols/globalTextures.rows == 1)
{
exportedTexture_ = globalTextures;
}
else if(globalTextures.cols/globalTextures.rows > 1)
{
// Visualization doesn't support more than one material, so concatenate to 1
std::vector<bool> materialsKept;
float scale = 0.0f;
cv::Size imageSize = globalTextures[0].size();
cv::Size imageSize(globalTextures.rows, globalTextures.rows);
int imageType = CV_8UC3;
rtabmap::util3d::concatenateTextureMaterials(*exportedMesh_, imageSize, textureSize, 1, scale, &materialsKept);
if(scale && exportedMesh_->tex_materials.size() == 1)
@@ -2515,7 +2770,7 @@ bool RTABMapApp::exportMesh(
// make a blank texture
cv::Size resizedImageSize(int(imageSize.width*scale), int(imageSize.height*scale));
int oi=0;
for(int i=0; i<(int)globalTextures.size(); ++i)
for(int i=0; i<(int)materialsKept.size(); ++i)
{
if(materialsKept.at(i))
{
@@ -2525,7 +2780,7 @@ bool RTABMapApp::exportMesh(
UASSERT(v < textureSize-resizedImageSize.height);
cv::Mat resizedImage;
cv::resize(globalTextures[i], resizedImage, resizedImageSize, 0.0f, 0.0f, cv::INTER_AREA);
cv::resize(globalTextures(cv::Range::all(), cv::Range(i*globalTextures.rows, (i+1)*globalTextures.rows)), resizedImage, resizedImageSize, 0.0f, 0.0f, cv::INTER_AREA);
UASSERT(resizedImage.type() == exportedTexture_.type());
resizedImage.copyTo(exportedTexture_(cv::Rect(u, v, resizedImage.cols, resizedImage.rows)));
@@ -2664,6 +2919,14 @@ bool RTABMapApp::exportMesh(
if(success)
{
LOGI("Saved ply to %s!", filePath.c_str());
// save in database
{
cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*mergedClouds)); // for database
boost::mutex::scoped_lock lock(rtabmapMutex_);
rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses);
}
mergedClouds->clear();
exportedMesh_.reset(new pcl::TextureMesh);
exportedMesh_->cloud = mesh.cloud;
app/android/jni/RTABMapApp.h
+5 -1
View File
@@ -56,7 +56,7 @@ class RTABMapApp : public UEventsHandler {
void setScreenRotation(int displayRotation, int cameraRotation);
int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize);
int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, const std::string & databaseSource=std::string());
bool onTangoServiceConnected(JNIEnv* env, jobject iBinder);
@@ -218,6 +218,7 @@ class RTABMapApp : public UEventsHandler {
bool filterPolygonsOnNextRender_;
int gainCompensationOnNextRender_;
bool bilateralFilteringOnNextRender_;
bool takeScreenshotOnNextRender_;
bool cameraJustInitialized_;
int meshDecimation_;
int totalPoints_;
@@ -233,6 +234,7 @@ class RTABMapApp : public UEventsHandler {
bool exportedMeshUpdated_;
pcl::TextureMesh::Ptr exportedMesh_;
cv::Mat exportedTexture_;
std::map<int, rtabmap::Transform> exportedPoses_;
// main_scene_ includes all drawable object for visualizing Tango device's
// movement and point cloud.
@@ -250,6 +252,8 @@ class RTABMapApp : public UEventsHandler {
boost::mutex poseMutex_;
boost::mutex renderingMutex_;
USemaphore screenshotReady_;
std::map<int, Mesh> createdMeshes_;
std::map<int, rtabmap::Transform> rawPoses_;
app/android/jni/jni_interface.cpp
+11
View File
@@ -71,6 +71,17 @@ Java_com_introlab_rtabmap_RTABMapLib_openDatabase(
return app.openDatabase(databasePathC, databaseInMemory, optimize);
}
JNIEXPORT int JNICALL
Java_com_introlab_rtabmap_RTABMapLib_openDatabase2(
JNIEnv* env, jobject, jstring databaseSource, jstring databasePath, bool databaseInMemory, bool optimize)
{
std::string databasePathC;
GetJStringContent(env,databasePath,databasePathC);
std::string databaseSourceC;
GetJStringContent(env,databaseSource,databaseSourceC);
return app.openDatabase(databasePathC, databaseInMemory, optimize, databaseSourceC);
}
JNIEXPORT bool JNICALL
Java_com_introlab_rtabmap_RTABMapLib_onTangoServiceConnected(
JNIEnv* env, jobject, jobject iBinder) {
app/android/jni/scene.h
+2
View File
@@ -57,6 +57,8 @@ class Scene {
// Setup GL view port.
void SetupViewPort(int w, int h);
int getViewPortWidth() const {return screenWidth_;}
int getViewPortHeight() const {return screenHeight_;}
void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {color_camera_to_display_rotation_ = colorCameraToDisplayRotation;}
app/android/jni/util.h
+2 -2
View File
@@ -44,8 +44,8 @@ class LogHandler : public UEventsHandler
public:
LogHandler()
{
ULogger::setLevel(ULogger::kWarning);
ULogger::setEventLevel(ULogger::kWarning);
ULogger::setLevel(ULogger::kDebug);
ULogger::setEventLevel(ULogger::kDebug);
ULogger::setPrintThreadId(true);
registerToEventsManager();
app/android/res/layout/database_list.xml
+22
View File
@@ -0,0 +1,22 @@
<?xml version="1.0" encoding="utf-8"?>
<LinearLayout xmlns:android="http://schemas.android.com/apk/res/android"
android:layout_width="match_parent"
android:layout_height="wrap_content"
android:background="#fff">
<ImageView
android:id="@+id/imageView"
android:layout_width="150dp"
android:layout_height="150dp"
android:layout_marginLeft="10dp"
android:padding="5dp"
android:src="@drawable/ic_launcher" />
<TextView
android:id="@+id/textView"
android:layout_width="wrap_content"
android:layout_height="wrap_content"
android:layout_gravity="center"
android:text="Demo"
android:textColor="#000" />
</LinearLayout>
app/android/src/com/introlab/rtabmap/DatabaseListArrayAdapter.java
+89
View File
@@ -0,0 +1,89 @@
package com.introlab.rtabmap;
import android.content.Context;
import android.database.Cursor;
import android.database.sqlite.SQLiteDatabase;
import android.graphics.Bitmap;
import android.graphics.BitmapFactory;
import android.util.Log;
import android.view.LayoutInflater;
import android.view.View;
import android.view.ViewGroup;
import android.widget.BaseAdapter;
import android.widget.ImageView;
import android.widget.SimpleAdapter;
import java.io.ByteArrayInputStream;
import java.io.InputStream;
import java.util.ArrayList;
import java.util.HashMap;
public class DatabaseListArrayAdapter extends SimpleAdapter {
LayoutInflater inflater;
Context context;
ArrayList<HashMap<String, String>> arrayList;
public DatabaseListArrayAdapter(Context context, ArrayList<HashMap<String, String>> data, int resource, String[] from, int[] to) {
super(context, data, resource, from, to);
this.context = context;
this.arrayList = data;
inflater.from(context);
}
@Override
public View getView(final int position, View convertView, ViewGroup parent) {
View view = super.getView(position, convertView, parent);
ImageView imageView = (ImageView) view.findViewById(R.id.imageView);
String path = this.arrayList.get(position).get("path");
if(!path.isEmpty())
{
SQLiteDatabase db = null;
try {
db = SQLiteDatabase.openDatabase(path, null, SQLiteDatabase.OPEN_READONLY);
// get version
Cursor c1 = db.rawQuery("SELECT version FROM Admin", null);
if(c1.moveToFirst()) {
String version = c1.getString(c1.getColumnIndex("version"));
Log.i(RTABMapActivity.TAG, "Version="+version);
if(Util.versionCompare(version, "0.12.0") >= 0) {
Cursor c2 = db.rawQuery("SELECT preview_image FROM Admin WHERE preview_image is not null", null);
if(c2.moveToFirst()) {
Log.i(RTABMapActivity.TAG, "Found image preview for db " + path);
byte[] bytes = c2.getBlob(c2.getColumnIndex("preview_image"));
ByteArrayInputStream inputStream = new ByteArrayInputStream(bytes);
Bitmap bitmap = BitmapFactory.decodeStream(inputStream);
imageView.setImageBitmap(bitmap);
}
else {
Log.i(RTABMapActivity.TAG, "Not found image preview for db " + path);
}
}
else {
Log.i(RTABMapActivity.TAG, "Too old database for preview image, path = " + path);
}
}
else {
Log.e(RTABMapActivity.TAG, "Failed getting version from database");
}
} catch (Exception e) {
Log.e(RTABMapActivity.TAG, e.getMessage());
}
finally {
if(db != null && db.isOpen()) {
db.close();
}
}
}
else
{
Log.e(RTABMapActivity.TAG, "Database path empty for item " + position);
}
return view;
}
}
app/android/src/com/introlab/rtabmap/RTABMapActivity.java
+109 -19
View File
@@ -12,8 +12,10 @@ import java.io.OutputStream;
import java.lang.reflect.InvocationTargetException;
import java.lang.reflect.Method;
import java.text.SimpleDateFormat;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Date;
import java.util.HashMap;
import java.util.List;
import java.util.zip.ZipEntry;
import java.util.zip.ZipOutputStream;
@@ -38,6 +40,8 @@ import android.content.pm.PackageInfo;
import android.content.pm.PackageManager;
import android.content.pm.PackageManager.NameNotFoundException;
import android.content.res.Configuration;
import android.database.Cursor;
import android.database.sqlite.SQLiteDatabase;
import android.graphics.Bitmap;
import android.graphics.Canvas;
import android.graphics.Color;
@@ -133,7 +137,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
private static enum State {
STATE_IDLE,
STATE_PROCESSING,
STATE_VISUALIZING
STATE_VISUALIZING,
STATE_VISUALIZING_WHILE_LOADING
}
State mState = State.STATE_IDLE;
@@ -1264,6 +1269,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
mButtonLighting.setVisibility(mHudVisible && !mItemRenderingPointCloud.isChecked()?View.VISIBLE:View.INVISIBLE);
mButtonWireframe.setVisibility(mHudVisible && !mItemRenderingPointCloud.isChecked()?View.VISIBLE:View.INVISIBLE);
mButtonCloseVisualization.setVisibility(mHudVisible?View.VISIBLE:View.INVISIBLE);
mButtonCloseVisualization.setEnabled(true);
mButtonSaveOnDevice.setVisibility(mHudVisible?View.VISIBLE:View.INVISIBLE);
mButtonShareOnSketchfab.setVisibility(mHudVisible?View.VISIBLE:View.INVISIBLE);
mItemSave.setEnabled(mButtonPause.isChecked());
@@ -1276,6 +1282,22 @@ public class RTABMapActivity extends Activity implements OnClickListener {
mButtonPause.setVisibility(View.INVISIBLE);
mItemDataRecorderMode.setEnabled(mButtonPause.isChecked());
break;
case STATE_VISUALIZING_WHILE_LOADING:
mButtonLighting.setVisibility(mHudVisible && !mItemRenderingPointCloud.isChecked()?View.VISIBLE:View.INVISIBLE);
mButtonWireframe.setVisibility(mHudVisible && !mItemRenderingPointCloud.isChecked()?View.VISIBLE:View.INVISIBLE);
mButtonCloseVisualization.setVisibility(mHudVisible?View.VISIBLE:View.INVISIBLE);
mButtonCloseVisualization.setEnabled(false);
mButtonSaveOnDevice.setVisibility(View.INVISIBLE);
mButtonShareOnSketchfab.setVisibility(View.INVISIBLE);
mItemSave.setEnabled(false);
mItemExport.setEnabled(false);
mItemOpen.setEnabled(false);
mItemPostProcessing.setEnabled(false);
mItemSettings.setEnabled(false);
mItemReset.setEnabled(false);
mItemModes.setEnabled(false);
mButtonPause.setVisibility(View.INVISIBLE);
break;
default:
mButtonLighting.setVisibility(View.INVISIBLE);
mButtonWireframe.setVisibility(View.INVISIBLE);
@@ -1357,6 +1379,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
}
public boolean onOptionsItemSelected(MenuItem item) {
resetNoTouchTimer();
if(!DISABLE_LOG) Log.i(TAG, "called onOptionsItemSelected; selected item: " + item);
int itemId = item.getItemId();
if (itemId == R.id.post_processing_standard)
@@ -1775,9 +1798,22 @@ public class RTABMapActivity extends Activity implements OnClickListener {
long mb = filePath.length()/(1024*1024);
filesWithSize[i] = files[i] + " ("+mb+" MB)";
}
ArrayList<HashMap<String, String> > arrayList = new ArrayList<HashMap<String, String> >();
for (int i = 0; i < filesWithSize.length; i++) {
HashMap<String, String> hashMap = new HashMap<String, String>();//create a hashmap to store the data in key value pair
hashMap.put("name", filesWithSize[i]);
hashMap.put("path", mWorkingDirectory + files[i]);
arrayList.add(hashMap);//add the hashmap into arrayList
}
String[] from = {"name", "path"};//string array
int[] to = {R.id.textView, R.id.imageView};//int array of views id's
DatabaseListArrayAdapter simpleAdapter = new DatabaseListArrayAdapter(this, arrayList, R.layout.database_list, from, to);//Create object and set the parameters for simpleAdapter
AlertDialog.Builder builder = new AlertDialog.Builder(this);
builder.setTitle("Choose Your File (*.db)");
builder.setItems(filesWithSize, new DialogInterface.OnClickListener() {
builder.setAdapter(simpleAdapter, new DialogInterface.OnClickListener() {
//builder.setItems(filesWithSize, new DialogInterface.OnClickListener() {
public void onClick(DialogInterface dialog, final int which) {
// Adjust color now?
@@ -1912,7 +1948,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
public void onClick(DialogInterface dialog, int which) {
mExportedOBJ = isOBJ;
resetNoTouchTimer();
mSavedRenderingType = !meshing?0:!isOBJ?1:2;
mSavedRenderingType = mItemRenderingPointCloud.isChecked()?0:mItemRenderingMesh.isChecked()?1:2;
if(!meshing)
{
mItemRenderingPointCloud.setChecked(true);
@@ -2206,6 +2242,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
{
mOpenedDatabasePath = mWorkingDirectory + fileName;
Log.i(TAG, "Open database " + mOpenedDatabasePath);
SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(getActivity());
final boolean databaseInMemory = sharedPref.getBoolean(getString(R.string.pref_key_db_in_memory), Boolean.parseBoolean(getString(R.string.pref_default_db_in_memory)));
@@ -2218,27 +2256,55 @@ public class RTABMapActivity extends Activity implements OnClickListener {
Thread openThread = new Thread(new Runnable() {
public void run() {
final String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;
(new File(tmpDatabase)).delete();
try{
copy(new File(mOpenedDatabasePath), new File(tmpDatabase));
}
catch(IOException e)
{
mToast.makeText(getActivity(), String.format("Failed to create temp database from %s!", mOpenedDatabasePath), mToast.LENGTH_LONG).show();
updateState(State.STATE_IDLE);
mProgressDialog.dismiss();
return;
SQLiteDatabase db = null;
try {
db = SQLiteDatabase.openDatabase(mOpenedDatabasePath, null, SQLiteDatabase.OPEN_READONLY);
// get version
Cursor c1 = db.rawQuery("SELECT version FROM Admin", null);
if(c1.moveToFirst()) {
String version = c1.getString(c1.getColumnIndex("version"));
Log.i(TAG, "Version="+version);
if(Util.versionCompare(version, "0.13.0") >= 0) {
Cursor c2 = db.rawQuery("SELECT version FROM Admin WHERE opt_cloud is not null", null);
if(c2.moveToFirst()) {
Log.i(TAG, "Found optimized mesh");
runOnUiThread(new Runnable() {
public void run() {
mProgressDialog.dismiss();
updateState(State.STATE_VISUALIZING_WHILE_LOADING);
mToast.makeText(getActivity(), String.format("Optimized mesh detected in the database. It will be shown while the database is loading..."), mToast.LENGTH_LONG).show();
}
});
}
else
{
Log.i(TAG, "Not found optimized mesh");
}
}
}
else
{
Log.e(TAG, "Failed getting version from database");
}
} catch (Exception e) {
Log.e(TAG, e.getMessage());
}
finally {
if(db != null && db.isOpen()) {
db.close();
}
}
final int status = RTABMapLib.openDatabase(tmpDatabase, databaseInMemory, optimize);
final String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;
final int status = RTABMapLib.openDatabase2(mOpenedDatabasePath, tmpDatabase, databaseInMemory, optimize);
runOnUiThread(new Runnable() {
public void run() {
setCamera(1);
updateState(State.STATE_IDLE);
if(status == -1)
{
updateState(State.STATE_IDLE);
mProgressDialog.dismiss();
new AlertDialog.Builder(getActivity())
.setCancelable(false)
@@ -2262,6 +2328,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
}
else if(status == -2)
{
updateState(State.STATE_IDLE);
mProgressDialog.dismiss();
new AlertDialog.Builder(getActivity())
.setCancelable(false)
@@ -2283,12 +2350,35 @@ public class RTABMapActivity extends Activity implements OnClickListener {
}
else
{
// creating meshes...
if(!mItemTrajectoryMode.isChecked())
if(status >= 1 && status<=3)
{
mProgressDialog.dismiss();
resetNoTouchTimer();
mSavedRenderingType = mItemRenderingPointCloud.isChecked()?0:mItemRenderingMesh.isChecked()?1:2;
if(status==1)
{
mItemRenderingPointCloud.setChecked(true);
}
else if(status==2)
{
mItemRenderingMesh.setChecked(true);
}
else // isOBJ
{
mItemRenderingTextureMesh.setChecked(true);
}
updateState(State.STATE_VISUALIZING);
mToast.makeText(getActivity(), String.format("Database loaded!"), mToast.LENGTH_LONG).show();
}
else if(!mItemTrajectoryMode.isChecked())
{
setCamera(1);
// creating meshes...
updateState(State.STATE_IDLE);
mProgressDialog.setTitle("Loading");
mProgressDialog.setMessage(String.format("Database \"%s\" loaded. Please wait while rendering point clouds and meshes...", fileName));
}
}
}
});
app/android/src/com/introlab/rtabmap/RTABMapLib.java
+1
View File
@@ -28,6 +28,7 @@ public class RTABMapLib
public static native void setScreenRotation(int displayRotation, int cameraRotation);
public static native int openDatabase(String databasePath, boolean databaseInMemory, boolean optimize);
public static native int openDatabase2(String databaseSource, String databasePath, boolean databaseInMemory, boolean optimize);
/*
* Called when the Tango service is connected.
app/android/src/com/introlab/rtabmap/Util.java
+33
View File
@@ -90,4 +90,37 @@ public class Util {
return fileList;
}
/**
* https://stackoverflow.com/questions/6701948/efficient-way-to-compare-version-strings-in-java
* Compares two version strings.
*
* Use this instead of String.compareTo() for a non-lexicographical
* comparison that works for version strings. e.g. "1.10".compareTo("1.6").
*
* @note It does not work if "1.10" is supposed to be equal to "1.10.0".
*
* @param str1 a string of ordinal numbers separated by decimal points.
* @param str2 a string of ordinal numbers separated by decimal points.
* @return The result is a negative integer if str1 is _numerically_ less than str2.
* The result is a positive integer if str1 is _numerically_ greater than str2.
* The result is zero if the strings are _numerically_ equal.
*/
public static int versionCompare(String str1, String str2) {
String[] vals1 = str1.split("\\.");
String[] vals2 = str2.split("\\.");
int i = 0;
// set index to first non-equal ordinal or length of shortest version string
while (i < vals1.length && i < vals2.length && vals1[i].equals(vals2[i])) {
i++;
}
// compare first non-equal ordinal number
if (i < vals1.length && i < vals2.length) {
int diff = Integer.valueOf(vals1[i]).compareTo(Integer.valueOf(vals2[i]));
return Integer.signum(diff);
}
// the strings are equal or one string is a substring of the other
// e.g. "1.2.3" = "1.2.3" or "1.2.3" < "1.2.3.4"
return Integer.signum(vals1.length - vals2.length);
}
}
corelib/include/rtabmap/core/DBDriver.h
+26
View File
@@ -98,6 +98,19 @@ public:
public:
void addInfoAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize, const ParametersMap & parameters) const;
void addStatistics(const Statistics & statistics) const;
void savePreviewImage(const cv::Mat & image) const;
cv::Mat loadPreviewImage() const;
void saveOptimizedMesh(
const cv::Mat & cloud,
const std::map<int, Transform> & poses = std::map<int, Transform>(), // if we want to do localization afterward using optimized mesh
const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(), // Textures -> polygons -> vertices
const std::vector<std::vector<Eigen::Vector2f> > & texCoords = std::vector<std::vector<Eigen::Vector2f> >(), // Textures -> uv coords for each vertex of the polygons
const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size);
cv::Mat loadOptimizedMesh(
std::map<int, Transform> * poses = 0,
std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
cv::Mat * textures = 0) const;
public:
// Mutex-protected methods of abstract versions below
@@ -200,6 +213,19 @@ private:
const cv::Mat & image) const = 0;
virtual void addStatisticsQuery(const Statistics & statistics) const = 0;
virtual void savePreviewImageQuery(const cv::Mat & image) const = 0;
virtual cv::Mat loadPreviewImageQuery() const = 0;
virtual void saveOptimizedMeshQuery(
const cv::Mat & cloud,
const std::map<int, Transform> & poses,
const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
const cv::Mat & textures) const = 0;
virtual cv::Mat loadOptimizedMeshQuery(
std::map<int, Transform> * poses,
std::vector<std::vector<std::vector<unsigned int> > > * polygons,
std::vector<std::vector<Eigen::Vector2f> > * texCoords,
cv::Mat * textures) const = 0;
// Load objects
virtual void loadQuery(VWDictionary * dictionary) const = 0;
corelib/include/rtabmap/core/Memory.h
+18
View File
@@ -43,6 +43,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
namespace pcl
{
class TextureMesh;
}
namespace rtabmap {
class Signature;
@@ -92,6 +97,19 @@ public:
int cleanup();
void saveStatistics(const Statistics & statistics);
void savePreviewImage(const cv::Mat & image) const;
cv::Mat loadPreviewImage() const;
void saveOptimizedMesh(
const cv::Mat & cloud,
const std::map<int, Transform> & poses = std::map<int, Transform>(), // if we want to do localization afterward using optimized mesh
const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(), // Textures -> polygons -> vertices
const std::vector<std::vector<Eigen::Vector2f> > & texCoords = std::vector<std::vector<Eigen::Vector2f> >(), // Textures -> uv coords for each vertex of the polygons
const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size)
cv::Mat loadOptimizedMesh(
std::map<int, Transform> * poses = 0,
std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
cv::Mat * textures = 0) const;
void emptyTrash();
void joinTrashThread();
bool addLink(const Link & link, bool addInDatabase = false);
corelib/include/rtabmap/core/Rtabmap.h
+5
View File
@@ -41,6 +41,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <stack>
#include <set>
namespace pcl
{
class TextureMesh;
}
namespace rtabmap
{
corelib/include/rtabmap/core/util3d.h
+12 -4
View File
@@ -190,14 +190,16 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImages(
float maxDepth,
float minDepth);
// return CV_32FC3
// return CV_32FC3 (x,y,z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
// return CV_32FC6
// return CV_32FC6 (x,y,z,normal_z,normal_y,normalz)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform());
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
// return CV_32FC4
// return CV_32FC4 (x,y,z,rgb)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform());
// return CV_32FC2
// return CV_32FC7 (x,y,z,rgb,normal_z,normal_y,normalz)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform());
// return CV_32FC2 (x,y)
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
// For laserScan of type CV_32FC2, z is set to null.
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform = Transform());
@@ -205,6 +207,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const cv::Mat & laserScan, const Transform & transform = Transform());
// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters.
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const cv::Mat & laserScan, const Transform & transform = Transform(), unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters.
// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP laserScanToPointCloudRGBNormal(const cv::Mat & laserScan, const Transform & transform = Transform(), unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
// For laserScan of type CV_32FC2, z is set to null.
pcl::PointXYZ RTABMAP_EXP laserScanToPoint(const cv::Mat & laserScan, int index);
@@ -212,6 +217,9 @@ pcl::PointXYZ RTABMAP_EXP laserScanToPoint(const cv::Mat & laserScan, int index)
pcl::PointNormal RTABMAP_EXP laserScanToPointNormal(const cv::Mat & laserScan, int index);
// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters.
pcl::PointXYZRGB RTABMAP_EXP laserScanToPointRGB(const cv::Mat & laserScan, int index, unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters.
// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.
pcl::PointXYZRGBNormal RTABMAP_EXP laserScanToPointRGBNormal(const cv::Mat & laserScan, int index, unsigned char r, unsigned char g, unsigned char b);
void RTABMAP_EXP getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max);
void RTABMAP_EXP getMinMax3D(const cv::Mat & laserScan, pcl::PointXYZ & min, pcl::PointXYZ & max);
corelib/include/rtabmap/core/util3d_surface.h
+2 -2
View File
@@ -177,9 +177,9 @@ void RTABMAP_EXP concatenateTextureMaterials(
/**
* Merge all textures in the mesh into "textureCount" textures of size "textureSize".
* @return merged textures corresponding to new materials set in TextureMesh
* @return merged textures corresponding to new materials set in TextureMesh (height=textureSize, width=textureSize*materials)
*/
std::vector<cv::Mat> RTABMAP_EXP mergeTextures(
cv::Mat RTABMAP_EXP mergeTextures(
pcl::TextureMesh & mesh,
const std::map<int, cv::Mat> & images, // raw or compressed, can be empty if memory or dbDriver should be used
const std::map<int, std::vector<CameraModel> > & calibrations, // Should match images
corelib/include/rtabmap/core/util3d_transforms.h
+3
View File
@@ -94,6 +94,9 @@ pcl::PointXYZRGB RTABMAP_EXP transformPoint(
pcl::PointNormal RTABMAP_EXP transformPoint(
const pcl::PointNormal & point,
const Transform & transform);
pcl::PointXYZRGBNormal RTABMAP_EXP transformPoint(
const pcl::PointXYZRGBNormal & point,
const Transform & transform);
} // namespace util3d
} // namespace rtabmap
corelib/src/DBDriver.cpp
+39
View File
@@ -1024,6 +1024,45 @@ void DBDriver::addStatistics(const Statistics & statistics) const
_dbSafeAccessMutex.unlock();
}
void DBDriver::savePreviewImage(const cv::Mat & image) const
{
_dbSafeAccessMutex.lock();
savePreviewImageQuery(image);
_dbSafeAccessMutex.unlock();
}
cv::Mat DBDriver::loadPreviewImage() const
{
_dbSafeAccessMutex.lock();
cv::Mat image = loadPreviewImageQuery();
_dbSafeAccessMutex.unlock();
return image;
}
void DBDriver::saveOptimizedMesh(
const cv::Mat & cloud,
const std::map<int, Transform> & poses,
const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
const cv::Mat & textures) const
{
_dbSafeAccessMutex.lock();
saveOptimizedMeshQuery(cloud, poses, polygons, texCoords, textures);
_dbSafeAccessMutex.unlock();
}
cv::Mat DBDriver::loadOptimizedMesh(
std::map<int, Transform> * poses,
std::vector<std::vector<std::vector<unsigned int> > > * polygons,
std::vector<std::vector<Eigen::Vector2f> > * texCoords,
cv::Mat * textures) const
{
_dbSafeAccessMutex.lock();
cv::Mat cloud = loadOptimizedMeshQuery(poses, polygons, texCoords, textures);
_dbSafeAccessMutex.unlock();
return cloud;
}
void DBDriver::generateGraph(
const std::string & fileName,
const std::set<int> & idsInput,
corelib/src/DBDriverSqlite3.cpp
+471
View File
@@ -3728,6 +3728,477 @@ void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const
}
}
void DBDriverSqlite3::savePreviewImageQuery(const cv::Mat & image) const
{
UDEBUG("");
if(_ppDb && uStrNumCmp(_version, "0.12.0") >= 0)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::string query;
// Update table Admin
query = uFormat("UPDATE Admin SET preview_image=? WHERE version='%s';", _version.c_str());
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
int index = 1;
cv::Mat compressedImage;
if(image.empty())
{
rc = sqlite3_bind_null(ppStmt, index);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
// compress
if(image.rows == 1 && image.type() == CV_8UC1)
{
// already compressed
compressedImage = image;
}
else
{
compressedImage = compressImage2(image, ".jpg");
}
rc = sqlite3_bind_blob(ppStmt, index++, compressedImage.data, compressedImage.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
//execute query
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
}
}
cv::Mat DBDriverSqlite3::loadPreviewImageQuery() const
{
UDEBUG("");
cv::Mat image;
if(_ppDb && uStrNumCmp(_version, "0.12.0") >= 0)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT preview_image "
<< "FROM Admin "
<< "WHERE version='" << _version.c_str()
<<"';";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
rc = sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_ROW, uFormat("DB error (%s): Not found first Admin row: query=\"%s\"", _version.c_str(), query.str().c_str()).c_str());
if(rc == SQLITE_ROW)
{
const void * data = 0;
int dataSize = 0;
int index = 0;
//opt_cloud
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && data)
{
image = uncompressImage(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
}
UDEBUG("Image=%dx%d", image.cols, image.rows);
rc = sqlite3_step(ppStmt); // next result...
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%fs", timer.ticks());
}
return image;
}
void DBDriverSqlite3::saveOptimizedMeshQuery(
const cv::Mat & cloud,
const std::map<int, Transform> & poses,
const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
const cv::Mat & textures) const
{
UDEBUG("");
if(_ppDb && uStrNumCmp(_version, "0.13.0") >= 0)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::string query;
// Update table Admin
query = uFormat("UPDATE Admin SET opt_cloud=?, opt_ids=?, opt_poses=?, opt_polygons_size=?, opt_polygons=?, opt_tex_coords=?, opt_tex_materials=?, time_enter = DATETIME('NOW') WHERE version='%s';", _version.c_str());
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(cloud.empty())
{
// set all fields to null
for(int i=1; i<=7; ++i)
{
rc = sqlite3_bind_null(ppStmt, i);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
//execute query
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
int index = 1;
// compress and save cloud
cv::Mat compressedCloud;
if(cloud.rows == 1 && cloud.type() == CV_8UC1)
{
// already compressed
compressedCloud = cloud;
}
else
{
compressedCloud = compressData2(cloud);
}
rc = sqlite3_bind_blob(ppStmt, index++, compressedCloud.data, compressedCloud.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// opt ids and poses
cv::Mat compressedIds;
cv::Mat compressedPoses;
cv::Mat compressedPolygons;
cv::Mat compressedTexCoords;
cv::Mat compressedTextures;
if(poses.empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
std::vector<int> serializedIds(poses.size());
std::vector<float> serializedPoses(poses.size()*12);
int i=0;
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
serializedIds[i] = iter->first;
memcpy(serializedPoses.data()+(12*sizeof(float)*i), iter->second.data(), 12*sizeof(float));
}
compressedIds = compressData2(cv::Mat(1,serializedIds.size(), CV_32SC1, serializedIds.data()));
compressedPoses = compressData2(cv::Mat(1,serializedPoses.size(), CV_32FC1, serializedPoses.data()));
rc = sqlite3_bind_blob(ppStmt, index++, compressedIds.data, compressedIds.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_blob(ppStmt, index++, compressedPoses.data, compressedPoses.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
// polygons
if(polygons.empty())
{
//polygon size
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// polygons
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// tex_coords
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// materials
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
std::vector<int> serializedPolygons;
std::vector<float> serializedTexCoords;
int polygonSize = 0;
int totalPolygonIndices = 0;
UASSERT(texCoords.empty() || polygons.size() == texCoords.size());
for(unsigned int t=0; t<polygons.size(); ++t)
{
unsigned int materialPolygonIndices = 0;
for(unsigned int p=0; p<polygons[t].size(); ++p)
{
if(polygonSize == 0)
{
UASSERT(polygons[t][p].size());
polygonSize = polygons[t][p].size();
}
else
{
UASSERT(polygonSize == (int)polygons[t][p].size());
}
materialPolygonIndices += polygons[t][p].size();
}
totalPolygonIndices += materialPolygonIndices;
if(!texCoords.empty())
{
UASSERT(materialPolygonIndices == texCoords[t].size());
}
}
UASSERT(totalPolygonIndices>0);
serializedPolygons.resize(totalPolygonIndices+polygons.size());
if(!texCoords.empty())
{
serializedTexCoords.resize(totalPolygonIndices*2+polygons.size());
}
int oi=0;
int ci=0;
for(unsigned int t=0; t<polygons.size(); ++t)
{
serializedPolygons[oi++] = polygons[t].size();
if(!texCoords.empty())
{
serializedTexCoords[ci++] = texCoords[t].size();
}
for(unsigned int p=0; p<polygons[t].size(); ++p)
{
int texIndex = p*polygonSize;
for(unsigned int i=0; i<polygons[t][p].size(); ++i)
{
serializedPolygons[oi++] = polygons[t][p][i];
if(!texCoords.empty())
{
serializedTexCoords[ci++] = texCoords[t][texIndex+i][0];
serializedTexCoords[ci++] = texCoords[t][texIndex+i][1];
}
}
}
}
compressedPolygons = compressData2(cv::Mat(1,serializedPolygons.size(), CV_32SC1, serializedPolygons.data()));
// polygon size
rc = sqlite3_bind_int(ppStmt, index++, polygonSize);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_blob(ppStmt, index++, compressedPolygons.data, compressedPolygons.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// tex coords
if(texCoords.empty())
{
// tex coords
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// materials
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
compressedTexCoords = compressData2(cv::Mat(1,serializedTexCoords.size(), CV_32FC1, serializedTexCoords.data()));
rc = sqlite3_bind_blob(ppStmt, index++, compressedTexCoords.data, compressedTexCoords.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UASSERT(!textures.empty() && textures.cols % textures.rows == 0 && textures.cols/textures.rows == (int)texCoords.size());
if(textures.rows == 1 && textures.type() == CV_8UC1)
{
//already compressed
compressedTextures = textures;
}
else
{
compressedTextures = compressImage2(textures, ".jpg");
}
rc = sqlite3_bind_blob(ppStmt, index++, compressedTextures.data, compressedTextures.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
//execute query
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
}
}
cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
std::map<int, Transform> * poses,
std::vector<std::vector<std::vector<unsigned int> > > * polygons,
std::vector<std::vector<Eigen::Vector2f> > * texCoords,
cv::Mat * textures) const
{
UDEBUG("");
cv::Mat cloud;
if(_ppDb && uStrNumCmp(_version, "0.13.0") >= 0)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT opt_cloud, opt_ids, opt_poses, opt_polygons_size, opt_polygons, opt_tex_coords, opt_tex_materials "
<< "FROM Admin "
<< "WHERE version='" << _version.c_str()
<<"';";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
rc = sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_ROW, uFormat("DB error (%s): Not found first Admin row: query=\"%s\"", _version.c_str(), query.str().c_str()).c_str());
if(rc == SQLITE_ROW)
{
const void * data = 0;
int dataSize = 0;
int index = 0;
//opt_cloud
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && data)
{
cloud = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
}
UDEBUG("Cloud=%d points", cloud.cols);
//opt_poses
cv::Mat serializedIds;
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && data)
{
serializedIds = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
UDEBUG("serializedIds=%d", serializedIds.cols);
}
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && data)
{
cv::Mat serializedPoses = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
UDEBUG("serializedPoses=%d", serializedPoses.cols);
if(poses)
{
UASSERT(serializedIds.cols == serializedPoses.cols/12);
}
}
//opt_polygons_size
int polygonSize = sqlite3_column_int(ppStmt, index++);
UDEBUG("polygonSize=%d", polygonSize);
//opt_polygons
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && data)
{
UASSERT(polygonSize > 0);
if(polygons)
{
cv::Mat serializedPolygons = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
UDEBUG("serializedPolygons=%d", serializedPolygons.cols);
UASSERT(serializedPolygons.total());
for(int t=0; t<serializedPolygons.cols; ++t)
{
UASSERT(serializedPolygons.at<int>(t) > 0);
std::vector<std::vector<unsigned int> > materialPolygons(serializedPolygons.at<int>(t), std::vector<unsigned int>(polygonSize));
++t;
UASSERT(t < serializedPolygons.cols);
UDEBUG("materialPolygons=%d", (int)materialPolygons.size());
for(int p=0; p<(int)materialPolygons.size(); ++p)
{
for(int i=0; i<polygonSize; ++i)
{
materialPolygons[p][i] = serializedPolygons.at<int>(t + p*polygonSize + i);
}
}
t+=materialPolygons.size()*polygonSize;
polygons->push_back(materialPolygons);
}
}
//opt_tex_coords
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && data)
{
if(texCoords)
{
cv::Mat serializedTexCoords = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
UDEBUG("serializedTexCoords=%d", serializedTexCoords.cols);
UASSERT(serializedTexCoords.total());
for(int t=0; t<serializedTexCoords.cols; ++t)
{
UASSERT(int(serializedTexCoords.at<float>(t)) > 0);
std::vector<Eigen::Vector2f> materialtexCoords(int(serializedTexCoords.at<float>(t)));
++t;
UASSERT(t < serializedTexCoords.cols);
UDEBUG("materialtexCoords=%d", (int)materialtexCoords.size());
for(int p=0; p<(int)materialtexCoords.size(); ++p)
{
materialtexCoords[p][0] = serializedTexCoords.at<float>(t + p*2);
materialtexCoords[p][1] = serializedTexCoords.at<float>(t + p*2 + 1);
}
t+=materialtexCoords.size()*2;
texCoords->push_back(materialtexCoords);
}
}
//opt_tex_materials
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && data)
{
if(textures)
{
*textures = uncompressImage(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
UDEBUG("textures=%dx%d", textures->cols, textures->rows);
}
}
}
}
rc = sqlite3_step(ppStmt); // next result...
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%fs", timer.ticks());
}
return cloud;
}
std::string DBDriverSqlite3::queryStepNode() const
{
if(uStrNumCmp(_version, "0.13.0") >= 0)
corelib/src/DBDriverSqlite3.h
+13
View File
@@ -95,6 +95,19 @@ private:
const cv::Mat & image) const;
virtual void addStatisticsQuery(const Statistics & statistics) const;
virtual void savePreviewImageQuery(const cv::Mat & image) const;
virtual cv::Mat loadPreviewImageQuery() const;
virtual void saveOptimizedMeshQuery(
const cv::Mat & cloud,
const std::map<int, Transform> & poses,
const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
const cv::Mat & textures) const;
virtual cv::Mat loadOptimizedMeshQuery(
std::map<int, Transform> * poses,
std::vector<std::vector<std::vector<unsigned int> > > * polygons,
std::vector<std::vector<Eigen::Vector2f> > * texCoords,
cv::Mat * textures) const;
// Load objects
virtual void loadQuery(VWDictionary * dictionary) const;
corelib/src/Memory.cpp
+42
View File
@@ -1651,6 +1651,48 @@ void Memory::saveStatistics(const Statistics & statistics)
}
}
void Memory::savePreviewImage(const cv::Mat & image) const
{
if(_dbDriver)
{
_dbDriver->savePreviewImage(image);
}
}
cv::Mat Memory::loadPreviewImage() const
{
if(_dbDriver)
{
return _dbDriver->loadPreviewImage();
}
return cv::Mat();
}
void Memory::saveOptimizedMesh(
const cv::Mat & cloud,
const std::map<int, Transform> & poses,
const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
const cv::Mat & textures) const
{
if(_dbDriver)
{
_dbDriver->saveOptimizedMesh(cloud, poses, polygons, texCoords, textures);
}
}
cv::Mat Memory::loadOptimizedMesh(
std::map<int, Transform> * poses,
std::vector<std::vector<std::vector<unsigned int> > > * polygons,
std::vector<std::vector<Eigen::Vector2f> > * texCoords,
cv::Mat * textures) const
{
if(_dbDriver)
{
return _dbDriver->loadOptimizedMesh(poses, polygons, texCoords, textures);
}
return cv::Mat();
}
void Memory::emptyTrash()
{
if(_dbDriver)
corelib/src/Rtabmap.cpp
+1
View File
@@ -50,6 +50,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/filters/crop_box.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/common.h>
#include <pcl/TextureMesh.h>
#include <stdlib.h>
#include <set>
corelib/src/resources/DatabaseSchema.sql.in
+10 -1
View File
@@ -104,7 +104,16 @@ CREATE TABLE Statistics (
CREATE TABLE Admin (
version TEXT,
preview_image BLOB,
preview_image BLOB, -- compressed image
opt_cloud BLOB, -- compressed data
opt_ids BLOB, -- Node ids used to generate the optimized cloud/mesh
opt_poses BLOB, -- compressed N*3x4 float
opt_polygons_size INTEGER, -- e.g., 3
opt_polygons BLOB, -- compressed data [length_v0, i0,i1,i3, length_v1, i0,i1,i3]
opt_tex_coords BLOB, -- compressed data [length_v0, u0,v0,u1,v1,u2,v2, length_v1, u0,v0,u1,v1,u2,v2]
opt_tex_materials BLOB, -- compressed image
time_enter DATE
);
corelib/src/util3d.cpp
+120 -10
View File
@@ -1299,6 +1299,38 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
return laserScan;
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
bool nullTransform = transform.isNull() || transform.isIdentity();
for(unsigned int i=0; i<cloud.size(); ++i)
{
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointXYZRGBNormal pt = util3d::transformPoint(cloud.at(i), transform);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
ptr[4] = pt.normal_x;
ptr[5] = pt.normal_y;
ptr[6] = pt.normal_z;
}
else
{
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
ptr[2] = cloud.at(i).z;
ptr[4] = cloud.at(i).normal_x;
ptr[5] = cloud.at(i).normal_y;
ptr[6] = cloud.at(i).normal_z;
}
int * ptrInt = (int*)ptr;
ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
}
return laserScan;
}
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
@@ -1325,7 +1357,7 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
output->resize(laserScan.cols);
@@ -1344,7 +1376,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserS
pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat & laserScan, const Transform & transform)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
output->resize(laserScan.cols);
@@ -1362,7 +1394,7 @@ pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat
pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const cv::Mat & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
output->resize(laserScan.cols);
@@ -1379,10 +1411,28 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const cv::Mat &
return output;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(const cv::Mat & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
output->resize(laserScan.cols);
bool nullTransform = transform.isNull() || transform.isIdentity();
for(int i=0; i<laserScan.cols; ++i)
{
output->at(i) = util3d::laserScanToPointRGBNormal(laserScan, i, r, g, b);
if(!nullTransform)
{
output->at(i) = util3d::transformPoint(output->at(i), transform);
}
}
return output;
}
pcl::PointXYZ laserScanToPoint(const cv::Mat & laserScan, int index)
{
UASSERT(!laserScan.empty() && index < laserScan.cols);
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointXYZ output;
const float * ptr = laserScan.ptr<float>(0, index);
output.x = ptr[0];
@@ -1397,7 +1447,7 @@ pcl::PointXYZ laserScanToPoint(const cv::Mat & laserScan, int index)
pcl::PointNormal laserScanToPointNormal(const cv::Mat & laserScan, int index)
{
UASSERT(!laserScan.empty() && index < laserScan.cols);
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointNormal output;
const float * ptr = laserScan.ptr<float>(0, index);
output.x = ptr[0];
@@ -1412,13 +1462,19 @@ pcl::PointNormal laserScanToPointNormal(const cv::Mat & laserScan, int index)
output.normal_y = ptr[4];
output.normal_z = ptr[5];
}
else if(laserScan.channels() == 7)
{
output.normal_x = ptr[4];
output.normal_y = ptr[5];
output.normal_z = ptr[6];
}
return output;
}
pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
{
UASSERT(!laserScan.empty() && index < laserScan.cols);
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointXYZRGB output;
const float * ptr = laserScan.ptr<float>(0, index);
output.x = ptr[0];
@@ -1427,7 +1483,47 @@ pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index, unsig
{
output.z = ptr[2];
}
if(laserScan.channels() == 4)
if(laserScan.channels() == 4 || laserScan.channels() == 7)
{
int * ptrInt = (int*)ptr;
output.b = (unsigned char)(ptrInt[3] & 0xFF);
output.g = (unsigned char)((ptrInt[3] >> 8) & 0xFF);
output.r = (unsigned char)((ptrInt[3] >> 16) & 0xFF);
}
else
{
output.r = r;
output.g = g;
output.b = b;
}
return output;
}
pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const cv::Mat & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
{
UASSERT(!laserScan.empty() && index < laserScan.cols);
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointXYZRGBNormal output;
const float * ptr = laserScan.ptr<float>(0, index);
output.x = ptr[0];
output.y = ptr[1];
if(laserScan.channels() >= 3)
{
output.z = ptr[2];
}
if(laserScan.channels() == 6)
{
output.normal_x = ptr[3];
output.normal_y = ptr[4];
output.normal_z = ptr[5];
}
else if(laserScan.channels() == 7)
{
output.normal_x = ptr[4];
output.normal_y = ptr[5];
output.normal_z = ptr[6];
}
if(laserScan.channels() == 4 || laserScan.channels() == 7)
{
int * ptrInt = (int*)ptr;
output.b = (unsigned char)(ptrInt[3] & 0xFF);
@@ -1446,7 +1542,7 @@ pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index, unsig
void getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max)
{
UASSERT(!laserScan.empty());
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
const float * ptr = laserScan.ptr<float>(0, 0);
min.x = max.x = ptr[0];
@@ -1529,7 +1625,7 @@ cv::Mat projectCloudToCamera(
{
UASSERT(!cameraTransform.isNull());
UASSERT(!laserScan.empty());
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6));
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
float fx = cameraMatrixK.at<double>(0,0);
@@ -1542,7 +1638,9 @@ cv::Mat projectCloudToCamera(
const cv::Vec2f* vec2Ptr = laserScan.ptr<cv::Vec2f>();
const cv::Vec3f* vec3Ptr = laserScan.ptr<cv::Vec3f>();
const cv::Vec4f* vec4Ptr = laserScan.ptr<cv::Vec4f>();
const cv::Vec6f* vec6Ptr = laserScan.ptr<cv::Vec6f>();
const float* vec7Ptr = laserScan.ptr<float>();
int count = 0;
for(int i=0; i<laserScan.cols; ++i)
@@ -1561,12 +1659,24 @@ cv::Mat projectCloudToCamera(
ptScan.y = vec3Ptr[i][1];
ptScan.z = vec3Ptr[i][2];
}
else
else if(laserScan.type() == CV_32FC(4))
{
ptScan.x = vec4Ptr[i][0];
ptScan.y = vec4Ptr[i][1];
ptScan.z = vec4Ptr[i][2];
}
else if(laserScan.type() == CV_32FC(6))
{
ptScan.x = vec6Ptr[i][0];
ptScan.y = vec6Ptr[i][1];
ptScan.z = vec6Ptr[i][2];
}
else // 7f
{
ptScan.x = (vec7Ptr+i*7)[0];
ptScan.y = (vec7Ptr+i*7)[1];
ptScan.z = (vec7Ptr+i*7)[2];
}
ptScan = util3d::transformPoint(ptScan, t);
// re-project in camera frame
corelib/src/util3d_surface.cpp
+32 -33
View File
@@ -1194,7 +1194,8 @@ double sqr(uchar v)
{
return double(v)*double(v);
}
std::vector<cv::Mat> mergeTextures(
cv::Mat mergeTextures(
pcl::TextureMesh & mesh,
const std::map<int, cv::Mat> & images,
const std::map<int, std::vector<CameraModel> > & calibrations,
@@ -1216,7 +1217,7 @@ std::vector<cv::Mat> mergeTextures(
//get texture size, if disabled use default 1024
UASSERT(textureSize%256 == 0);
UDEBUG("textureSize = %d", textureSize);
std::vector<cv::Mat> globalTextures;
cv::Mat globalTextures;
if(mesh.tex_materials.size() > 1)
{
std::vector<std::pair<int, int> > textures(mesh.tex_materials.size(), std::pair<int, int>(-1,-1));
@@ -1346,13 +1347,8 @@ std::vector<cv::Mat> mergeTextures(
int cols = float(textureSize)/(scale*imageSize.width);
int rows = float(textureSize)/(scale*imageSize.height);
std::vector<cv::Mat> globalTextureMasks(materials);
globalTextures.resize(materials);
for(int i=0; i<materials; ++i)
{
globalTextures[i] = cv::Mat(textureSize, textureSize, imageType, cv::Scalar::all(255));
globalTextureMasks[i] = cv::Mat(textureSize, textureSize, CV_8UC1, cv::Scalar::all(0));
}
globalTextures = cv::Mat(textureSize, materials*textureSize, imageType, cv::Scalar::all(255));
cv::Mat globalTextureMasks = cv::Mat(textureSize, materials*textureSize, CV_8UC1, cv::Scalar::all(0));
// used for multi camera texturing, to avoid reloading same texture for sub cameras
cv::Mat previousImage;
@@ -1440,13 +1436,13 @@ std::vector<cv::Mat> mergeTextures(
cv::cvtColor(resizedImage, resizedImageColor, CV_GRAY2BGR);
resizedImage = resizedImageColor;
}
UASSERT(resizedImage.type() == globalTextures[indexMaterial].type());
resizedImage.copyTo(globalTextures[indexMaterial](cv::Rect(u, v, resizedImage.cols, resizedImage.rows)));
emptyImageMask.copyTo(globalTextureMasks[indexMaterial](cv::Rect(u, v, resizedImage.cols, resizedImage.rows)));
UASSERT(resizedImage.type() == globalTextures.type());
resizedImage.copyTo(globalTextures(cv::Rect(u+indexMaterial*globalTextures.rows, v, resizedImage.cols, resizedImage.rows)));
emptyImageMask.copyTo(globalTextureMasks(cv::Rect(u+indexMaterial*globalTextureMasks.rows, v, resizedImage.cols, resizedImage.rows)));
}
else
{
emptyImage.copyTo(globalTextures[indexMaterial](cv::Rect(u, v, emptyImage.cols, emptyImage.rows)));
emptyImage.copyTo(globalTextures(cv::Rect(u+indexMaterial*globalTextures.rows, v, emptyImage.cols, emptyImage.rows)));
}
++oi;
}
@@ -1455,7 +1451,7 @@ std::vector<cv::Mat> mergeTextures(
{
if(state->isCanceled())
{
return std::vector<cv::Mat>();
return cv::Mat();
}
state->callback(uFormat("Assembled texture %d/%d.", t+1, (int)textures.size()));
}
@@ -1510,8 +1506,8 @@ std::vector<cv::Mat> mergeTextures(
int vi = (1.0-iter->second.y)*emptyImage.rows + imageOrigin[iter->first].y;
int uj = jter->second.x*emptyImage.cols + imageOrigin[jter->first].x;
int vj = (1.0-jter->second.y)*emptyImage.rows + imageOrigin[jter->first].y;
cv::Vec3b * pt1 = globalTextures[indexMaterial].ptr<cv::Vec3b>(vi,ui);
cv::Vec3b * pt2 = globalTextures[indexMaterial].ptr<cv::Vec3b>(vj,uj);
cv::Vec3b * pt1 = globalTextures.ptr<cv::Vec3b>(vi,ui+indexMaterial*globalTextures.rows);
cv::Vec3b * pt2 = globalTextures.ptr<cv::Vec3b>(vj,uj+indexMaterial*globalTextures.rows);
I(i, j) += std::sqrt(static_cast<double>(sqr(pt1->val[0]) + sqr(pt1->val[1]) + sqr(pt1->val[2])));
I(j, i) += std::sqrt(static_cast<double>(sqr(pt2->val[0]) + sqr(pt2->val[1]) + sqr(pt2->val[2])));
@@ -1609,7 +1605,7 @@ std::vector<cv::Mat> mergeTextures(
UDEBUG("Gain cam%d = %f", newCamIndex[t], gainsGray(newCamIndex[t], 0));
int indexMaterial = newCamIndex[t] / (cols*rows);
cv::Mat roi = globalTextures[indexMaterial](cv::Rect(u, v, emptyImage.cols, emptyImage.rows));
cv::Mat roi = globalTextures(cv::Rect(u+indexMaterial*globalTextures.rows, v, emptyImage.cols, emptyImage.rows));
std::vector<cv::Mat> channels;
cv::split(roi, channels);
@@ -1689,7 +1685,7 @@ std::vector<cv::Mat> mergeTextures(
std::vector<cv::Mat> blendGains(materials);
for(int i=0; i<materials;++i)
{
blendGains[i] = cv::Mat(globalTextures[i].rows/decimation, globalTextures[i].cols/decimation, CV_32FC3, cv::Scalar::all(1.0f));
blendGains[i] = cv::Mat(globalTextures.rows/decimation, globalTextures.rows/decimation, CV_32FC3, cv::Scalar::all(1.0f));
}
for(unsigned int p=0; p<vertexToPixels.size(); ++p)
@@ -1715,7 +1711,7 @@ std::vector<cv::Mat> mergeTextures(
weight = 0.0f;
}
int indexMaterial = newCamIndex[iter->first] / (cols*rows);
cv::Vec3b * pt = globalTextures[indexMaterial].ptr<cv::Vec3b>(v,u);
cv::Vec3b * pt = globalTextures.ptr<cv::Vec3b>(v,u+indexMaterial*globalTextures.rows);
gainsB[k] = static_cast<double>(pt->val[0]) * weight;
gainsG[k] = static_cast<double>(pt->val[1]) * weight;
gainsR[k] = static_cast<double>(pt->val[2]) * weight;
@@ -1740,7 +1736,7 @@ std::vector<cv::Mat> mergeTextures(
int u = iter->second.x*emptyImage.cols + imageOrigin[iter->first].x;
int v = (1.0-iter->second.y)*emptyImage.rows + imageOrigin[iter->first].y;
int indexMaterial = newCamIndex[iter->first] / (cols*rows);
cv::Vec3b * pt = globalTextures[indexMaterial].ptr<cv::Vec3b>(v,u);
cv::Vec3b * pt = globalTextures.ptr<cv::Vec3b>(v,u+indexMaterial*globalTextures.rows);
float gB = targetColor[0]/(pt->val[0]==0?1.0f:pt->val[0]);
float gG = targetColor[1]/(pt->val[1]==0?1.0f:pt->val[1]);
float gR = targetColor[2]/(pt->val[2]==0?1.0f:pt->val[2]);
@@ -1766,9 +1762,10 @@ std::vector<cv::Mat> mergeTextures(
channels[2].convertTo(img,CV_8U,128.0,0);
cv::imwrite("blendSmallR.png", img);*/
cv::Mat globalTexturesROI = globalTextures(cv::Range::all(), cv::Range(i*globalTextures.rows, (i+1)*globalTextures.rows));
cv::Mat dst;
cv::blur(blendGains[i], dst, cv::Size(3,3));
cv::resize(dst, blendGains[i], globalTextures[i].size(), 0, 0, cv::INTER_LINEAR);
cv::resize(dst, blendGains[i], globalTexturesROI.size(), 0, 0, cv::INTER_LINEAR);
/*cv::split(blendGains, channels);
channels[0].convertTo(img,CV_8U,128.0,0);
@@ -1778,7 +1775,7 @@ std::vector<cv::Mat> mergeTextures(
channels[2].convertTo(img,CV_8U,128.0,0);
cv::imwrite("blendFullR.png", img);*/
cv::multiply(globalTextures[i], blendGains[i], globalTextures[i], 1.0, CV_8UC3);
cv::multiply(globalTexturesROI, blendGains[i], globalTexturesROI, 1.0, CV_8UC3);
//UWARN("Saving blending.png", globalTexture);
//cv::imwrite("blending.png", globalTexture);
@@ -1792,36 +1789,38 @@ std::vector<cv::Mat> mergeTextures(
{
for(int i=0; i<materials; ++i)
{
cv::Mat globalTexturesROI = globalTextures(cv::Range::all(), cv::Range(i*globalTextures.rows, (i+1)*globalTextures.rows));
cv::Mat globalTextureMasksROI = globalTextureMasks(cv::Range::all(), cv::Range(i*globalTextureMasks.rows, (i+1)*globalTextureMasks.rows));
if(exposureFusion)
{
std::vector<cv::Mat> images;
images.push_back(globalTextures[i]);
images.push_back(globalTexturesROI);
if (brightnessContrastRatioLow > 0)
{
images.push_back(util2d::brightnessAndContrastAuto(
globalTextures[i],
globalTextureMasks[i],
globalTexturesROI,
globalTextureMasksROI,
(float)brightnessContrastRatioLow,
0.0f));
}
if (brightnessContrastRatioHigh > 0)
{
images.push_back(util2d::brightnessAndContrastAuto(
globalTextures[i],
globalTextureMasks[i],
globalTexturesROI,
globalTextureMasksROI,
0.0f,
(float)brightnessContrastRatioHigh));
}
globalTextures[i] = util2d::exposureFusion(images);
util2d::exposureFusion(images).copyTo(globalTexturesROI);
}
else
{
globalTextures[i] = util2d::brightnessAndContrastAuto(
globalTextures[i],
globalTextureMasks[i],
util2d::brightnessAndContrastAuto(
globalTexturesROI,
globalTextureMasksROI,
(float)brightnessContrastRatioLow,
(float)brightnessContrastRatioHigh);
(float)brightnessContrastRatioHigh).copyTo(globalTexturesROI);
}
}
if(state) state->callback(uFormat("Brightness and contrast auto %fs", timer.ticks()));
@@ -1829,7 +1828,7 @@ std::vector<cv::Mat> mergeTextures(
}
}
}
UDEBUG("globalTextures=%d", (int)globalTextures.size());
UDEBUG("globalTextures=%d", globalTextures.cols / globalTextures.rows);
return globalTextures;
}
corelib/src/util3d_transforms.cpp
+17
View File
@@ -208,6 +208,23 @@ pcl::PointNormal transformPoint(
ret.normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
return ret;
}
pcl::PointXYZRGBNormal transformPoint(
const pcl::PointXYZRGBNormal & point,
const Transform & transform)
{
pcl::PointXYZRGBNormal ret;
Eigen::Matrix<float, 3, 1> pt (point.x, point.y, point.z);
ret.x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
ret.y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
ret.z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
// Rotate normals
Eigen::Matrix<float, 3, 1> nt (point.normal_x, point.normal_y, point.normal_z);
ret.normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
ret.normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
ret.normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
return ret;
}
}