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

Refactored Camera classes and Preferences->Source menu

Browse Source
This commit is contained in:
matlabbe
2015-06-26 18:21:32 -04:00
parent 6f1df94b18
commit 6df403ed42
39 changed files with 3543 additions and 3388 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
tools/Calibration/main.cpp
View File

@@ -25,8 +25,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/CameraRGB.h"
#include "rtabmap/core/CameraRGBD.h"
#include "rtabmap/core/CameraStereo.h"
#include "rtabmap/core/CameraThread.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UConversion.h"
@@ -130,11 +131,10 @@ int main(int argc, char * argv[])
bool switchImages = false;
rtabmap::Camera * cameraUsb = 0;
rtabmap::CameraRGBD * camera = 0;
rtabmap::Camera * camera = 0;
if(driver == -1)
{
cameraUsb = new rtabmap::CameraVideo(device);
camera = new rtabmap::CameraVideo(device);
}
else if(driver == 0)
{
@@ -211,17 +211,7 @@ int main(int argc, char * argv[])
rtabmap::CameraThread * cameraThread = 0;
if(cameraUsb)
{
if(!cameraUsb->init())
{
printf("Camera init failed!\n");
delete cameraUsb;
exit(1);
}
cameraThread = new rtabmap::CameraThread(cameraUsb);
}
else if(camera)
if(camera)
{
if(!camera->init(""))
{

22
tools/Camera/main.cpp
View File

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/CameraRGB.h"
#include "rtabmap/core/DBReader.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UFile.h"
@@ -40,7 +40,7 @@ void showUsage()
"rtabmap-camera [option] \n"
" Options:\n"
" --device # USB camera device id (default 0).\n"
" --rate # Frame rate (default 30 Hz). 0 means as fast as possible.\n"
" --rate # Frame rate (default 0 Hz). 0 means as fast as possible.\n"
" --path "" Path to a directory of images or a video file.\n"
" --calibration "" Calibration file (*.yaml).\n\n");
exit(1);
@@ -53,7 +53,7 @@ int main(int argc, char * argv[])
int device = 0;
std::string path;
float rate = 30.0f;
float rate = 0.0f;
std::string calibrationFile;
for(int i=1; i<argc; ++i)
{
@@ -164,18 +164,16 @@ int main(int argc, char * argv[])
if(camera)
{
if(!camera->init())
if(!calibrationFile.empty())
{
UINFO("Set calibration: %s", calibrationFile.c_str());
}
if(!camera->init(UDirectory::getDir(calibrationFile), UFile::getName(calibrationFile)))
{
delete camera;
UERROR("Cannot initialize the camera.");
return -1;
}
if(!calibrationFile.empty())
{
UINFO("Set calibration: %s", calibrationFile.c_str());
camera->setCalibration(calibrationFile);
}
}
if(dbReader)
@@ -189,7 +187,7 @@ int main(int argc, char * argv[])
}
cv::Mat rgb;
rgb = camera?camera->takeImage():dbReader->getNextData().data().imageRaw();
rgb = camera?camera->takeImage().imageRaw():dbReader->getNextData().data().imageRaw();
cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window
while(!rgb.empty())
{
@@ -199,7 +197,7 @@ int main(int argc, char * argv[])
if(c == 27)
break; // if ESC, break and quit
rgb = camera?camera->takeImage():dbReader->getNextData().data().imageRaw();
rgb = camera?camera->takeImage().imageRaw():dbReader->getNextData().data().imageRaw();
}
cv::destroyWindow("Video");
if(camera)

66
tools/CameraRGBD/main.cpp
View File

@@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/CameraRGBD.h"
#include "rtabmap/core/CameraStereo.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/utilite/ULogger.h"
@@ -77,7 +78,7 @@ int main(int argc, char * argv[])
}
UINFO("Using driver %d", driver);
rtabmap::CameraRGBD * camera = 0;
rtabmap::Camera * camera = 0;
if(driver == 0)
{
camera = new rtabmap::CameraOpenni();
@@ -156,42 +157,55 @@ int main(int argc, char * argv[])
delete camera;
exit(1);
}
cv::Mat rgb, depth;
float fx, fy, cx, cy;
double stamp = 0.0;
camera->takeImage(rgb, depth, fx, fy, cx, cy, stamp);
if(rgb.cols != depth.cols || rgb.rows != depth.rows)
rtabmap::SensorData data = camera->takeImage();
if(data.imageRaw().cols != data.depthOrRightRaw().cols || data.imageRaw().rows != data.depthOrRightRaw().rows)
{
UWARN("RGB (%d/%d) and depth (%d/%d) frames are not the same size! The registered cloud cannot be shown.",
rgb.cols, rgb.rows, depth.cols, depth.rows);
data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows);
}
if(!fx || !fy)
if(!data.stereoCameraModel().isValid() && (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValid()))
{
UWARN("fx and/or fy are not set! The registered cloud cannot be shown.");
UWARN("Camera not calibrated! The registered cloud cannot be shown.");
}
pcl::visualization::CloudViewer viewer("cloud");
rtabmap::Transform t(1, 0, 0, 0,
0, -1, 0, 0,
0, 0, -1, 0);
while(!rgb.empty() && !viewer.wasStopped())
while(!data.imageRaw().empty() && !viewer.wasStopped())
{
if(depth.type() == CV_16UC1 || depth.type() == CV_32FC1)
cv::Mat rgb = data.imageRaw();
if(!data.depthRaw().empty() && (data.depthRaw().type() == CV_16UC1 || data.depthRaw().type() == CV_32FC1))
{
// depth
cv::Mat depth = data.depthRaw();
if(depth.type() == CV_32FC1)
{
depth = rtabmap::util3d::cvtDepthFromFloat(depth);
}
if(rgb.cols == depth.cols && rgb.rows == depth.rows && fx && fy)
if(rgb.cols == depth.cols && rgb.rows == depth.rows &&
data.cameraModels().size() &&
data.cameraModels()[0].isValid())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(rgb, depth, cx, cy, fx, fy);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(
rgb, depth,
data.cameraModels()[0].cx(),
data.cameraModels()[0].cy(),
data.cameraModels()[0].fx(),
data.cameraModels()[0].fy());
cloud = rtabmap::util3d::transformPointCloud(cloud, t);
viewer.showCloud(cloud, "cloud");
}
else if(!depth.empty() && fx && fy)
else if(!depth.empty() &&
data.cameraModels().size() &&
data.cameraModels()[0].isValid())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(depth, cx, cy, fx, fy);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(
depth,
data.cameraModels()[0].cx(),
data.cameraModels()[0].cy(),
data.cameraModels()[0].fx(),
data.cameraModels()[0].fy());
cloud = rtabmap::util3d::transformPointCloud(cloud, t);
viewer.showCloud(cloud, "cloud");
}
@@ -204,19 +218,25 @@ int main(int argc, char * argv[])
cv::imshow("Video", rgb); // show frame
cv::imshow("Depth", tmp);
}
else
else if(!data.rightRaw().empty())
{
// stereo
cv::Mat right = data.rightRaw();
cv::imshow("Left", rgb); // show frame
cv::imshow("Right", depth);
cv::imshow("Right", right);
if(rgb.cols == depth.cols && rgb.rows == depth.rows && fx && fy)
if(rgb.cols == right.cols && rgb.rows == right.rows && data.stereoCameraModel().isValid())
{
if(depth.channels() == 3)
if(right.channels() == 3)
{
cv::cvtColor(depth, depth, CV_BGR2GRAY);
cv::cvtColor(right, right, CV_BGR2GRAY);
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromStereoImages(rgb, depth, cx, cy, fx, fy);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromStereoImages(
rgb, right,
data.stereoCameraModel().left().cx(),
data.stereoCameraModel().left().cy(),
data.stereoCameraModel().left().fx(),
data.stereoCameraModel().baseline());
cloud = rtabmap::util3d::transformPointCloud(cloud, t);
viewer.showCloud(cloud, "cloud");
}
@@ -226,9 +246,7 @@ int main(int argc, char * argv[])
if(c == 27)
break; // if ESC, break and quit
rgb = cv::Mat();
depth = cv::Mat();
camera->takeImage(rgb, depth, fx, fy, cx, cy, stamp);
data = camera->takeImage();
}
cv::destroyWindow("Video");
cv::destroyWindow("Depth");

61
tools/ConsoleApp/main.cpp
View File

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
#include "rtabmap/core/Rtabmap.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/CameraRGB.h"
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UConversion.h>
@@ -53,10 +53,6 @@ void showUsage()
" -rateHz #.## Acquisition rate (Hz), for convenience\n"
" -repeat # Repeat the process on the data set # times (minimum of 1)\n"
" -createGT Generate a ground truth file\n"
" -image_width # Force an image width (Default 0: original size used).\n"
" The height must be also specified if changed.\n"
" -image_height # Force an image height (Default 0: original size used)\n"
" The height must be also specified if changed.\n"
" -start_at # When \"path\" is a directory of images, set this parameter\n"
" to start processing at image # (default 1).\n"
" -\"parameter name\" \"value\" Overwrite a specific RTAB-Map's parameter :\n"
@@ -118,8 +114,6 @@ int main(int argc, char * argv[])
int repeat = 0;
bool createGT = false;
std::string inputDbPath;
int imageWidth = 0;
int imageHeight = 0;
int startAt = 1;
ParametersMap pm;
ULogger::Level logLevel = ULogger::kError;
@@ -194,40 +188,6 @@ int main(int argc, char * argv[])
}
continue;
}
if(strcmp(argv[i], "-image_width") == 0)
{
++i;
if(i < argc)
{
imageWidth = std::atoi(argv[i]);
if(imageWidth < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-image_height") == 0)
{
++i;
if(i < argc)
{
imageHeight = std::atoi(argv[i]);
if(imageHeight < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-start_at") == 0)
{
++i;
@@ -328,12 +288,6 @@ int main(int argc, char * argv[])
printf("Cannot create a Ground truth if repeat is on.\n");
showUsage();
}
else if((imageWidth && imageHeight == 0) ||
(imageHeight && imageWidth == 0))
{
printf("If imageWidth is set, imageHeight must be too.\n");
showUsage();
}
UTimer timer;
timer.start();
@@ -342,11 +296,11 @@ int main(int argc, char * argv[])
Camera * camera = 0;
if(UDirectory::exists(path))
{
camera = new CameraImages(path, startAt, false, 1/rate, imageWidth, imageHeight);
camera = new CameraImages(path, startAt, false, 1/rate);
}
else
{
camera = new CameraVideo(path, 1/rate, imageWidth, imageHeight);
camera = new CameraVideo(path, 1/rate);
}
if(!camera || !camera->init())
@@ -395,7 +349,6 @@ int main(int argc, char * argv[])
printf(" Time threshold = %1.2f ms\n", rtabmap.getTimeThreshold());
printf(" Image rate = %1.2f s (%1.2f Hz)\n", rate, 1/rate);
printf(" Repeating data set = %s\n", repeat?"true":"false");
printf(" Camera width=%d, height=%d (0 is default)\n", imageWidth, imageHeight);
printf(" Camera starts at image %d (default 1)\n", startAt);
if(createGT)
{
@@ -422,23 +375,23 @@ int main(int argc, char * argv[])
std::list<std::vector<float> > teleopActions;
while(loopDataset <= repeat && g_forever)
{
cv::Mat img = camera->takeImage();
SensorData data = camera->takeImage();
int i=0;
double maxIterationTime = 0.0;
int maxIterationTimeId = 0;
while(!img.empty() && g_forever)
while(!data.imageRaw().empty() && g_forever)
{
++imagesProcessed;
iterationTimer.start();
rtabmapTimer.start();
rtabmap.process(img);
rtabmap.process(data.imageRaw());
double rtabmapTime = rtabmapTimer.elapsed();
loopClosureId = rtabmap.getLoopClosureId();
if(rtabmap.getLoopClosureId())
{
++countLoopDetected;
}
img = camera->takeImage();
data = camera->takeImage();
if(++count % 100 == 0)
{
printf(" count = %d, loop closures = %d, max time (at %d) = %fs\n",

5
tools/DataRecorder/main.cpp
View File

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/CameraThread.h>
#include <rtabmap/core/CameraRGBD.h>
#include <rtabmap/core/CameraStereo.h>
#include <rtabmap/core/Camera.h>
#include <rtabmap/core/CameraThread.h>
#include <rtabmap/gui/DataRecorder.h>
@@ -174,7 +175,7 @@ int main (int argc, char * argv[])
signal(SIGTERM, &sighandler);
signal(SIGINT, &sighandler);
rtabmap::CameraRGBD * camera = 0;
rtabmap::Camera * camera = 0;
rtabmap::Transform t=rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
if(driver == 0)
{
@@ -263,7 +264,7 @@ int main (int argc, char * argv[])
app->processEvents();
}
if(cam->init())
if(camera->init())
{
cam->start();

3
tools/OdometryViewer/main.cpp
View File

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/gui/OdometryViewer.h>
#include <rtabmap/core/CameraThread.h>
#include <rtabmap/core/CameraRGBD.h>
#include <rtabmap/core/CameraStereo.h>
#include <rtabmap/core/DBReader.h>
#include <rtabmap/core/VWDictionary.h>
#include <QApplication>
@@ -725,7 +726,7 @@ int main (int argc, char * argv[])
}
else
{
rtabmap::CameraRGBD * camera = 0;
rtabmap::Camera * camera = 0;
rtabmap::Transform t=rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
if(driver == 0)
{