int main (int argc, char * argv[])
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kInfo);
ULogger::setPrintTime(false);
ULogger::setPrintWhere(false);
// parse arguments
float rate = 0.0;
std::string inputDatabase;
int driver = 0;
int odomType = rtabmap::Parameters::defaultOdomFeatureType();
bool icp = false;
bool flow = false;
bool mono = false;
int nnType = rtabmap::Parameters::defaultOdomBowNNType();
float nndr = rtabmap::Parameters::defaultOdomBowNNDR();
float distance = rtabmap::Parameters::defaultOdomInlierDistance();
int maxWords = rtabmap::Parameters::defaultOdomMaxFeatures();
int minInliers = rtabmap::Parameters::defaultOdomMinInliers();
float maxDepth = rtabmap::Parameters::defaultOdomMaxDepth();
int iterations = rtabmap::Parameters::defaultOdomIterations();
int resetCountdown = rtabmap::Parameters::defaultOdomResetCountdown();
int decimation = 4;
float voxel = 0.005;
int samples = 10000;
float ratio = 0.7f;
int maxClouds = 10;
int briefBytes = rtabmap::Parameters::defaultBRIEFBytes();
int fastThr = rtabmap::Parameters::defaultFASTThreshold();
float sec = 0.0f;
bool gpu = false;
int localHistory = rtabmap::Parameters::defaultOdomBowLocalHistorySize();
bool p2p = rtabmap::Parameters::defaultOdomPnPEstimation();
for(int i=1; i<argc; ++i)
{
if(strcmp(argv[i], "-driver") == 0)
{
++i;
if(i < argc)
{
driver = std::atoi(argv[i]);
if(driver < 0 || driver > 7)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-o") == 0)
{
++i;
if(i < argc)
{
odomType = std::atoi(argv[i]);
if(odomType < 0 || odomType > 6)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-nn") == 0)
{
++i;
if(i < argc)
{
nnType = std::atoi(argv[i]);
if(nnType < 0 || nnType > 4)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-nndr") == 0)
{
++i;
if(i < argc)
{
nndr = uStr2Float(argv[i]);
if(nndr < 0.0f)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-hz") == 0)
{
++i;
if(i < argc)
{
rate = uStr2Float(argv[i]);
if(rate < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-db") == 0)
{
++i;
if(i < argc)
{
inputDatabase = argv[i];
if(UFile::getExtension(inputDatabase).compare("db") != 0)
{
printf("Database path (%s) should end with \"db\" \n", inputDatabase.c_str());
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-clouds") == 0)
{
++i;
if(i < argc)
{
maxClouds = std::atoi(argv[i]);
if(maxClouds < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-sec") == 0)
{
++i;
if(i < argc)
{
sec = uStr2Float(argv[i]);
if(sec < 0.0f)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-in") == 0)
{
++i;
if(i < argc)
{
distance = uStr2Float(argv[i]);
if(distance <= 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-max") == 0)
{
++i;
if(i < argc)
{
maxWords = std::atoi(argv[i]);
if(maxWords < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-min") == 0)
{
++i;
if(i < argc)
{
minInliers = std::atoi(argv[i]);
if(minInliers < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-depth") == 0)
{
++i;
if(i < argc)
{
maxDepth = uStr2Float(argv[i]);
if(maxDepth < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-i") == 0)
{
++i;
if(i < argc)
{
iterations = std::atoi(argv[i]);
if(iterations <= 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-reset") == 0)
{
++i;
if(i < argc)
{
resetCountdown = std::atoi(argv[i]);
if(resetCountdown < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-d") == 0)
{
++i;
if(i < argc)
{
decimation = std::atoi(argv[i]);
if(decimation < 1)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-v") == 0)
{
++i;
if(i < argc)
{
voxel = uStr2Float(argv[i]);
if(voxel < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-s") == 0)
{
++i;
if(i < argc)
{
samples = std::atoi(argv[i]);
if(samples < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-cr") == 0)
{
++i;
if(i < argc)
{
ratio = uStr2Float(argv[i]);
if(ratio < 0.0f)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-gpu") == 0)
{
gpu = true;
continue;
}
if(strcmp(argv[i], "-lh") == 0)
{
++i;
if(i < argc)
{
localHistory = std::atoi(argv[i]);
if(localHistory < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-brief_bytes") == 0)
{
++i;
if(i < argc)
{
briefBytes = std::atoi(argv[i]);
if(briefBytes < 1)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-fast_thr") == 0)
{
++i;
if(i < argc)
{
fastThr = std::atoi(argv[i]);
if(fastThr < 1)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-icp") == 0)
{
icp = true;
continue;
}
if(strcmp(argv[i], "-flow") == 0)
{
flow = true;
continue;
}
if(strcmp(argv[i], "-mono") == 0)
{
mono = true;
continue;
}
if(strcmp(argv[i], "-p2p") == 0)
{
p2p = true;
continue;
}
if(strcmp(argv[i], "-debug") == 0)
{
ULogger::setLevel(ULogger::kDebug);
ULogger::setPrintTime(true);
ULogger::setPrintWhere(true);
continue;
}
printf("Unrecognized option : %s\n", argv[i]);
showUsage();
}
if(odomType > 1 && nnType == rtabmap::VWDictionary::kNNFlannKdTree)
{
UERROR("You set \"-o %d\" (binary descriptor), you must use \"-nn 2\" (any \"-nn\" other than kNNFlannKdTree)", odomType);
showUsage();
}
else if(odomType <= 1 && nnType == rtabmap::VWDictionary::kNNFlannLSH)
{
UERROR("You set \"-o %d\" (float descriptor), you must use \"-nn 1\" (any \"-nn\" other than kNNFlannLSH)", odomType);
showUsage();
}
if(inputDatabase.size())
{
UINFO("Using database input \"%s\"", inputDatabase.c_str());
}
else
{
UINFO("Using OpenNI camera");
}
std::string odomName;
if(odomType == 0)
{
odomName = "SURF";
}
else if(odomType == 1)
{
odomName = "SIFT";
}
else if(odomType == 2)
{
odomName = "ORB";
}
else if(odomType == 3)
{
odomName = "FAST+FREAK";
}
else if(odomType == 4)
{
odomName = "FAST+BRIEF";
}
else if(odomType == 5)
{
odomName = "GFTT+FREAK";
}
else if(odomType == 6)
{
odomName = "GFTT+BRIEF";
}
else if(odomType == 7)
{
odomName = "BRISK";
}
if(icp)
{
odomName= "ICP";
}
if(flow)
{
odomName= "Optical Flow";
}
std::string nnName;
if(nnType == 0)
{
nnName = "kNNFlannLinear";
}
else if(nnType == 1)
{
nnName = "kNNFlannKdTree";
}
else if(nnType == 2)
{
nnName= "kNNFlannLSH";
}
else if(nnType == 3)
{
nnName= "kNNBruteForce";
}
else if(nnType == 4)
{
nnName= "kNNBruteForceGPU";
}
UINFO("Odometry used = %s", odomName.c_str());
UINFO("Camera rate = %f Hz", rate);
UINFO("Maximum clouds shown = %d", maxClouds);
UINFO("Delay = %f s", sec);
UINFO("Max depth = %f", maxDepth);
UINFO("Reset odometry coutdown = %d", resetCountdown);
QApplication app(argc, argv);
rtabmap::Odometry * odom = 0;
rtabmap::ParametersMap parameters;
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMaxDepth(), uNumber2Str(maxDepth)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomResetCountdown(), uNumber2Str(resetCountdown)));
if(!icp)
{
UINFO("Min inliers = %d", minInliers);
UINFO("Inlier maximum correspondences distance = %f", distance);
UINFO("RANSAC iterations = %d", iterations);
UINFO("Max features = %d", maxWords);
UINFO("GPU = %s", gpu?"true":"false");
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomInlierDistance(), uNumber2Str(distance)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMinInliers(), uNumber2Str(minInliers)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomIterations(), uNumber2Str(iterations)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMaxFeatures(), uNumber2Str(maxWords)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomFeatureType(), uNumber2Str(odomType)));
if(odomType == 0)
{
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kSURFGpuVersion(), uBool2Str(gpu)));
}
if(odomType == 2)
{
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kORBGpu(), uBool2Str(gpu)));
}
if(odomType == 3 || odomType == 4)
{
UINFO("FAST threshold = %d", fastThr);
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTThreshold(), uNumber2Str(fastThr)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTGpu(), uBool2Str(gpu)));
}
if(odomType == 4 || odomType == 6)
{
UINFO("BRIEF bytes = %d", briefBytes);
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), uNumber2Str(briefBytes)));
}
if(flow)
{
// Optical Flow
odom = new rtabmap::OdometryOpticalFlow(parameters);
}
else
{
//BOW
UINFO("Nearest neighbor = %s", nnName.c_str());
UINFO("Nearest neighbor ratio = %f", nndr);
UINFO("Local history = %d", localHistory);
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowNNType(), uNumber2Str(nnType)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowNNDR(), uNumber2Str(nndr)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowLocalHistorySize(), uNumber2Str(localHistory)));
if(mono)
{
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomPnPFlags(), uNumber2Str(0))); //CV_ITERATIVE
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomPnPReprojError(), "4.0"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomIterations(), "100"));
odom = new rtabmap::OdometryMono(parameters);
}
else
{
odom = new rtabmap::OdometryBOW(parameters);
}
}
}
else if(icp) // ICP
{
UINFO("ICP maximum correspondences distance = %f", distance);
UINFO("ICP iterations = %d", iterations);
UINFO("Cloud decimation = %d", decimation);
UINFO("Cloud voxel size = %f", voxel);
UINFO("Cloud samples = %d", samples);
UINFO("Cloud correspondence ratio = %f", ratio);
UINFO("Cloud point to plane = %s", p2p?"false":"true");
odom = new rtabmap::OdometryICP(decimation, voxel, samples, distance, iterations, ratio, !p2p);
}
rtabmap::OdometryThread odomThread(odom);
rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0, 50);
UEventsManager::addHandler(&odomThread);
UEventsManager::addHandler(&odomViewer);
odomViewer.setWindowTitle("Odometry view");
odomViewer.resize(1280, 480+QPushButton().minimumHeight());
if(inputDatabase.size())
{
rtabmap::DBReader camera(inputDatabase, rate, true);
if(camera.init())
{
odomThread.start();
if(sec > 0)
{
uSleep(sec*1000);
}
camera.start();
app.exec();
camera.join(true);
odomThread.join(true);
}
}
else
{
rtabmap::CameraRGBD * camera = 0;
rtabmap::Transform t=rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
if(driver == 0)
{
camera = new rtabmap::CameraOpenni("", rate, t);
}
else if(driver == 1)
{
if(!rtabmap::CameraOpenNI2::available())
{
UERROR("Not built with OpenNI2 support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNI2("", rate, t);
}
else if(driver == 2)
{
if(!rtabmap::CameraFreenect::available())
{
UERROR("Not built with Freenect support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect(0, rate, t);
}
else if(driver == 3)
{
if(!rtabmap::CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(false, rate, t);
}
else if(driver == 4)
{
if(!rtabmap::CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(true, rate, t);
}
else if(driver == 5)
{
if(!rtabmap::CameraFreenect2::available())
{
UERROR("Not built with Freenect2 support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeRGBDepthSD, rate, t);
}
else if(driver == 6)
{
if(!rtabmap::CameraStereoDC1394::available())
{
UERROR("Not built with dc1394 support...");
exit(-1);
}
camera = new rtabmap::CameraStereoDC1394(rate, t);
}
else if(driver == 7)
{
if(!rtabmap::CameraStereoFlyCapture2::available())
{
UERROR("Not built with FlyCapture2/Triclops support...");
exit(-1);
}
camera = new rtabmap::CameraStereoFlyCapture2(rate, t);
}
else
{
UFATAL("Camera driver (%d) not found!", driver);
}
//pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
if(camera->init())
{
if(camera->isCalibrated())
{
rtabmap::CameraThread cameraThread(camera);
odomThread.start();
cameraThread.start();
odomViewer.exec();
cameraThread.join(true);
odomThread.join(true);
}
else
{
printf("The camera is not calibrated! You should calibrate the camera first.\n");
delete camera;
}
}
else
{
printf("Failed to initialize the camera! Please select another driver (see \"--help\").\n");
delete camera;
}
}
return 0;
}
Transform Odometry::process(SensorData & data, OdometryInfo * info)
{
if(_pose.isNull())
{
_pose.setIdentity(); // initialized
}
UASSERT(!data.imageRaw().empty());
if(!data.stereoCameraModel().isValidForProjection() &&
(data.cameraModels().size() == 0 || !data.cameraModels()[0].isValidForProjection()))
{
UERROR("Rectified images required! Calibrate your camera.");
return Transform();
}
double dt = previousStamp_>0.0f?data.stamp() - previousStamp_:0.0;
Transform guess = dt && guessFromMotion_?Transform::getIdentity():Transform();
UASSERT_MSG(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull()), uFormat("dt=%f previous transform=%s", dt, previousVelocityTransform_.prettyPrint().c_str()).c_str());
if(!previousVelocityTransform_.isNull())
{
if(guessFromMotion_)
{
if(_filteringStrategy == 1)
{
// use Kalman predict transform
float vx,vy,vz, vroll,vpitch,vyaw;
predictKalmanFilter(dt, &vx,&vy,&vz,&vroll,&vpitch,&vyaw);
guess = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
}
else
{
float vx,vy,vz, vroll,vpitch,vyaw;
previousVelocityTransform_.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
guess = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
}
}
else if(_filteringStrategy == 1)
{
predictKalmanFilter(dt);
}
}
UTimer time;
Transform t;
if(_imageDecimation > 1)
{
// Decimation of images with calibrations
SensorData decimatedData = data;
decimatedData.setImageRaw(util2d::decimate(decimatedData.imageRaw(), _imageDecimation));
decimatedData.setDepthOrRightRaw(util2d::decimate(decimatedData.depthOrRightRaw(), _imageDecimation));
std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
for(unsigned int i=0; i<cameraModels.size(); ++i)
{
cameraModels[i] = cameraModels[i].scaled(1.0/double(_imageDecimation));
}
decimatedData.setCameraModels(cameraModels);
StereoCameraModel stereoModel = decimatedData.stereoCameraModel();
if(stereoModel.isValidForProjection())
{
stereoModel.scale(1.0/double(_imageDecimation));
}
decimatedData.setStereoCameraModel(stereoModel);
// compute transform
t = this->computeTransform(decimatedData, guess, info);
// transform back the keypoints in the original image
std::vector<cv::KeyPoint> kpts = decimatedData.keypoints();
double log2value = log(double(_imageDecimation))/log(2.0);
for(unsigned int i=0; i<kpts.size(); ++i)
{
kpts[i].pt.x *= _imageDecimation;
kpts[i].pt.y *= _imageDecimation;
kpts[i].size *= _imageDecimation;
kpts[i].octave += log2value;
}
data.setFeatures(kpts, decimatedData.descriptors());
if(info)
{
UASSERT(info->newCorners.size() == info->refCorners.size());
for(unsigned int i=0; i<info->newCorners.size(); ++i)
{
info->refCorners[i].x *= _imageDecimation;
info->refCorners[i].y *= _imageDecimation;
info->newCorners[i].x *= _imageDecimation;
info->newCorners[i].y *= _imageDecimation;
}
for(std::multimap<int, cv::KeyPoint>::iterator iter=info->words.begin(); iter!=info->words.end(); ++iter)
{
iter->second.pt.x *= _imageDecimation;
iter->second.pt.y *= _imageDecimation;
iter->second.size *= _imageDecimation;
iter->second.octave += log2value;
}
}
}
else
{
t = this->computeTransform(data, guess, info);
}
if(info)
{
info->timeEstimation = time.ticks();
info->lost = t.isNull();
info->stamp = data.stamp();
info->interval = dt;
info->transform = t;
if(!data.groundTruth().isNull())
{
if(!previousGroundTruthPose_.isNull())
{
info->transformGroundTruth = previousGroundTruthPose_.inverse() * data.groundTruth();
}
previousGroundTruthPose_ = data.groundTruth();
}
}
if(!t.isNull())
{
_resetCurrentCount = _resetCountdown;
float vx,vy,vz, vroll,vpitch,vyaw;
t.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
// transform to velocity
if(dt)
{
vx /= dt;
vy /= dt;
vz /= dt;
vroll /= dt;
vpitch /= dt;
vyaw /= dt;
}
if(_force3DoF || !_holonomic || particleFilters_.size() || _filteringStrategy==1)
{
if(_filteringStrategy == 1)
{
if(previousVelocityTransform_.isNull())
{
// reset Kalman
if(dt)
{
initKalmanFilter(t, vx,vy,vz,vroll,vpitch,vyaw);
}
else
{
initKalmanFilter(t);
}
}
else
{
// Kalman filtering
updateKalmanFilter(vx,vy,vz,vroll,vpitch,vyaw);
}
}
else if(particleFilters_.size())
{
// Particle filtering
UASSERT(particleFilters_.size()==6);
if(previousVelocityTransform_.isNull())
{
particleFilters_[0]->init(vx);
particleFilters_[1]->init(vy);
particleFilters_[2]->init(vz);
particleFilters_[3]->init(vroll);
particleFilters_[4]->init(vpitch);
particleFilters_[5]->init(vyaw);
}
else
{
vx = particleFilters_[0]->filter(vx);
vy = particleFilters_[1]->filter(vy);
vyaw = particleFilters_[5]->filter(vyaw);
if(!_holonomic)
{
// arc trajectory around ICR
float tmpY = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
if(fabs(tmpY) < fabs(vy) || (tmpY<=0 && vy >=0) || (tmpY>=0 && vy<=0))
{
vy = tmpY;
}
else
{
vyaw = (atan(vx/vy)*2.0f-CV_PI)*-1;
}
}
if(!_force3DoF)
{
vz = particleFilters_[2]->filter(vz);
vroll = particleFilters_[3]->filter(vroll);
vpitch = particleFilters_[4]->filter(vpitch);
}
}
if(info)
{
info->timeParticleFiltering = time.ticks();
}
if(_force3DoF)
{
vz = 0.0f;
vroll = 0.0f;
vpitch = 0.0f;
}
}
else if(!_holonomic)
{
// arc trajectory around ICR
vy = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
if(_force3DoF)
{
vz = 0.0f;
vroll = 0.0f;
vpitch = 0.0f;
}
}
if(dt)
{
t = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
}
else
{
t = Transform(vx, vy, vz, vroll, vpitch, vyaw);
}
if(info)
{
info->transformFiltered = t;
}
}
previousStamp_ = data.stamp();
previousVelocityTransform_.setNull();
if(dt)
{
previousVelocityTransform_ = Transform(vx, vy, vz, vroll, vpitch, vyaw);
}
if(info)
{
distanceTravelled_ += t.getNorm();
info->distanceTravelled = distanceTravelled_;
}
return _pose *= t; // updated
}
else if(_resetCurrentCount > 0)
{
UWARN("Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", _resetCurrentCount);
--_resetCurrentCount;
if(_resetCurrentCount == 0)
{
UWARN("Odometry automatically reset to latest pose!");
this->reset(_pose);
}
}
previousVelocityTransform_.setNull();
previousStamp_ = 0;
return Transform();
}
int main(int argc, char * argv[])
{
if(argc < 2)
{
showUsage();
}
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kDebug);
std::string dictionaryPath = argv[argc-1];
std::list<std::vector<float> > objectDescriptors;
//std::list<std::vector<float> > descriptors;
std::map<int, std::vector<float> > descriptors;
int dimension = 0;
UTimer timer;
int objectDescriptorsSize= 400;
std::ifstream file;
if(!dictionaryPath.empty())
{
file.open(dictionaryPath.c_str(), std::ifstream::in);
}
if(file.good())
{
UDEBUG("Loading the dictionary from \"%s\"", dictionaryPath.c_str());
// first line is the header
std::string str;
std::list<std::string> strList;
std::getline(file, str);
strList = uSplitNumChar(str);
for(std::list<std::string>::iterator iter = strList.begin(); iter != strList.end(); ++iter)
{
if(uIsDigit(iter->at(0)))
{
dimension = std::atoi(iter->c_str());
break;
}
}
if(dimension == 0 || dimension > 1000)
{
UERROR("Invalid dictionary file, visual word dimension (%d) is not valid, \"%s\"", dimension, dictionaryPath.c_str());
}
else
{
int descriptorsLoaded = 0;
// Process all words
while(file.good())
{
std::getline(file, str);
strList = uSplit(str);
if((int)strList.size() == dimension+1)
{
//first one is the visual word id
std::list<std::string>::iterator iter = strList.begin();
int id = atoi(iter->c_str());
++iter;
std::vector<float> descriptor(dimension);
int i=0;
//get descriptor
for(;i<dimension && iter != strList.end(); ++i, ++iter)
{
descriptor[i] = uStr2Float(*iter);
}
if(i != dimension)
{
UERROR("");
}
if(++descriptorsLoaded<=objectDescriptorsSize)
{
objectDescriptors.push_back(descriptor);
}
else
{
//descriptors.push_back(descriptor);
descriptors.insert(std::make_pair(id, descriptor));
}
}
else if(str.size())
{
UWARN("Cannot parse line \"%s\"", str.c_str());
}
}
}
UDEBUG("Time loading dictionary = %fs, dimension=%d", timer.ticks(), dimension);
}
else
{
UERROR("Cannot open dictionary file \"%s\"", dictionaryPath.c_str());
}
file.close();
if(descriptors.size() && objectDescriptors.size() && dimension)
{
cv::Mat dataTree;
cv::Mat queries;
UDEBUG("Creating data structures...");
// Create the data structure
dataTree = cv::Mat((int)descriptors.size(), dimension, CV_32F); // SURF descriptors are CV_32F
{//scope
//std::list<std::vector<float> >::const_iterator iter = descriptors.begin();
std::map<int, std::vector<float> >::const_iterator iter = descriptors.begin();
for(unsigned int i=0; i < descriptors.size(); ++i, ++iter)
{
UTimer tim;
//memcpy(dataTree.ptr<float>(i), iter->data(), dimension*sizeof(float));
memcpy(dataTree.ptr<float>(i), iter->second.data(), dimension*sizeof(float));
//if(i%100==0)
// UDEBUG("i=%d/%d tim=%fs", i, descriptors.size(), tim.ticks());
}
}
queries = cv::Mat((int)objectDescriptors.size(), dimension, CV_32F); // SURF descriptors are CV_32F
{//scope
std::list<std::vector<float> >::const_iterator iter = objectDescriptors.begin();
for(unsigned int i=0; i < objectDescriptors.size(); ++i, ++iter)
{
UTimer tim;
memcpy(queries.ptr<float>(i), iter->data(), dimension*sizeof(float));
//if(i%100==0)
// UDEBUG("i=%d/%d tim=%fs", i, objectDescriptors.size(), tim.ticks());
}
}
UDEBUG("descriptors.size()=%d, objectDescriptorsSize=%d, copying data = %f s",descriptors.size(), objectDescriptors.size(), timer.ticks());
UDEBUG("Creating indexes...");
cv::flann::Index * linearIndex = new cv::flann::Index(dataTree, cv::flann::LinearIndexParams());
UDEBUG("Time to create linearIndex = %f s", timer.ticks());
cv::flann::Index * kdTreeIndex1 = new cv::flann::Index(dataTree, cv::flann::KDTreeIndexParams(1));
UDEBUG("Time to create kdTreeIndex1 = %f s", timer.ticks());
cv::flann::Index * kdTreeIndex4 = new cv::flann::Index(dataTree, cv::flann::KDTreeIndexParams(4));
UDEBUG("Time to create kdTreeIndex4 = %f s", timer.ticks());
cv::flann::Index * kMeansIndex = new cv::flann::Index(dataTree, cv::flann::KMeansIndexParams());
UDEBUG("Time to create kMeansIndex = %f s", timer.ticks());
cv::flann::Index * compositeIndex = new cv::flann::Index(dataTree, cv::flann::CompositeIndexParams());
UDEBUG("Time to create compositeIndex = %f s", timer.ticks());
//cv::flann::Index * autoTunedIndex = new cv::flann::Index(dataTree, cv::flann::AutotunedIndexParams());
//UDEBUG("Time to create autoTunedIndex = %f s", timer.ticks());
UDEBUG("Search indexes...");
int k=2; // 2 nearest neighbors
cv::Mat results(queries.rows, k, CV_32SC1); // results index
cv::Mat dists(queries.rows, k, CV_32FC1); // Distance results are CV_32FC1
linearIndex->knnSearch(queries, results, dists, k);
//std::cout << results.t() << std::endl;
cv::Mat transposedLinear = dists.t();
UDEBUG("Time to search linearIndex = %f s", timer.ticks());
kdTreeIndex1->knnSearch(queries, results, dists, k);
//std::cout << results.t() << std::endl;
cv::Mat transposed = dists.t();
UDEBUG("Time to search kdTreeIndex1 = %f s (size=%d, dist error(k1,k2)=(%f,%f))",
timer.ticks(),
transposed.cols,
uMeanSquaredError( (float*)transposed.data,
transposed.cols,
(float*)transposedLinear.data,
transposedLinear.cols),
uMeanSquaredError( &transposed.at<float>(1,0),
transposed.cols,
&transposedLinear.at<float>(1,0),
transposedLinear.cols));
kdTreeIndex4->knnSearch(queries, results, dists, k);
//std::cout << results.t() << std::endl;
transposed = dists.t();
UDEBUG("Time to search kdTreeIndex4 = %f s (size=%d, dist error(k1,k2)=(%f,%f))",
timer.ticks(),
transposed.cols,
uMeanSquaredError( (float*)transposed.data,
transposed.cols,
(float*)transposedLinear.data,
transposedLinear.cols),
uMeanSquaredError( &transposed.at<float>(1,0),
transposed.cols,
&transposedLinear.at<float>(1,0),
transposedLinear.cols));
kMeansIndex->knnSearch(queries, results, dists, k);
//std::cout << results.t() << std::endl;
transposed = dists.t();
UDEBUG("Time to search kMeansIndex = %f s (size=%d, dist error(k1,k2)=(%f,%f))",
timer.ticks(),
transposed.cols,
uMeanSquaredError( (float*)transposed.data,
transposed.cols,
(float*)transposedLinear.data,
transposedLinear.cols),
uMeanSquaredError( &transposed.at<float>(1,0),
transposed.cols,
&transposedLinear.at<float>(1,0),
transposedLinear.cols));
compositeIndex->knnSearch(queries, results, dists, k);
//std::cout << results.t() << std::endl;
transposed = dists.t();
UDEBUG("Time to search compositeIndex = %f s (size=%d, dist error(k1,k2)=(%f,%f))",
timer.ticks(),
transposed.cols,
uMeanSquaredError( (float*)transposed.data,
transposed.cols,
(float*)transposedLinear.data,
transposedLinear.cols),
uMeanSquaredError( &transposed.at<float>(1,0),
transposed.cols,
&transposedLinear.at<float>(1,0),
transposedLinear.cols));
//autoTunedIndex->knnSearch(queries, results, dists, k);
//UDEBUG("Time to search autoTunedIndex = %f s", timer.ticks());
delete linearIndex;
delete kdTreeIndex1;
delete kdTreeIndex4;
delete kMeansIndex;
delete compositeIndex;
//delete autoTunedIndex;
}
return 0;
}
bool CalibrationDialog::save()
{
bool saved = false;
processingData_ = true;
if(!stereo_)
{
UASSERT(models_[0].isValid());
QString cameraName = models_[0].name().c_str();
QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_+"/"+cameraName+".yaml", "*.yaml");
if(!filePath.isEmpty())
{
QString name = QFileInfo(filePath).baseName();
QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
models_[0].setName(name.toStdString());
if(models_[0].save(dir.toStdString()))
{
QMessageBox::information(this, tr("Export"), tr("Calibration file saved to \"%1\".").arg(filePath));
UINFO("Saved \"%s\"!", filePath.toStdString().c_str());
savedCalibration_ = true;
saved = true;
}
else
{
UERROR("Error saving \"%s\"", filePath.toStdString().c_str());
}
}
}
else
{
UASSERT(stereoModel_.left().isValid() &&
stereoModel_.right().isValid()&&
(!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0));
QString cameraName = stereoModel_.name().c_str();
QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_ + "/" + cameraName, "*.yaml");
QString name = QFileInfo(filePath).baseName();
QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
if(!name.isEmpty())
{
stereoModel_.setName(name.toStdString());
std::string base = (dir+QDir::separator()+name).toStdString();
std::string leftPath = base+"_left.yaml";
std::string rightPath = base+"_right.yaml";
std::string posePath = base+"_pose.yaml";
if(stereoModel_.save(dir.toStdString(), false))
{
QMessageBox::information(this, tr("Export"), tr("Calibration files saved:\n \"%1\"\n \"%2\"\n \"%3\".").
arg(leftPath.c_str()).arg(rightPath.c_str()).arg(posePath.c_str()));
UINFO("Saved \"%s\" and \"%s\"!", leftPath.c_str(), rightPath.c_str());
savedCalibration_ = true;
saved = true;
}
else
{
UERROR("Error saving \"%s\" and \"%s\"", leftPath.c_str(), rightPath.c_str());
}
}
}
processingData_ = false;
return saved;
}
cv::flann::IndexParams * Settings::createFlannIndexParams()
{
cv::flann::IndexParams * params = 0;
QString str = getNearestNeighbor_1Strategy();
QStringList split = str.split(':');
if(split.size()==2)
{
bool ok = false;
int index = split.first().toInt(&ok);
if(ok)
{
QStringList strategies = split.last().split(';');
if(strategies.size() == 6 && index>=0 && index<6)
{
switch(index)
{
case 0:
if(strategies.at(index).compare("Linear") == 0)
{
UDEBUG("type=%s", "Linear");
params = new cv::flann::LinearIndexParams();
}
break;
case 1:
if(strategies.at(index).compare("KDTree") == 0)
{
UDEBUG("type=%s", "KDTree");
params = new cv::flann::KDTreeIndexParams(
getNearestNeighbor_KDTree_trees());
}
break;
case 2:
if(strategies.at(index).compare("KMeans") == 0)
{
cvflann::flann_centers_init_t centers_init = cvflann::FLANN_CENTERS_RANDOM;
QString str = getNearestNeighbor_KMeans_centers_init();
QStringList split = str.split(':');
if(split.size()==2)
{
bool ok = false;
int index = split.first().toInt(&ok);
if(ok)
{
centers_init = (cvflann::flann_centers_init_t)index;
}
}
UDEBUG("type=%s", "KMeans");
params = new cv::flann::KMeansIndexParams(
getNearestNeighbor_KMeans_branching(),
getNearestNeighbor_KMeans_iterations(),
centers_init,
getNearestNeighbor_KMeans_cb_index());
}
break;
case 3:
if(strategies.at(index).compare("Composite") == 0)
{
cvflann::flann_centers_init_t centers_init = cvflann::FLANN_CENTERS_RANDOM;
QString str = getNearestNeighbor_Composite_centers_init();
QStringList split = str.split(':');
if(split.size()==2)
{
bool ok = false;
int index = split.first().toInt(&ok);
if(ok)
{
centers_init = (cvflann::flann_centers_init_t)index;
}
}
UDEBUG("type=%s", "Composite");
params = new cv::flann::CompositeIndexParams(
getNearestNeighbor_Composite_trees(),
getNearestNeighbor_Composite_branching(),
getNearestNeighbor_Composite_iterations(),
centers_init,
getNearestNeighbor_Composite_cb_index());
}
break;
case 4:
if(strategies.at(index).compare("Autotuned") == 0)
{
UDEBUG("type=%s", "Autotuned");
params = new cv::flann::AutotunedIndexParams(
getNearestNeighbor_Autotuned_target_precision(),
getNearestNeighbor_Autotuned_build_weight(),
getNearestNeighbor_Autotuned_memory_weight(),
getNearestNeighbor_Autotuned_sample_fraction());
}
break;
case 5:
if(strategies.at(index).compare("Lsh") == 0)
{
UDEBUG("type=%s", "Lsh");
params = new cv::flann::LshIndexParams(
getNearestNeighbor_Lsh_table_number(),
getNearestNeighbor_Lsh_key_size(),
getNearestNeighbor_Lsh_multi_probe_level());
}
break;
default:
break;
}
}
}
}
if(!params)
{
UERROR("NN strategy not found !? Using default KDTRee...");
params = new cv::flann::KDTreeIndexParams();
}
return params ;
}
bool UAudioCaptureMic::init()
{
this->close();
bool ok = UAudioCapture::init();
if(ok)
{
std::string::size_type loc;
if(_fileName.size())
{
loc = _fileName.find( ".mp3", 0 );
if( loc != std::string::npos )
{
#ifdef BUILT_WITH_LAME
_encodeToMp3 = true;
#else
_fileName.append(".wav");
UERROR("Cannot write to a mp3, saving to a wav instead (%s)", _fileName.c_str());
#endif
}
_fp = fopen(_fileName.c_str(), "wb");
}
FMOD_RESULT result;
FMOD_BOOL isRecording = false;
result = UAudioSystem::isRecording(_driver, &isRecording); UASSERT_MSG(result==FMOD_OK, FMOD_ErrorString(result));
if(isRecording)
{
result = UAudioSystem::recordStop(_driver); UASSERT_MSG(result==FMOD_OK, FMOD_ErrorString(result));
}
_dataLength = 0;
_soundLength = 0;
_lastRecordPos = 0;
FMOD_CREATESOUNDEXINFO exinfo;
memset(&exinfo, 0, sizeof(FMOD_CREATESOUNDEXINFO));
exinfo.cbsize = sizeof(FMOD_CREATESOUNDEXINFO);
exinfo.numchannels = channels();
if(bytesPerSample() == 1)
{
exinfo.format = FMOD_SOUND_FORMAT_PCM8;
}
else if(bytesPerSample() == 2)
{
exinfo.format = FMOD_SOUND_FORMAT_PCM16;
}
else if(bytesPerSample() == 3)
{
exinfo.format = FMOD_SOUND_FORMAT_PCM24;
}
else if(bytesPerSample() == 4)
{
exinfo.format = FMOD_SOUND_FORMAT_PCM32;
}
exinfo.defaultfrequency = (int)fs();
exinfo.length = exinfo.defaultfrequency * bytesPerSample() * exinfo.numchannels * 2; // 2 -> pour deux secondes
result = UAudioSystem::createSound(0, FMOD_2D | FMOD_SOFTWARE | FMOD_OPENUSER, &exinfo, &_sound); UASSERT_MSG(result==FMOD_OK, FMOD_ErrorString(result));
if(_fp)
{
int channels, bits;
float rate;
FMOD_Sound_GetFormat(_sound, 0, 0, &channels, &bits);
FMOD_Sound_GetDefaults(_sound, &rate, 0, 0, 0);
UWav::writeWavHeader(_fp, _dataLength, rate, channels, bits); // Write out the wav header. La longueur sera de 0 puisqu'elle est incunnue pour l'instant.
}
result = FMOD_Sound_GetLength(_sound, &_soundLength, FMOD_TIMEUNIT_PCM); UASSERT_MSG(result==FMOD_OK, FMOD_ErrorString(result));
}
return ok;
}
int main(int argc, char * argv[])
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kInfo);
int driver = 0;
if(argc < 2)
{
showUsage();
}
else
{
driver = atoi(argv[argc-1]);
if(driver < 0 || driver > 4)
{
UERROR("driver should be between 0 and 4.");
showUsage();
}
}
UINFO("Using driver %d", driver);
rtabmap::CameraRGBD * camera = 0;
if(driver == 0)
{
camera = new rtabmap::CameraOpenni("", 0);
}
else if(driver == 1)
{
if(!rtabmap::CameraOpenNI2::available())
{
UERROR("Not built with OpenNI2 support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNI2(0);
}
else if(driver == 2)
{
if(!rtabmap::CameraFreenect::available())
{
UERROR("Not built with Freenect support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect(0, 0);
}
else if(driver == 3)
{
if(!rtabmap::CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(false, 0);
}
else if(driver == 4)
{
if(!rtabmap::CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(true, 0);
}
else
{
UFATAL("");
}
if(!camera->init())
{
printf("Camera init failed!\n");
delete camera;
exit(1);
}
cv::Mat rgb, depth;
float fx, fy, cx, cy;
camera->takeImage(rgb, depth, fx, fy, cx, cy);
cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window
cv::namedWindow("Depth", CV_WINDOW_AUTOSIZE); // create window
pcl::visualization::CloudViewer viewer("cloud");
while(!rgb.empty() && !viewer.wasStopped())
{
cv::Mat tmp;
depth.convertTo(tmp, CV_8UC1, 255.0/2048.0);
cv::imshow("Video", rgb); // show frame
cv::imshow("Depth",tmp);
viewer.showCloud(rtabmap::util3d::cloudFromDepthRGB(rgb, depth, cx, cy, fx, fy), "cloud");
int c = cv::waitKey(10); // wait 10 ms or for key stroke
if(c == 27)
break; // if ESC, break and quit
rgb = cv::Mat();
depth = cv::Mat();
camera->takeImage(rgb, depth, fx, fy, cx, cy);
}
cv::destroyWindow("Video");
cv::destroyWindow("Depth");
delete camera;
return 0;
}
void CameraThread::mainLoop()
{
UDEBUG("");
CameraInfo info;
SensorData data = _camera->takeImage(&info);
if(!data.imageRaw().empty())
{
if(_colorOnly && !data.depthRaw().empty())
{
data.setDepthOrRightRaw(cv::Mat());
}
if(_imageDecimation>1 && !data.imageRaw().empty())
{
UDEBUG("");
UTimer timer;
if(!data.depthRaw().empty() &&
!(data.depthRaw().rows % _imageDecimation == 0 && data.depthRaw().cols % _imageDecimation == 0))
{
UERROR("Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! "
"Images won't be resized.", _imageDecimation, data.depthRaw().cols, data.depthRaw().rows);
}
else
{
data.setImageRaw(util2d::decimate(data.imageRaw(), _imageDecimation));
data.setDepthOrRightRaw(util2d::decimate(data.depthOrRightRaw(), _imageDecimation));
std::vector<CameraModel> models = data.cameraModels();
for(unsigned int i=0; i<models.size(); ++i)
{
if(models[i].isValidForProjection())
{
models[i] = models[i].scaled(1.0/double(_imageDecimation));
}
}
data.setCameraModels(models);
StereoCameraModel stereoModel = data.stereoCameraModel();
if(stereoModel.isValidForProjection())
{
stereoModel.scale(1.0/double(_imageDecimation));
data.setStereoCameraModel(stereoModel);
}
}
info.timeImageDecimation = timer.ticks();
}
if(_mirroring && data.cameraModels().size() == 1)
{
UDEBUG("");
UTimer timer;
cv::Mat tmpRgb;
cv::flip(data.imageRaw(), tmpRgb, 1);
data.setImageRaw(tmpRgb);
UASSERT_MSG(data.cameraModels().size() <= 1 && !data.stereoCameraModel().isValidForProjection(), "Only single RGBD cameras are supported for mirroring.");
if(data.cameraModels().size() && data.cameraModels()[0].cx())
{
CameraModel tmpModel(
data.cameraModels()[0].fx(),
data.cameraModels()[0].fy(),
float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
data.cameraModels()[0].cy(),
data.cameraModels()[0].localTransform());
data.setCameraModel(tmpModel);
}
if(!data.depthRaw().empty())
{
cv::Mat tmpDepth;
cv::flip(data.depthRaw(), tmpDepth, 1);
data.setDepthOrRightRaw(tmpDepth);
}
info.timeMirroring = timer.ticks();
}
if(_stereoToDepth && data.stereoCameraModel().isValidForProjection() && !data.rightRaw().empty())
{
UDEBUG("");
UTimer timer;
cv::Mat depth = util2d::depthFromDisparity(
_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
data.stereoCameraModel().left().fx(),
data.stereoCameraModel().baseline());
data.setCameraModel(data.stereoCameraModel().left());
data.setDepthOrRightRaw(depth);
data.setStereoCameraModel(StereoCameraModel());
info.timeDisparity = timer.ticks();
UDEBUG("Computing disparity = %f s", info.timeDisparity);
}
if(_scanFromDepth &&
data.cameraModels().size() &&
data.cameraModels().at(0).isValidForProjection() &&
!data.depthRaw().empty())
{
UDEBUG("");
if(data.laserScanRaw().empty())
{
UASSERT(_scanDecimation >= 1);
UTimer timer;
pcl::IndicesPtr validIndices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, _scanDecimation, _scanMaxDepth, _scanMinDepth, validIndices.get());
float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
if(_scanVoxelSize>0.0f)
{
cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize);
float ratio = float(cloud->size()) / float(validIndices->size());
maxPoints = ratio * maxPoints;
}
else if(!cloud->is_dense)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
cloud = denseCloud;
}
cv::Mat scan;
if(_scanNormalsK>0)
{
scan = util3d::laserScanFromPointCloud(*util3d::computeNormals(cloud, _scanNormalsK));
}
else
{
scan = util3d::laserScanFromPointCloud(*cloud);
}
data.setLaserScanRaw(scan, (int)maxPoints, _scanMaxDepth);
info.timeScanFromDepth = timer.ticks();
UDEBUG("Computing scan from depth = %f s", info.timeScanFromDepth);
}
else
{
UWARN("Option to create laser scan from depth image is enabled, but "
"there is already a laser scan in the captured sensor data. Scan from "
"depth will not be created.");
}
}
info.cameraName = _camera->getSerial();
this->post(new CameraEvent(data, info));
}
else if(!this->isKilled())
{
UWARN("no more images...");
this->kill();
this->post(new CameraEvent());
}
}
// return not null transform if odometry is correctly computed
Transform OdometryF2F::computeTransform(
SensorData & data,
const Transform & guess,
OdometryInfo * info)
{
UTimer timer;
Transform output;
if(!data.rightRaw().empty() && !data.stereoCameraModel().isValidForProjection())
{
UERROR("Calibrated stereo camera required");
return output;
}
if(!data.depthRaw().empty() &&
(data.cameraModels().size() != 1 || !data.cameraModels()[0].isValidForProjection()))
{
UERROR("Calibrated camera required (multi-cameras not supported).");
return output;
}
RegistrationInfo regInfo;
UASSERT(!this->getPose().isNull());
if(lastKeyFramePose_.isNull())
{
lastKeyFramePose_ = this->getPose(); // reset to current pose
}
Transform motionSinceLastKeyFrame = lastKeyFramePose_.inverse()*this->getPose();
Signature newFrame(data);
if(refFrame_.sensorData().isValid())
{
Signature tmpRefFrame = refFrame_;
output = registrationPipeline_->computeTransformationMod(
tmpRefFrame,
newFrame,
!guess.isNull()?motionSinceLastKeyFrame*guess:Transform(),
®Info);
if(info && this->isInfoDataFilled())
{
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
EpipolarGeometry::findPairsUnique(tmpRefFrame.getWords(), newFrame.getWords(), pairs);
info->refCorners.resize(pairs.size());
info->newCorners.resize(pairs.size());
std::map<int, int> idToIndex;
int i=0;
for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin();
iter!=pairs.end();
++iter)
{
info->refCorners[i] = iter->second.first.pt;
info->newCorners[i] = iter->second.second.pt;
idToIndex.insert(std::make_pair(iter->first, i));
++i;
}
info->cornerInliers.resize(regInfo.inliersIDs.size(), 1);
i=0;
for(; i<(int)regInfo.inliersIDs.size(); ++i)
{
info->cornerInliers[i] = idToIndex.at(regInfo.inliersIDs[i]);
}
Transform t = this->getPose()*motionSinceLastKeyFrame.inverse();
for(std::multimap<int, cv::Point3f>::const_iterator iter=tmpRefFrame.getWords3().begin(); iter!=tmpRefFrame.getWords3().end(); ++iter)
{
info->localMap.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t)));
}
info->words = newFrame.getWords();
}
}
else
{
//return Identity
output = Transform::getIdentity();
// a very high variance tells that the new pose is not linked with the previous one
regInfo.variance = 9999;
}
if(!output.isNull())
{
output = motionSinceLastKeyFrame.inverse() * output;
// new key-frame?
if( (registrationPipeline_->isImageRequired() && (keyFrameThr_ == 0 || float(regInfo.inliers) <= keyFrameThr_*float(refFrame_.sensorData().keypoints().size()))) ||
(registrationPipeline_->isScanRequired() && (scanKeyFrameThr_ == 0 || regInfo.icpInliersRatio <= scanKeyFrameThr_)))
{
UDEBUG("Update key frame");
int features = newFrame.getWordsDescriptors().size();
if(features == 0)
{
newFrame = Signature(data);
// this will generate features only for the first frame or if optical flow was used (no 3d words)
Signature dummy;
registrationPipeline_->computeTransformationMod(
newFrame,
dummy);
features = (int)newFrame.sensorData().keypoints().size();
}
if((features >= registrationPipeline_->getMinVisualCorrespondences()) &&
(registrationPipeline_->getMinGeometryCorrespondencesRatio()==0.0f ||
(newFrame.sensorData().laserScanRaw().cols &&
(newFrame.sensorData().laserScanMaxPts() == 0 || float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanMaxPts())>=registrationPipeline_->getMinGeometryCorrespondencesRatio()))))
{
refFrame_ = newFrame;
refFrame_.setWords(std::multimap<int, cv::KeyPoint>());
refFrame_.setWords3(std::multimap<int, cv::Point3f>());
refFrame_.setWordsDescriptors(std::multimap<int, cv::Mat>());
//reset motion
lastKeyFramePose_.setNull();
}
else
{
if (!refFrame_.sensorData().isValid())
{
// Don't send odometry if we don't have a keyframe yet
output.setNull();
}
if(features < registrationPipeline_->getMinVisualCorrespondences())
{
UWARN("Too low 2D features (%d), keeping last key frame...", features);
}
if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newFrame.sensorData().laserScanRaw().cols==0)
{
UWARN("Too low scan points (%d), keeping last key frame...", newFrame.sensorData().laserScanRaw().cols);
}
else if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newFrame.sensorData().laserScanMaxPts() != 0 && float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanMaxPts())<registrationPipeline_->getMinGeometryCorrespondencesRatio())
{
UWARN("Too low scan points ratio (%d < %d), keeping last key frame...", float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanMaxPts()), registrationPipeline_->getMinGeometryCorrespondencesRatio());
}
}
}
}
else if(!regInfo.rejectedMsg.empty())
{
UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
}
data.setFeatures(newFrame.sensorData().keypoints(), newFrame.sensorData().descriptors());
if(info)
{
info->type = 1;
info->variance = regInfo.variance;
info->inliers = regInfo.inliers;
info->icpInliersRatio = regInfo.icpInliersRatio;
info->matches = regInfo.matches;
info->features = newFrame.sensorData().keypoints().size();
}
UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s",
timer.elapsed(),
output.isNull()?"true":"false",
(int)regInfo.inliers,
(int)newFrame.sensorData().keypoints().size(),
!output.isNull()?"true":"false");
return output;
}
int main(int argc, char * argv[])
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kInfo);
ULogger::setPrintTime(false);
ULogger::setPrintWhere(false);
int driver = -1;
int device = 0;
bool stereo = false;
for(int i=1; i<argc; ++i)
{
if(strcmp(argv[i], "--driver") == 0)
{
++i;
if(i < argc)
{
driver = std::atoi(argv[i]);
if(driver < -1)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "--device") == 0)
{
++i;
if(i < argc)
{
device = std::atoi(argv[i]);
if(device < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "--debug") == 0)
{
ULogger::setLevel(ULogger::kDebug);
ULogger::setPrintTime(true);
ULogger::setPrintWhere(true);
continue;
}
if(strcmp(argv[i], "--stereo") == 0)
{
stereo=true;
continue;
}
if(strcmp(argv[i], "--help") == 0)
{
showUsage();
}
printf("Unrecognized option : %s\n", argv[i]);
showUsage();
}
if(driver < -1 || driver > 7)
{
UERROR("driver should be between -1 and 7.");
showUsage();
}
UINFO("Using driver %d", driver);
UINFO("Using device %d", device);
UINFO("Stereo: %s", stereo?"true":"false");
bool switchImages = false;
rtabmap::Camera * camera = 0;
if(driver == -1)
{
camera = new rtabmap::CameraVideo(device);
}
else if(driver == 0)
{
camera = new rtabmap::CameraOpenni();
}
else if(driver == 1)
{
if(!rtabmap::CameraOpenNI2::available())
{
UERROR("Not built with OpenNI2 support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNI2();
}
else if(driver == 2)
{
if(!rtabmap::CameraFreenect::available())
{
UERROR("Not built with Freenect support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect();
}
else if(driver == 3)
{
if(!rtabmap::CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(false);
}
else if(driver == 4)
{
if(!rtabmap::CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(true);
}
else if(driver == 5)
{
if(!rtabmap::CameraFreenect2::available())
{
UERROR("Not built with Freenect2 support...");
exit(-1);
}
switchImages = true;
camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeColorIR);
}
else if(driver == 6)
{
if(!rtabmap::CameraStereoDC1394::available())
{
UERROR("Not built with DC1394 support...");
exit(-1);
}
camera = new rtabmap::CameraStereoDC1394();
}
else if(driver == 7)
{
if(!rtabmap::CameraStereoFlyCapture2::available())
{
UERROR("Not built with FlyCapture2/Triclops support...");
exit(-1);
}
camera = new rtabmap::CameraStereoFlyCapture2();
}
else
{
UFATAL("");
}
rtabmap::CameraThread * cameraThread = 0;
if(camera)
{
if(!camera->init(""))
{
printf("Camera init failed!\n");
delete camera;
exit(1);
}
cameraThread = new rtabmap::CameraThread(camera);
}
QApplication app(argc, argv);
rtabmap::CalibrationDialog dialog(stereo, ".", switchImages);
dialog.registerToEventsManager();
dialog.show();
cameraThread->start();
app.exec();
cameraThread->join(true);
delete cameraThread;
}
int main(int argc, char * argv[])
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kInfo);
//ULogger::setPrintTime(false);
//ULogger::setPrintWhere(false);
int driver = 0;
if(argc < 2)
{
showUsage();
}
else
{
if(strcmp(argv[argc-1], "--help") == 0)
{
showUsage();
}
driver = atoi(argv[argc-1]);
if(driver < 0 || driver > 7)
{
UERROR("driver should be between 0 and 6.");
showUsage();
}
}
UINFO("Using driver %d", driver);
rtabmap::CameraRGBD * camera = 0;
if(driver == 0)
{
camera = new rtabmap::CameraOpenni();
}
else if(driver == 1)
{
if(!rtabmap::CameraOpenNI2::available())
{
UERROR("Not built with OpenNI2 support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNI2();
}
else if(driver == 2)
{
if(!rtabmap::CameraFreenect::available())
{
UERROR("Not built with Freenect support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect();
}
else if(driver == 3)
{
if(!rtabmap::CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(false);
}
else if(driver == 4)
{
if(!rtabmap::CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(true);
}
else if(driver == 5)
{
if(!rtabmap::CameraFreenect2::available())
{
UERROR("Not built with Freenect2 support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeRGBDepthSD);
}
else if(driver == 6)
{
if(!rtabmap::CameraStereoDC1394::available())
{
UERROR("Not built with DC1394 support...");
exit(-1);
}
camera = new rtabmap::CameraStereoDC1394();
}
else if(driver == 7)
{
if(!rtabmap::CameraStereoFlyCapture2::available())
{
UERROR("Not built with FlyCapture2/Triclops support...");
exit(-1);
}
camera = new rtabmap::CameraStereoFlyCapture2();
}
else
{
UFATAL("");
}
if(!camera->init())
{
printf("Camera init failed! Please select another driver (see \"--help\").\n");
delete camera;
exit(1);
}
cv::Mat rgb, depth;
float fx, fy, cx, cy;
camera->takeImage(rgb, depth, fx, fy, cx, cy);
if(rgb.cols != depth.cols || rgb.rows != depth.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);
}
if(!fx || !fy)
{
UWARN("fx and/or fy are not set! 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())
{
if(depth.type() == CV_16UC1 || depth.type() == CV_32FC1)
{
// depth
if(depth.type() == CV_32FC1)
{
depth = rtabmap::util3d::cvtDepthFromFloat(depth);
}
if(rgb.cols == depth.cols && rgb.rows == depth.rows && fx && fy)
{
std::cout << "------------------------------------------------- tools/CameraRGBD/main.cpp -------------------------------------------------" << std::endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(rgb, depth, cx, cy, fx, fy);
cloud = rtabmap::util3d::transformPointCloud(cloud, t);
viewer.showCloud(cloud, "cloud");
}
else if(!depth.empty() && fx && fy)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(depth, cx, cy, fx, fy);
cloud = rtabmap::util3d::transformPointCloud(cloud, t);
viewer.showCloud(cloud, "cloud");
}
cv::Mat tmp;
unsigned short min=0, max = 2048;
uMinMax((unsigned short*)depth.data, depth.rows*depth.cols, min, max);
depth.convertTo(tmp, CV_8UC1, 255.0/max);
cv::imshow("Video", rgb); // show frame
cv::imshow("Depth", tmp);
}
else
{
// stereo
cv::imshow("Left", rgb); // show frame
cv::imshow("Right", depth);
if(rgb.cols == depth.cols && rgb.rows == depth.rows && fx && fy)
{
if(depth.channels() == 3)
{
cv::cvtColor(depth, depth, CV_BGR2GRAY);
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromStereoImages(rgb, depth, cx, cy, fx, fy);
cloud = rtabmap::util3d::transformPointCloud(cloud, t);
viewer.showCloud(cloud, "cloud");
}
}
int c = cv::waitKey(10); // wait 10 ms or for key stroke
if(c == 27)
break; // if ESC, break and quit
rgb = cv::Mat();
depth = cv::Mat();
camera->takeImage(rgb, depth, fx, fy, cx, cy);
}
cv::destroyWindow("Video");
cv::destroyWindow("Depth");
delete camera;
return 0;
}
SensorData DBReader::getNextData()
{
SensorData data;
if(_dbDriver)
{
float frameRate = _frameRate;
if(frameRate>0.0f)
{
int sleepTime = (1000.0f/frameRate - 1000.0f*_timer.getElapsedTime());
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
// Add precision at the cost of a small overhead
while(_timer.getElapsedTime() < 1.0/double(frameRate)-0.000001)
{
//
}
double slept = _timer.getElapsedTime();
_timer.start();
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(frameRate));
}
if(!this->isKilled() && _currentId != _ids.end())
{
cv::Mat imageBytes;
cv::Mat depthBytes;
cv::Mat laserScanBytes;
int mapId;
float fx,fy,cx,cy;
Transform localTransform, pose;
float rotVariance = 1.0f;
float transVariance = 1.0f;
std::vector<unsigned char> userData;
_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform);
// info
int weight;
std::string label;
double stamp;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData);
if(!_odometryIgnored)
{
std::map<int, Link> links;
_dbDriver->loadLinks(*_currentId, links, Link::kNeighbor);
if(links.size())
{
// assume the first is the backward neighbor, take its variance
rotVariance = links.begin()->second.rotVariance();
transVariance = links.begin()->second.transVariance();
}
}
else
{
pose.setNull();
}
int seq = *_currentId;
++_currentId;
if(imageBytes.empty())
{
UWARN("No image loaded from the database for id=%d!", *_currentId);
}
rtabmap::CompressionThread ctImage(imageBytes, true);
rtabmap::CompressionThread ctDepth(depthBytes, true);
rtabmap::CompressionThread ctLaserScan(laserScanBytes, false);
ctImage.start();
ctDepth.start();
ctLaserScan.start();
ctImage.join();
ctDepth.join();
ctLaserScan.join();
data = SensorData(
ctLaserScan.getUncompressedData(),
ctImage.getUncompressedData(),
ctDepth.getUncompressedData(),
fx,fy,cx,cy,
localTransform,
pose,
rotVariance,
transVariance,
seq,
stamp,
userData);
UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
data.laserScan().empty()?0:1,
data.image().empty()?0:1,
data.depth().empty()?0:1,
data.rightImage().empty()?0:1);
}
}
else
{
UERROR("Not initialized...");
}
return data;
}
void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
{
UASSERT(dataPtr!=0);
SensorData & data = *dataPtr;
if(_colorOnly && !data.depthRaw().empty())
{
data.setDepthOrRightRaw(cv::Mat());
}
if(_distortionModel && !data.depthRaw().empty())
{
UTimer timer;
if(_distortionModel->getWidth() == data.depthRaw().cols &&
_distortionModel->getHeight() == data.depthRaw().rows )
{
cv::Mat depth = data.depthRaw().clone();// make sure we are not modifying data in cached signatures.
_distortionModel->undistort(depth);
data.setDepthOrRightRaw(depth);
}
else
{
UERROR("Distortion model size is %dx%d but dpeth image is %dx%d!",
_distortionModel->getWidth(), _distortionModel->getHeight(),
data.depthRaw().cols, data.depthRaw().rows);
}
if(info) info->timeUndistortDepth = timer.ticks();
}
if(_bilateralFiltering && !data.depthRaw().empty())
{
UTimer timer;
data.setDepthOrRightRaw(util2d::fastBilateralFiltering(data.depthRaw(), _bilateralSigmaS, _bilateralSigmaR));
if(info) info->timeBilateralFiltering = timer.ticks();
}
if(_imageDecimation>1 && !data.imageRaw().empty())
{
UDEBUG("");
UTimer timer;
if(!data.depthRaw().empty() &&
!(data.depthRaw().rows % _imageDecimation == 0 && data.depthRaw().cols % _imageDecimation == 0))
{
UERROR("Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! "
"Images won't be resized.", _imageDecimation, data.depthRaw().cols, data.depthRaw().rows);
}
else
{
data.setImageRaw(util2d::decimate(data.imageRaw(), _imageDecimation));
data.setDepthOrRightRaw(util2d::decimate(data.depthOrRightRaw(), _imageDecimation));
std::vector<CameraModel> models = data.cameraModels();
for(unsigned int i=0; i<models.size(); ++i)
{
if(models[i].isValidForProjection())
{
models[i] = models[i].scaled(1.0/double(_imageDecimation));
}
}
data.setCameraModels(models);
StereoCameraModel stereoModel = data.stereoCameraModel();
if(stereoModel.isValidForProjection())
{
stereoModel.scale(1.0/double(_imageDecimation));
data.setStereoCameraModel(stereoModel);
}
}
if(info) info->timeImageDecimation = timer.ticks();
}
if(_mirroring && !data.imageRaw().empty() && data.cameraModels().size() == 1)
{
UDEBUG("");
UTimer timer;
cv::Mat tmpRgb;
cv::flip(data.imageRaw(), tmpRgb, 1);
data.setImageRaw(tmpRgb);
UASSERT_MSG(data.cameraModels().size() <= 1 && !data.stereoCameraModel().isValidForProjection(), "Only single RGBD cameras are supported for mirroring.");
if(data.cameraModels().size() && data.cameraModels()[0].cx())
{
CameraModel tmpModel(
data.cameraModels()[0].fx(),
data.cameraModels()[0].fy(),
float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
data.cameraModels()[0].cy(),
data.cameraModels()[0].localTransform());
data.setCameraModel(tmpModel);
}
if(!data.depthRaw().empty())
{
cv::Mat tmpDepth;
cv::flip(data.depthRaw(), tmpDepth, 1);
data.setDepthOrRightRaw(tmpDepth);
}
if(info) info->timeMirroring = timer.ticks();
}
if(_stereoToDepth && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection() && !data.rightRaw().empty())
{
UDEBUG("");
UTimer timer;
cv::Mat depth = util2d::depthFromDisparity(
_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
data.stereoCameraModel().left().fx(),
data.stereoCameraModel().baseline());
// set Tx for stereo bundle adjustment (when used)
CameraModel model = CameraModel(
data.stereoCameraModel().left().fx(),
data.stereoCameraModel().left().fy(),
data.stereoCameraModel().left().cx(),
data.stereoCameraModel().left().cy(),
data.stereoCameraModel().localTransform(),
-data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx());
data.setCameraModel(model);
data.setDepthOrRightRaw(depth);
data.setStereoCameraModel(StereoCameraModel());
if(info) info->timeDisparity = timer.ticks();
}
if(_scanFromDepth &&
data.cameraModels().size() &&
data.cameraModels().at(0).isValidForProjection() &&
!data.depthRaw().empty())
{
UDEBUG("");
if(data.laserScanRaw().empty())
{
UASSERT(_scanDecimation >= 1);
UTimer timer;
pcl::IndicesPtr validIndices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(
data,
_scanDecimation,
_scanMaxDepth,
_scanMinDepth,
validIndices.get());
float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
cv::Mat scan;
const Transform & baseToScan = data.cameraModels()[0].localTransform();
if(validIndices->size())
{
if(_scanVoxelSize>0.0f)
{
cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize);
float ratio = float(cloud->size()) / float(validIndices->size());
maxPoints = ratio * maxPoints;
}
else if(!cloud->is_dense)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
cloud = denseCloud;
}
if(cloud->size())
{
if(_scanNormalsK>0)
{
Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z());
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, viewPoint);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*cloud, *normals, *cloudNormals);
scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
}
else
{
scan = util3d::laserScanFromPointCloud(*cloud, baseToScan.inverse());
}
}
}
data.setLaserScanRaw(scan, LaserScanInfo((int)maxPoints, _scanMaxDepth, baseToScan));
if(info) info->timeScanFromDepth = timer.ticks();
}
else
{
UWARN("Option to create laser scan from depth image is enabled, but "
"there is already a laser scan in the captured sensor data. Scan from "
"depth will not be created.");
}
}
}
bool Camera::start()
{
//freopen("out.txt","w",stdout);
if(!capture_.isOpened() && images_.empty() && cameraTcpServer_ == 0)
{
if(Settings::getCamera_6useTcpCamera())
{
cameraTcpServer_ = new CameraTcpServer(Settings::getCamera_8port(), this);
if(!cameraTcpServer_->isListening())
{
UWARN("CameraTCP: Cannot listen to port %d", cameraTcpServer_->getPort());
delete cameraTcpServer_;
cameraTcpServer_ = 0;
}
else
{
UINFO("CameraTCP: listening to port %d (IP=%s)",
cameraTcpServer_->getPort(),
cameraTcpServer_->getHostAddress().toString().toStdString().c_str());
}
}
else
{
QString path = Settings::getCamera_5mediaPath();
if(UDirectory::exists(path.toStdString()))
{
//Images directory
QString ext = Settings::getGeneral_imageFormats();
ext.remove('*');
ext.remove('.');
UDirectory dir(path.toStdString(), ext.toStdString()); // this will load fileNames matching the extensions (in natural order)
const std::list<std::string> & fileNames = dir.getFileNames();
currentImageIndex_ = 0;
images_.clear();
// Modify to have full path
for(std::list<std::string>::const_iterator iter = fileNames.begin(); iter!=fileNames.end(); ++iter)
{
images_.append(path.toStdString() + UDirectory::separator() + *iter);
}
UINFO("Camera: Reading %d images from directory \"%s\"...", (int)images_.size(), path.toStdString().c_str());
if(images_.isEmpty())
{
UWARN("Camera: Directory \"%s\" is empty (no images matching the \"%s\" extensions). "
"If you want to disable loading automatically this directory, "
"clear the Camera/mediaPath parameter. By default, webcam will be used instead of the directory.",
path.toStdString().c_str(),
ext.toStdString().c_str());
}
}
else if(!path.isEmpty())
{
//Video file
capture_.open(path.toStdString().c_str());
if(!capture_.isOpened())
{
UWARN("Camera: Cannot open file \"%s\". If you want to disable loading "
"automatically this video file, clear the Camera/mediaPath parameter. "
"By default, webcam will be used instead of the file.", path.toStdString().c_str());
}
else
{
UINFO("Camera: Reading from video file \"%s\"...", path.toStdString().c_str());
}
}
if(!capture_.isOpened() && images_.empty())
{
//set camera device
capture_.open(Settings::getCamera_1deviceId());
if(Settings::getCamera_2imageWidth() && Settings::getCamera_3imageHeight())
{
capture_.set(CV_CAP_PROP_FRAME_WIDTH, double(Settings::getCamera_2imageWidth()));
capture_.set(CV_CAP_PROP_FRAME_HEIGHT, double(Settings::getCamera_3imageHeight()));
}
UINFO("Camera: Reading from camera device %d...", Settings::getCamera_1deviceId());
}
}
}
if(!capture_.isOpened() && images_.empty() && cameraTcpServer_ == 0)
{
UERROR("Camera: Failed to open a capture object!");
return false;
}
startTimer();
return true;
}
int main(int argc, char * argv[])
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kInfo);
//ULogger::setPrintTime(false);
//ULogger::setPrintWhere(false);
int driver = 0;
std::string stereoSavePath;
float rate = 0.0f;
std::string fourcc = "MJPG";
if(argc < 2)
{
showUsage();
}
else
{
for(int i=1; i<argc; ++i)
{
if(strcmp(argv[i], "-rate") == 0)
{
++i;
if(i < argc)
{
rate = uStr2Float(argv[i]);
if(rate < 0.0f)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-save_stereo") == 0)
{
++i;
if(i < argc)
{
stereoSavePath = argv[i];
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-fourcc") == 0)
{
++i;
if(i < argc)
{
fourcc = argv[i];
if(fourcc.size() != 4)
{
UERROR("fourcc should be 4 characters.");
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-help") == 0)
{
showUsage();
}
else if(i< argc-1)
{
printf("Unrecognized option \"%s\"", argv[i]);
showUsage();
}
// last
driver = atoi(argv[i]);
if(driver < 0 || driver > 8)
{
UERROR("driver should be between 0 and 8.");
showUsage();
}
}
}
UINFO("Using driver %d", driver);
rtabmap::Camera * camera = 0;
if(driver < 6)
{
if(!stereoSavePath.empty())
{
UWARN("-save_stereo option cannot be used with RGB-D drivers.");
stereoSavePath.clear();
}
if(driver == 0)
{
camera = new rtabmap::CameraOpenni();
}
else if(driver == 1)
{
if(!rtabmap::CameraOpenNI2::available())
{
UERROR("Not built with OpenNI2 support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNI2();
}
else if(driver == 2)
{
if(!rtabmap::CameraFreenect::available())
{
UERROR("Not built with Freenect support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect();
}
else if(driver == 3)
{
if(!rtabmap::CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(false);
}
else if(driver == 4)
{
if(!rtabmap::CameraOpenNICV::available())
{
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(true);
}
else if(driver == 5)
{
if(!rtabmap::CameraFreenect2::available())
{
UERROR("Not built with Freenect2 support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeColor2DepthSD);
}
}
else if(driver == 6)
{
if(!rtabmap::CameraStereoDC1394::available())
{
UERROR("Not built with DC1394 support...");
exit(-1);
}
camera = new rtabmap::CameraStereoDC1394();
}
else if(driver == 7)
{
if(!rtabmap::CameraStereoFlyCapture2::available())
{
UERROR("Not built with FlyCapture2/Triclops support...");
exit(-1);
}
camera = new rtabmap::CameraStereoFlyCapture2();
}
else if(driver == 8)
{
if(!rtabmap::CameraStereoZed::available())
{
UERROR("Not built with ZED sdk support...");
exit(-1);
}
camera = new rtabmap::CameraStereoZed(0);
}
else
{
UFATAL("");
}
if(!camera->init())
{
printf("Camera init failed! Please select another driver (see \"--help\").\n");
delete camera;
exit(1);
}
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.",
data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows);
}
pcl::visualization::CloudViewer * viewer = 0;
if(!data.stereoCameraModel().isValidForProjection() && (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValidForProjection()))
{
UWARN("Camera not calibrated! The registered cloud cannot be shown.");
}
else
{
viewer = new pcl::visualization::CloudViewer("cloud");
}
rtabmap::Transform t(1, 0, 0, 0,
0, -1, 0, 0,
0, 0, -1, 0);
cv::VideoWriter videoWriter;
UDirectory dir;
if(!stereoSavePath.empty() &&
!data.imageRaw().empty() &&
!data.rightRaw().empty())
{
if(UFile::getExtension(stereoSavePath).compare("avi") == 0)
{
if(data.imageRaw().size() == data.rightRaw().size())
{
if(rate <= 0)
{
UERROR("You should set the input rate when saving stereo images to a video file.");
showUsage();
}
cv::Size targetSize = data.imageRaw().size();
targetSize.width *= 2;
UASSERT(fourcc.size() == 4);
videoWriter.open(
stereoSavePath,
CV_FOURCC(fourcc.at(0), fourcc.at(1), fourcc.at(2), fourcc.at(3)),
rate,
targetSize,
data.imageRaw().channels() == 3);
}
else
{
UERROR("Images not the same size, cannot save stereo images to the video file.");
}
}
else if(UDirectory::exists(stereoSavePath))
{
UDirectory::makeDir(stereoSavePath+"/"+"left");
UDirectory::makeDir(stereoSavePath+"/"+"right");
}
else
{
UERROR("Directory \"%s\" doesn't exist.", stereoSavePath.c_str());
stereoSavePath.clear();
}
}
// to catch the ctrl-c
signal(SIGABRT, &sighandler);
signal(SIGTERM, &sighandler);
signal(SIGINT, &sighandler);
int id=1;
while(!data.imageRaw().empty() && (viewer==0 || !viewer->wasStopped()) && running)
{
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::util2d::cvtDepthFromFloat(depth);
}
if(rgb.cols == depth.cols && rgb.rows == depth.rows &&
data.cameraModels().size() &&
data.cameraModels()[0].isValidForProjection())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(
rgb, depth,
data.cameraModels()[0]);
cloud = rtabmap::util3d::transformPointCloud(cloud, t);
if(viewer)
viewer->showCloud(cloud, "cloud");
}
else if(!depth.empty() &&
data.cameraModels().size() &&
data.cameraModels()[0].isValidForProjection())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(
depth,
data.cameraModels()[0]);
cloud = rtabmap::util3d::transformPointCloud(cloud, t);
viewer->showCloud(cloud, "cloud");
}
cv::Mat tmp;
unsigned short min=0, max = 2048;
uMinMax((unsigned short*)depth.data, depth.rows*depth.cols, min, max);
depth.convertTo(tmp, CV_8UC1, 255.0/max);
cv::imshow("Video", rgb); // show frame
cv::imshow("Depth", tmp);
}
else if(!data.rightRaw().empty())
{
// stereo
cv::Mat right = data.rightRaw();
cv::imshow("Left", rgb); // show frame
cv::imshow("Right", right);
if(rgb.cols == right.cols && rgb.rows == right.rows && data.stereoCameraModel().isValidForProjection())
{
if(right.channels() == 3)
{
cv::cvtColor(right, right, CV_BGR2GRAY);
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromStereoImages(
rgb, right,
data.stereoCameraModel());
cloud = rtabmap::util3d::transformPointCloud(cloud, t);
if(viewer)
viewer->showCloud(cloud, "cloud");
}
}
int c = cv::waitKey(10); // wait 10 ms or for key stroke
if(c == 27)
break; // if ESC, break and quit
if(videoWriter.isOpened())
{
cv::Mat left = data.imageRaw();
cv::Mat right = data.rightRaw();
if(left.size() == right.size())
{
cv::Size targetSize = left.size();
targetSize.width *= 2;
cv::Mat targetImage(targetSize, left.type());
if(right.type() != left.type())
{
cv::Mat tmp;
cv::cvtColor(right, tmp, left.channels()==3?CV_GRAY2BGR:CV_BGR2GRAY);
right = tmp;
}
UASSERT(left.type() == right.type());
cv::Mat roiA(targetImage, cv::Rect( 0, 0, left.size().width, left.size().height ));
left.copyTo(roiA);
cv::Mat roiB( targetImage, cvRect( left.size().width, 0, left.size().width, left.size().height ) );
right.copyTo(roiB);
videoWriter.write(targetImage);
printf("Saved frame %d to \"%s\"\n", id, stereoSavePath.c_str());
}
else
{
UERROR("Left and right images are not the same size!?");
}
}
else if(!stereoSavePath.empty())
{
cv::imwrite(stereoSavePath+"/"+"left/"+uNumber2Str(id) + ".jpg", data.imageRaw());
cv::imwrite(stereoSavePath+"/"+"right/"+uNumber2Str(id) + ".jpg", data.rightRaw());
printf("Saved frames %d to \"%s/left\" and \"%s/right\" directories\n", id, stereoSavePath.c_str(), stereoSavePath.c_str());
}
++id;
data = camera->takeImage();
}
printf("Closing...\n");
if(viewer)
{
delete viewer;
}
cv::destroyWindow("Video");
cv::destroyWindow("Depth");
delete camera;
return 0;
}
// OpenGL thread
int RTABMapApp::Render()
{
// should be before clearSceneOnNextRender_ in case openDatabase is called
std::list<rtabmap::Statistics> rtabmapEvents;
{
boost::mutex::scoped_lock lock(rtabmapMutex_);
rtabmapEvents = rtabmapEvents_;
rtabmapEvents_.clear();
}
if(clearSceneOnNextRender_)
{
odomMutex_.lock();
odomEvents_.clear();
odomMutex_.unlock();
poseMutex_.lock();
poseEvents_.clear();
poseMutex_.unlock();
main_scene_.clear();
clearSceneOnNextRender_ = false;
createdMeshes_.clear();
rawPoses_.clear();
totalPoints_ = 0;
totalPolygons_ = 0;
lastDrawnCloudsCount_ = 0;
renderingFPS_ = 0.0f;
}
// Process events
rtabmap::Transform pose;
{
boost::mutex::scoped_lock lock(poseMutex_);
if(poseEvents_.size())
{
pose = poseEvents_.back();
poseEvents_.clear();
}
}
if(!pose.isNull())
{
// update camera pose?
main_scene_.SetCameraPose(pose);
}
bool notifyDataLoaded = false;
if(rtabmapEvents.size())
{
LOGI("Process rtabmap events");
// update buffered signatures
std::map<int, rtabmap::SensorData> bufferedSensorData;
if(!trajectoryMode_)
{
for(std::list<rtabmap::Statistics>::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter)
{
for(std::map<int, rtabmap::Signature>::const_iterator jter=iter->getSignatures().begin(); jter!=iter->getSignatures().end(); ++jter)
{
if(!jter->second.sensorData().imageRaw().empty() &&
!jter->second.sensorData().depthRaw().empty())
{
uInsert(bufferedSensorData, std::make_pair(jter->first, jter->second.sensorData()));
uInsert(rawPoses_, std::make_pair(jter->first, jter->second.getPose()));
}
else if(!jter->second.sensorData().imageCompressed().empty() &&
!jter->second.sensorData().depthOrRightCompressed().empty())
{
// uncompress
rtabmap::SensorData data = jter->second.sensorData();
cv::Mat tmpA,tmpB;
data.uncompressData(&tmpA, &tmpB);
uInsert(bufferedSensorData, std::make_pair(jter->first, data));
uInsert(rawPoses_, std::make_pair(jter->first, jter->second.getPose()));
notifyDataLoaded = true;
}
}
}
}
std::map<int, rtabmap::Transform> poses = rtabmapEvents.back().poses();
// Transform pose in OpenGL world
for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(!graphOptimization_)
{
std::map<int, rtabmap::Transform>::iterator jter = rawPoses_.find(iter->first);
if(jter != rawPoses_.end())
{
iter->second = opengl_world_T_rtabmap_world*jter->second;
}
}
else
{
iter->second = opengl_world_T_rtabmap_world*iter->second;
}
}
const std::multimap<int, rtabmap::Link> & links = rtabmapEvents.back().constraints();
if(poses.size())
{
//update graph
main_scene_.updateGraph(poses, links);
// update clouds
boost::mutex::scoped_lock lock(meshesMutex_);
std::set<std::string> strIds;
for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
int id = iter->first;
if(!iter->second.isNull())
{
if(main_scene_.hasCloud(id))
{
//just update pose
main_scene_.setCloudPose(id, iter->second);
main_scene_.setCloudVisible(id, true);
std::map<int, Mesh>::iterator meshIter = createdMeshes_.find(id);
if(meshIter!=createdMeshes_.end())
{
meshIter->second.pose = iter->second;
}
else
{
UERROR("Not found mesh %d !?!?", id);
}
}
else if(uContains(bufferedSensorData, id))
{
rtabmap::SensorData & data = bufferedSensorData.at(id);
if(!data.imageRaw().empty() && !data.depthRaw().empty())
{
// Voxelize and filter depending on the previous cloud?
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>);
LOGI("Creating node cloud %d (image size=%dx%d)", id, data.imageRaw().cols, data.imageRaw().rows);
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, data.imageRaw().rows/data.depthRaw().rows, maxCloudDepth_, 0, indices.get());
if(cloud->size() && indices->size())
{
UTimer time;
// pcl::organizedFastMesh doesn't take indices, so set to NaN points we don't need to mesh
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::ExtractIndices<pcl::PointXYZRGB> filter;
filter.setIndices(indices);
filter.setKeepOrganized(true);
filter.setInputCloud(cloud);
filter.filter(*output);
std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(output, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::Vertices> outputPolygons;
outputCloud = output;
outputPolygons = polygons;
LOGI("Creating mesh, %d polygons (%fs)", (int)outputPolygons.size(), time.ticks());
if(outputCloud->size() && outputPolygons.size())
{
totalPolygons_ += outputPolygons.size();
main_scene_.addCloud(id, outputCloud, outputPolygons, iter->second, data.imageRaw());
// protect createdMeshes_ used also by exportMesh() method
std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
UASSERT(inserted.second);
inserted.first->second.cloud = outputCloud;
inserted.first->second.polygons = outputPolygons;
inserted.first->second.pose = iter->second;
inserted.first->second.texture = data.imageCompressed();
}
else
{
LOGE("Not mesh could be created for node %d", id);
}
}
totalPoints_+=indices->size();
}
}
}
}
}
//filter poses?
if(poses.size() > 2)
{
if(nodesFiltering_)
{
for(std::multimap<int, rtabmap::Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() != rtabmap::Link::kNeighbor)
{
int oldId = iter->second.to()>iter->second.from()?iter->second.from():iter->second.to();
poses.erase(oldId);
}
}
}
}
//update cloud visibility
std::set<int> addedClouds = main_scene_.getAddedClouds();
for(std::set<int>::const_iterator iter=addedClouds.begin();
iter!=addedClouds.end();
++iter)
{
if(*iter > 0 && poses.find(*iter) == poses.end())
{
main_scene_.setCloudVisible(*iter, false);
}
}
}
else
{
rtabmap::OdometryEvent event;
bool set = false;
{
boost::mutex::scoped_lock lock(odomMutex_);
if(odomEvents_.size())
{
LOGI("Process odom events");
event = odomEvents_.back();
odomEvents_.clear();
set = true;
}
}
main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_);
//just process the last one
if(set && !event.pose().isNull())
{
if(odomCloudShown_ && !trajectoryMode_)
{
if(!event.data().imageRaw().empty() && !event.data().depthRaw().empty())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = rtabmap::util3d::cloudRGBFromSensorData(event.data(), event.data().imageRaw().rows/event.data().depthRaw().rows, maxCloudDepth_);
if(cloud->size())
{
LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
event.data().imageRaw().cols, event.data().imageRaw().rows,
event.data().depthRaw().cols, event.data().depthRaw().rows,
(int)cloud->width, (int)cloud->height);
std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
main_scene_.addCloud(-1, cloud, polygons, opengl_world_T_rtabmap_world*event.pose(), event.data().imageRaw());
main_scene_.setCloudVisible(-1, true);
}
else
{
LOGE("Generated cloud is empty!");
}
}
else
{
LOGE("Odom data images are empty!");
}
}
}
}
UTimer fpsTime;
lastDrawnCloudsCount_ = main_scene_.Render();
renderingFPS_ = 1.0/fpsTime.elapsed();
if(rtabmapEvents.size())
{
// send statistics to GUI
LOGI("Posting PostRenderEvent!");
UEventsManager::post(new PostRenderEvent(rtabmapEvents.back()));
}
return notifyDataLoaded?1:0;
}
SensorData DBReader::getNextData()
{
SensorData data;
if(_dbDriver)
{
float frameRate = _frameRate;
if(frameRate>0.0f)
{
int sleepTime = (1000.0f/frameRate - 1000.0f*_timer.getElapsedTime());
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
// Add precision at the cost of a small overhead
while(_timer.getElapsedTime() < 1.0/double(frameRate)-0.000001)
{
//
}
double slept = _timer.getElapsedTime();
_timer.start();
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(frameRate));
}
if(!this->isKilled() && _currentId != _ids.end())
{
cv::Mat imageBytes;
cv::Mat depthBytes;
cv::Mat laserScanBytes;
int mapId;
float fx,fy,cx,cy;
Transform localTransform, pose;
float variance = 1.0f;
_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform);
if(!_odometryIgnored)
{
_dbDriver->getPose(*_currentId, pose, mapId);
std::map<int, Link> links;
_dbDriver->loadLinks(*_currentId, links, Link::kNeighbor);
if(links.size())
{
// assume the first is the backward neighbor, take its variance
variance = links.begin()->second.variance();
}
}
int seq = *_currentId;
++_currentId;
if(imageBytes.empty())
{
UWARN("No image loaded from the database for id=%d!", *_currentId);
}
util3d::CompressionThread ctImage(imageBytes, true);
util3d::CompressionThread ctDepth(depthBytes, true);
util3d::CompressionThread ctLaserScan(laserScanBytes, false);
ctImage.start();
ctDepth.start();
ctLaserScan.start();
ctImage.join();
ctDepth.join();
ctLaserScan.join();
data = SensorData(
ctLaserScan.getUncompressedData(),
ctImage.getUncompressedData(),
ctDepth.getUncompressedData(),
fx,fy,cx,cy,
localTransform,
pose,
variance,
seq);
}
}
else
{
UERROR("Not initialized...");
}
return data;
}
bool RTABMapApp::exportMesh(const std::string & filePath)
{
bool success = false;
//Assemble the meshes
if(UFile::getExtension(filePath).compare("obj") == 0)
{
pcl::TextureMesh textureMesh;
std::vector<cv::Mat> textures;
int totalPolygons = 0;
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
{
boost::mutex::scoped_lock lock(meshesMutex_);
textureMesh.tex_materials.resize(createdMeshes_.size());
textureMesh.tex_polygons.resize(createdMeshes_.size());
textureMesh.tex_coordinates.resize(createdMeshes_.size());
textures.resize(createdMeshes_.size());
int polygonsStep = 0;
int oi = 0;
for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin();
iter!= createdMeshes_.end();
++iter)
{
UASSERT(!iter->second.cloud->is_dense);
if(!iter->second.texture.empty() &&
iter->second.cloud->size() &&
iter->second.polygons.size())
{
// OBJ format requires normals
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(iter->second.cloud, 20);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
pcl::concatenateFields(*iter->second.cloud, *normals, *cloudWithNormals);
// create dense cloud
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
std::vector<pcl::Vertices> densePolygons;
std::map<int, int> newToOldIndices;
newToOldIndices = rtabmap::util3d::filterNotUsedVerticesFromMesh(
*cloudWithNormals,
iter->second.polygons,
*denseCloud,
densePolygons);
// polygons
UASSERT(densePolygons.size());
unsigned int polygonSize = densePolygons.front().vertices.size();
textureMesh.tex_polygons[oi].resize(densePolygons.size());
textureMesh.tex_coordinates[oi].resize(densePolygons.size() * polygonSize);
for(unsigned int j=0; j<densePolygons.size(); ++j)
{
pcl::Vertices vertices = densePolygons[j];
UASSERT(polygonSize == vertices.vertices.size());
for(unsigned int k=0; k<vertices.vertices.size(); ++k)
{
//uv
std::map<int, int>::iterator jter = newToOldIndices.find(vertices.vertices[k]);
textureMesh.tex_coordinates[oi][j*vertices.vertices.size()+k] = Eigen::Vector2f(
float(jter->second % iter->second.cloud->width) / float(iter->second.cloud->width), // u
float(iter->second.cloud->height - jter->second / iter->second.cloud->width) / float(iter->second.cloud->height)); // v
vertices.vertices[k] += polygonsStep;
}
textureMesh.tex_polygons[oi][j] = vertices;
}
totalPolygons += densePolygons.size();
polygonsStep += denseCloud->size();
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(denseCloud, iter->second.pose);
if(mergedClouds->size() == 0)
{
*mergedClouds = *transformedCloud;
}
else
{
*mergedClouds += *transformedCloud;
}
textures[oi] = iter->second.texture;
textureMesh.tex_materials[oi].tex_illum = 1;
textureMesh.tex_materials[oi].tex_name = uFormat("material_%d", iter->first);
++oi;
}
else
{
UERROR("Texture not set for mesh %d", iter->first);
}
}
textureMesh.tex_materials.resize(oi);
textureMesh.tex_polygons.resize(oi);
textures.resize(oi);
if(textures.size())
{
pcl::toPCLPointCloud2(*mergedClouds, textureMesh.cloud);
std::string textureDirectory = uSplit(filePath, '.').front();
UINFO("Saving %d textures to %s.", textures.size(), textureDirectory.c_str());
UDirectory::makeDir(textureDirectory);
for(unsigned int i=0;i<textures.size(); ++i)
{
cv::Mat rawImage = rtabmap::uncompressImage(textures[i]);
std::string texFile = textureDirectory+"/"+textureMesh.tex_materials[i].tex_name+".png";
cv::imwrite(texFile, rawImage);
UINFO("Saved %s (%d bytes).", texFile.c_str(), rawImage.total()*rawImage.channels());
// relative path
textureMesh.tex_materials[i].tex_file = uSplit(UFile::getName(filePath), '.').front()+"/"+textureMesh.tex_materials[i].tex_name+".png";
}
UINFO("Saving obj (%d vertices, %d polygons) to %s.", (int)mergedClouds->size(), totalPolygons, filePath.c_str());
success = pcl::io::saveOBJFile(filePath, textureMesh) == 0;
if(success)
{
UINFO("Saved obj to %s!", filePath.c_str());
}
else
{
UERROR("Failed saving obj to %s!", filePath.c_str());
}
}
}
}
else
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::Vertices> mergedPolygons;
{
boost::mutex::scoped_lock lock(meshesMutex_);
for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin();
iter!= createdMeshes_.end();
++iter)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::Vertices> densePolygons;
rtabmap::util3d::filterNotUsedVerticesFromMesh(
*iter->second.cloud,
iter->second.polygons,
*denseCloud,
densePolygons);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(denseCloud, iter->second.pose);
if(mergedClouds->size() == 0)
{
*mergedClouds = *transformedCloud;
mergedPolygons = densePolygons;
}
else
{
rtabmap::util3d::appendMesh(*mergedClouds, mergedPolygons, *transformedCloud, densePolygons);
}
}
}
if(mergedClouds->size() && mergedPolygons.size())
{
pcl::PolygonMesh mesh;
pcl::toPCLPointCloud2(*mergedClouds, mesh.cloud);
mesh.polygons = mergedPolygons;
UINFO("Saving ply (%d vertices, %d polygons) to %s.", (int)mergedClouds->size(), (int)mergedPolygons.size(), filePath.c_str());
success = pcl::io::savePLYFileBinary(filePath, mesh) == 0;
if(success)
{
UINFO("Saved ply to %s!", filePath.c_str());
}
else
{
UERROR("Failed saving ply to %s!", filePath.c_str());
}
}
}
return success;
}
/*********************************
Fonction RecordFichier
Permet d'enregistrer un fichier WAV
*********************************/
void UAudioCaptureMic::mainLoop()
{
if(!_sound)
{
UERROR("Recorder is not initialized.");
this->kill();
return;
}
FMOD_RESULT result;
void *ptr1 = 0, *ptr2 = 0;
int blockLength;
unsigned int len1 = 0, len2 = 0;
unsigned int recordPos = 0;
result = UAudioSystem::getRecordPosition(_driver, &recordPos); UASSERT_MSG(result==FMOD_OK, FMOD_ErrorString(result));
if (recordPos != _lastRecordPos)
{
blockLength = (int)recordPos - (int)_lastRecordPos;
if (blockLength < 0)
{
blockLength += _soundLength;
}
// * exinfo.numchannels * 2 = stereo 16bit. 1 sample = 4 bytes.
// Lock the sound to get access to the raw data.
FMOD_Sound_Lock(_sound, _lastRecordPos * channels() * bytesPerSample(), blockLength * channels() * bytesPerSample(), &ptr1, &ptr2, &len1, &len2);
{
if (ptr1 && len1) // Write it to disk.
{
if(_fp)
{
//write to file
_dataLength += fwrite(ptr1, 1, len1, _fp);
}
// push back in the frames buffer
pushBackSamples(ptr1, len1);
}
if (ptr2 && len2) // Write it to disk.
{
if(_fp)
{
//write to file
_dataLength += fwrite(ptr2, 1, len2, _fp);
}
// push back in the frames buffer
pushBackSamples(ptr2, len2);
}
}
//Unlock the sound to allow FMOD to use it again.
FMOD_Sound_Unlock(_sound, ptr1, ptr2, len1, len2);
_lastRecordPos = recordPos;
}
UAudioSystem::update();
uSleep(10);
// If we are recording to a file, make sure to stop
// when the maximum file size is reached
if (_fp && _maxFileSize != 0 && int(_dataLength) + frameLength()*bytesPerSample() > _maxFileSize)
{
UWARN("Recording max memory reached (%d Mb)... stopped", _maxFileSize/1000000);
this->kill();
}
}
void RTABMapApp::handleEvent(UEvent * event)
{
if(camera_ && camera_->isRunning())
{
// called from events manager thread, so protect the data
if(event->getClassName().compare("OdometryEvent") == 0)
{
LOGI("Received OdometryEvent!");
if(odomMutex_.try_lock())
{
odomEvents_.clear();
if(camera_->isRunning())
{
odomEvents_.push_back(*((rtabmap::OdometryEvent*)(event)));
}
odomMutex_.unlock();
}
}
if(status_.first == rtabmap::RtabmapEventInit::kInitialized &&
event->getClassName().compare("RtabmapEvent") == 0)
{
LOGI("Received RtabmapEvent!");
if(camera_->isRunning())
{
boost::mutex::scoped_lock lock(rtabmapMutex_);
rtabmapEvents_.push_back(((rtabmap::RtabmapEvent*)event)->getStats());
}
}
}
if(event->getClassName().compare("PoseEvent") == 0)
{
if(poseMutex_.try_lock())
{
poseEvents_.clear();
poseEvents_.push_back(((rtabmap::PoseEvent*)event)->pose());
poseMutex_.unlock();
}
}
if(event->getClassName().compare("CameraTangoEvent") == 0)
{
rtabmap::CameraTangoEvent * tangoEvent = (rtabmap::CameraTangoEvent*)event;
// Call JAVA callback with tango event msg
bool success = false;
if(jvm && RTABMapActivity)
{
JNIEnv *env = 0;
jint rs = jvm->AttachCurrentThread(&env, NULL);
if(rs == JNI_OK && env)
{
jclass clazz = env->GetObjectClass(RTABMapActivity);
if(clazz)
{
jmethodID methodID = env->GetMethodID(clazz, "tangoEventCallback", "(ILjava/lang/String;Ljava/lang/String;)V" );
if(methodID)
{
env->CallVoidMethod(RTABMapActivity, methodID,
tangoEvent->type(),
env->NewStringUTF(tangoEvent->key().c_str()),
env->NewStringUTF(tangoEvent->value().c_str()));
success = true;
}
}
}
jvm->DetachCurrentThread();
}
if(!success)
{
UERROR("Failed to call RTABMapActivity::tangoEventCallback");
}
}
if(event->getClassName().compare("RtabmapEventInit") == 0)
{
LOGI("Received RtabmapEventInit!");
status_.first = ((rtabmap::RtabmapEventInit*)event)->getStatus();
status_.second = ((rtabmap::RtabmapEventInit*)event)->getInfo();
if(status_.first == rtabmap::RtabmapEventInit::kClosed)
{
clearSceneOnNextRender_ = true;
}
// Call JAVA callback with init msg
bool success = false;
if(jvm && RTABMapActivity)
{
JNIEnv *env = 0;
jint rs = jvm->AttachCurrentThread(&env, NULL);
if(rs == JNI_OK && env)
{
jclass clazz = env->GetObjectClass(RTABMapActivity);
if(clazz)
{
jmethodID methodID = env->GetMethodID(clazz, "rtabmapInitEventCallback", "(ILjava/lang/String;)V" );
if(methodID)
{
env->CallVoidMethod(RTABMapActivity, methodID,
status_.first,
env->NewStringUTF(status_.second.c_str()));
success = true;
}
}
}
jvm->DetachCurrentThread();
}
if(!success)
{
UERROR("Failed to call RTABMapActivity::rtabmapInitEventsCallback");
}
}
if(event->getClassName().compare("PostRenderEvent") == 0)
{
LOGI("Received PostRenderEvent!");
const rtabmap::Statistics & stats = ((PostRenderEvent*)event)->getStats();
int nodes = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryWorking_memory_size(), 0.0f) +
uValue(stats.data(), rtabmap::Statistics::kMemoryShort_time_memory_size(), 0.0f);
int words = (int)uValue(stats.data(), rtabmap::Statistics::kKeypointDictionary_size(), 0.0f);
float updateTime = uValue(stats.data(), rtabmap::Statistics::kTimingTotal(), 0.0f);
int loopClosureId = stats.loopClosureId()>0?stats.loopClosureId():stats.proximityDetectionId()>0?stats.proximityDetectionId():0;
int highestHypId = (int)uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_id(), 0.0f);
int databaseMemoryUsed = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryDatabase_memory_used(), 0.0f);
int inliers = (int)uValue(stats.data(), rtabmap::Statistics::kLoopVisual_inliers(), 0.0f);
int rejected = (int)uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f);
int featuresExtracted = stats.getSignatures().size()?stats.getSignatures().rbegin()->second.getWords().size():0;
float hypothesis = uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f);
// Call JAVA callback with some stats
UINFO("Send statistics to GUI");
bool success = false;
if(jvm && RTABMapActivity)
{
JNIEnv *env = 0;
jint rs = jvm->AttachCurrentThread(&env, NULL);
if(rs == JNI_OK && env)
{
jclass clazz = env->GetObjectClass(RTABMapActivity);
if(clazz)
{
jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIFIFI)V" );
if(methodID)
{
env->CallVoidMethod(RTABMapActivity, methodID,
nodes,
words,
totalPoints_,
totalPolygons_,
updateTime,
loopClosureId,
highestHypId,
databaseMemoryUsed,
inliers,
featuresExtracted,
hypothesis,
lastDrawnCloudsCount_,
renderingFPS_,
rejected);
success = true;
}
}
}
jvm->DetachCurrentThread();
}
if(!success)
{
UERROR("Failed to call RTABMapActivity::updateStatsCallback");
}
}
}
void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat & imageRight, const QString & cameraName)
{
processingData_ = true;
if(cameraName_.isEmpty())
{
cameraName_ = "0000";
if(!cameraName.isEmpty())
{
cameraName_ = cameraName;
}
}
if(ui_->label_serial->text().isEmpty())
{
ui_->label_serial->setText(cameraName_);
}
std::vector<cv::Mat> inputRawImages(2);
if(ui_->checkBox_switchImages->isChecked())
{
inputRawImages[0] = imageRight;
inputRawImages[1] = imageLeft;
}
else
{
inputRawImages[0] = imageLeft;
inputRawImages[1] = imageRight;
}
std::vector<cv::Mat> images(2);
images[0] = inputRawImages[0];
images[1] = inputRawImages[1];
imageSize_[0] = images[0].size();
imageSize_[1] = images[1].size();
bool boardFound[2] = {false};
bool boardAccepted[2] = {false};
bool readyToCalibrate[2] = {false};
std::vector<std::vector<cv::Point2f> > pointBuf(2);
bool depthDetected = false;
for(int id=0; id<(stereo_?2:1); ++id)
{
cv::Mat viewGray;
if(!images[id].empty())
{
if(images[id].type() == CV_16UC1)
{
depthDetected = true;
//assume IR image: convert to gray scaled
const float factor = 255.0f / float((maxIrs_[id] - minIrs_[id]));
viewGray = cv::Mat(images[id].rows, images[id].cols, CV_8UC1);
for(int i=0; i<images[id].rows; ++i)
{
for(int j=0; j<images[id].cols; ++j)
{
viewGray.at<unsigned char>(i, j) = (unsigned char)std::min(float(std::max(images[id].at<unsigned short>(i,j) - minIrs_[id], 0)) * factor, 255.0f);
}
}
cvtColor(viewGray, images[id], cv::COLOR_GRAY2BGR); // convert to show detected points in color
}
else if(images[id].channels() == 3)
{
cvtColor(images[id], viewGray, cv::COLOR_BGR2GRAY);
}
else
{
viewGray = images[id];
cvtColor(viewGray, images[id], cv::COLOR_GRAY2BGR); // convert to show detected points in color
}
}
else
{
UERROR("Image %d is empty!! Should not!", id);
}
minIrs_[id] = 0;
maxIrs_[id] = 0x7FFF;
//Dot it only if not yet calibrated
if(!ui_->pushButton_save->isEnabled())
{
cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
if(!viewGray.empty())
{
int flags = CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE;
if(!viewGray.empty())
{
int maxScale = viewGray.cols < 640?2:1;
for( int scale = 1; scale <= maxScale; scale++ )
{
cv::Mat timg;
if( scale == 1 )
timg = viewGray;
else
cv::resize(viewGray, timg, cv::Size(), scale, scale, CV_INTER_CUBIC);
boardFound[id] = cv::findChessboardCorners(timg, boardSize, pointBuf[id], flags);
if(boardFound[id])
{
if( scale > 1 )
{
cv::Mat cornersMat(pointBuf[id]);
cornersMat *= 1./scale;
}
break;
}
}
}
}
if(boardFound[id]) // If done with success,
{
// improve the found corners' coordinate accuracy for chessboard
float minSquareDistance = -1.0f;
for(unsigned int i=0; i<pointBuf[id].size()-1; ++i)
{
float d = cv::norm(pointBuf[id][i] - pointBuf[id][i+1]);
if(minSquareDistance == -1.0f || minSquareDistance > d)
{
minSquareDistance = d;
}
}
float radius = minSquareDistance/2.0f +0.5f;
cv::cornerSubPix( viewGray, pointBuf[id], cv::Size(radius, radius), cv::Size(-1,-1),
cv::TermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1 ));
// Draw the corners.
cv::drawChessboardCorners(images[id], boardSize, cv::Mat(pointBuf[id]), boardFound[id]);
std::vector<float> params(4,0);
getParams(pointBuf[id], boardSize, imageSize_[id], params[0], params[1], params[2], params[3]);
bool addSample = true;
for(unsigned int i=0; i<imageParams_[id].size(); ++i)
{
if(fabs(params[0] - imageParams_[id][i].at(0)) < 0.1 && // x
fabs(params[1] - imageParams_[id][i].at(1)) < 0.1 && // y
fabs(params[2] - imageParams_[id][i].at(2)) < 0.05 && // size
fabs(params[3] - imageParams_[id][i].at(3)) < 0.1) // skew
{
addSample = false;
}
}
if(addSample)
{
boardAccepted[id] = true;
imagePoints_[id].push_back(pointBuf[id]);
imageParams_[id].push_back(params);
UINFO("[%d] Added board, total=%d. (x=%f, y=%f, size=%f, skew=%f)", id, (int)imagePoints_[id].size(), params[0], params[1], params[2], params[3]);
}
// update statistics
std::vector<float> xRange(2, imageParams_[id][0].at(0));
std::vector<float> yRange(2, imageParams_[id][0].at(1));
std::vector<float> sizeRange(2, imageParams_[id][0].at(2));
std::vector<float> skewRange(2, imageParams_[id][0].at(3));
for(unsigned int i=1; i<imageParams_[id].size(); ++i)
{
xRange[0] = imageParams_[id][i].at(0) < xRange[0] ? imageParams_[id][i].at(0) : xRange[0];
xRange[1] = imageParams_[id][i].at(0) > xRange[1] ? imageParams_[id][i].at(0) : xRange[1];
yRange[0] = imageParams_[id][i].at(1) < yRange[0] ? imageParams_[id][i].at(1) : yRange[0];
yRange[1] = imageParams_[id][i].at(1) > yRange[1] ? imageParams_[id][i].at(1) : yRange[1];
sizeRange[0] = imageParams_[id][i].at(2) < sizeRange[0] ? imageParams_[id][i].at(2) : sizeRange[0];
sizeRange[1] = imageParams_[id][i].at(2) > sizeRange[1] ? imageParams_[id][i].at(2) : sizeRange[1];
skewRange[0] = imageParams_[id][i].at(3) < skewRange[0] ? imageParams_[id][i].at(3) : skewRange[0];
skewRange[1] = imageParams_[id][i].at(3) > skewRange[1] ? imageParams_[id][i].at(3) : skewRange[1];
}
//UINFO("Stats [%d]:", id);
//UINFO(" Count = %d", (int)imagePoints_[id].size());
//UINFO(" x = [%f -> %f]", xRange[0], xRange[1]);
//UINFO(" y = [%f -> %f]", yRange[0], yRange[1]);
//UINFO(" size = [%f -> %f]", sizeRange[0], sizeRange[1]);
//UINFO(" skew = [%f -> %f]", skewRange[0], skewRange[1]);
float xGood = xRange[1] - xRange[0];
float yGood = yRange[1] - yRange[0];
float sizeGood = sizeRange[1] - sizeRange[0];
float skewGood = skewRange[1] - skewRange[0];
if(id == 0)
{
ui_->progressBar_x->setValue(xGood*100);
ui_->progressBar_y->setValue(yGood*100);
ui_->progressBar_size->setValue(sizeGood*100);
ui_->progressBar_skew->setValue(skewGood*100);
if((int)imagePoints_[id].size() > ui_->progressBar_count->maximum())
{
ui_->progressBar_count->setMaximum((int)imagePoints_[id].size());
}
ui_->progressBar_count->setValue((int)imagePoints_[id].size());
}
else
{
ui_->progressBar_x_2->setValue(xGood*100);
ui_->progressBar_y_2->setValue(yGood*100);
ui_->progressBar_size_2->setValue(sizeGood*100);
ui_->progressBar_skew_2->setValue(skewGood*100);
if((int)imagePoints_[id].size() > ui_->progressBar_count_2->maximum())
{
ui_->progressBar_count_2->setMaximum((int)imagePoints_[id].size());
}
ui_->progressBar_count_2->setValue((int)imagePoints_[id].size());
}
if(imagePoints_[id].size() >= COUNT_MIN && xGood > 0.5 && yGood > 0.5 && sizeGood > 0.4 && skewGood > 0.5)
{
readyToCalibrate[id] = true;
}
//update IR values
if(inputRawImages[id].type() == CV_16UC1)
{
//update min max IR if the chessboard was found
minIrs_[id] = 0xFFFF;
maxIrs_[id] = 0;
for(size_t i = 0; i < pointBuf[id].size(); ++i)
{
const cv::Point2f &p = pointBuf[id][i];
cv::Rect roi(std::max(0, (int)p.x - 3), std::max(0, (int)p.y - 3), 6, 6);
roi.width = std::min(roi.width, inputRawImages[id].cols - roi.x);
roi.height = std::min(roi.height, inputRawImages[id].rows - roi.y);
//find minMax in the roi
double min, max;
cv::minMaxLoc(inputRawImages[id](roi), &min, &max);
if(min < minIrs_[id])
{
minIrs_[id] = min;
}
if(max > maxIrs_[id])
{
maxIrs_[id] = max;
}
}
}
}
}
}
ui_->label_baseline->setVisible(!depthDetected);
ui_->label_baseline_name->setVisible(!depthDetected);
if(stereo_ && ((boardAccepted[0] && boardFound[1]) || (boardAccepted[1] && boardFound[0])))
{
stereoImagePoints_[0].push_back(pointBuf[0]);
stereoImagePoints_[1].push_back(pointBuf[1]);
UINFO("Add stereo image points (size=%d)", (int)stereoImagePoints_[0].size());
}
if(!stereo_ && readyToCalibrate[0])
{
ui_->pushButton_calibrate->setEnabled(true);
}
else if(stereo_ && readyToCalibrate[0] && readyToCalibrate[1] && stereoImagePoints_[0].size())
{
ui_->pushButton_calibrate->setEnabled(true);
}
if(ui_->radioButton_rectified->isChecked())
{
if(models_[0].isValid())
{
images[0] = models_[0].rectifyImage(images[0]);
}
if(models_[1].isValid())
{
images[1] = models_[1].rectifyImage(images[1]);
}
}
else if(ui_->radioButton_stereoRectified->isChecked() &&
(stereoModel_.left().isValid() &&
stereoModel_.right().isValid()&&
(!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0)))
{
images[0] = stereoModel_.left().rectifyImage(images[0]);
images[1] = stereoModel_.right().rectifyImage(images[1]);
}
if(ui_->checkBox_showHorizontalLines->isChecked())
{
for(int id=0; id<(stereo_?2:1); ++id)
{
int step = imageSize_[id].height/16;
for(int i=step; i<imageSize_[id].height; i+=step)
{
cv::line(images[id], cv::Point(0, i), cv::Point(imageSize_[id].width, i), CV_RGB(0,255,0));
}
}
}
ui_->label_left->setText(tr("%1x%2").arg(images[0].cols).arg(images[0].rows));
//show frame
ui_->image_view->setImage(uCvMat2QImage(images[0]).mirrored(ui_->checkBox_mirror->isChecked(), false));
if(stereo_)
{
ui_->label_right->setText(tr("%1x%2").arg(images[1].cols).arg(images[1].rows));
ui_->image_view_2->setImage(uCvMat2QImage(images[1]).mirrored(ui_->checkBox_mirror->isChecked(), false));
}
processingData_ = false;
}
void LoopClosureViewer::updateView(const Transform & transform, const ParametersMap & parameters)
{
if(sA_.id()>0 && sB_.id()>0)
{
int decimation = 1;
float maxDepth = 0;
float minDepth = 0;
if(!ui_->checkBox_rawCloud->isChecked())
{
decimation = decimation_;
maxDepth = maxDepth_;
minDepth = minDepth_;
}
UDEBUG("decimation = %d", decimation);
UDEBUG("maxDepth = %f", maxDepth);
UDEBUG("minDepth = %d", minDepth);
Transform t;
if(!transform.isNull())
{
transform_ = transform;
t = transform;
}
else if(!transform_.isNull())
{
t = transform_;
}
else
{
t = sB_.getPose();
}
UDEBUG("t= %s", t.prettyPrint().c_str());
ui_->label_transform->setText(QString("(%1)").arg(t.prettyPrint().c_str()));
if(!t.isNull())
{
//cloud 3d
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA, cloudB;
cloudA = util3d::cloudRGBFromSensorData(sA_.sensorData(), decimation, maxDepth, minDepth, 0, parameters);
cloudB = util3d::cloudRGBFromSensorData(sB_.sensorData(), decimation, maxDepth, minDepth, 0, parameters);
//cloud 2d
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = util3d::laserScanToPointCloud(sA_.sensorData().laserScanRaw(), sA_.sensorData().laserScanRaw().localTransform());
scanB = util3d::laserScanToPointCloud(sB_.sensorData().laserScanRaw(), sB_.sensorData().laserScanRaw().localTransform());
ui_->label_idA->setText(QString("[%1 (%2) -> %3 (%4)]").arg(sB_.id()).arg(cloudB->size()).arg(sA_.id()).arg(cloudA->size()));
if(cloudA->size())
{
ui_->cloudViewerTransform->addCloud("cloud0", cloudA);
}
if(cloudB->size())
{
cloudB = util3d::transformPointCloud(cloudB, t);
ui_->cloudViewerTransform->addCloud("cloud1", cloudB);
}
if(scanA->size())
{
ui_->cloudViewerTransform->addCloud("scan0", scanA);
}
if(scanB->size())
{
scanB = util3d::transformPointCloud(scanB, t);
ui_->cloudViewerTransform->addCloud("scan1", scanB);
}
}
else
{
UERROR("loop transform is null !?!?");
ui_->cloudViewerTransform->removeAllClouds();
}
ui_->cloudViewerTransform->update();
}
void computeDescriptors( const cv::Mat& image,
std::vector<cv::KeyPoint>& keypoints,
cv::Mat& descriptors)
{
UERROR("GPUFAST:computeDescriptors() Should not be used!");
}
void DBReader::mainLoop()
{
OdometryEvent odom = this->getNextData();
if(odom.data().id())
{
std::string goalId;
double previousStamp = odom.data().stamp();
if(previousStamp == 0)
{
odom.data().setStamp(UTimer::now());
}
if(!_goalsIgnored &&
odom.data().userDataRaw().type() == CV_8SC1 &&
odom.data().userDataRaw().cols >= 7 && // including null str ending
odom.data().userDataRaw().rows == 1 &&
memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)
{
//GOAL format detected, remove it from the user data and send it as goal event
std::string goalStr = (const char *)odom.data().userDataRaw().data;
if(!goalStr.empty())
{
std::list<std::string> strs = uSplit(goalStr, ':');
if(strs.size() == 2)
{
goalId = *strs.rbegin();
odom.data().setUserData(cv::Mat());
}
}
}
if(!_odometryIgnored)
{
if(odom.pose().isNull())
{
UWARN("Reading the database: odometry is null! "
"Please set \"Ignore odometry = true\" if there is "
"no odometry in the database.");
}
this->post(new OdometryEvent(odom));
}
else
{
this->post(new CameraEvent(odom.data()));
}
if(!goalId.empty())
{
double delay = 0.0;
if(!_ignoreGoalDelay && _currentId != _ids.end())
{
// get stamp for the next signature to compute the delay
// that was used originally for planning
int weight;
std::string label;
double stamp;
int mapId;
Transform localTransform, pose;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp);
if(previousStamp && stamp && stamp > previousStamp)
{
delay = stamp - previousStamp;
}
}
if(delay > 0.0)
{
UWARN("Goal \"%s\" detected, posting it! Waiting %f seconds before sending next data...",
goalId.c_str(), delay);
}
else
{
UWARN("Goal \"%s\" detected, posting it!", goalId.c_str());
}
if(uIsInteger(goalId))
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, atoi(goalId.c_str())));
}
else
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId));
}
if(delay > 0.0)
{
uSleep(delay*1000);
}
}
}
else if(!this->isKilled())
{
UINFO("no more images...");
if(_paths.size() > 1)
{
_paths.pop_front();
UWARN("Loading next database \"%s\"...", _paths.front().c_str());
if(!this->init())
{
UERROR("Failed to initialize the next database \"%s\"", _paths.front().c_str());
this->kill();
this->post(new CameraEvent());
}
}
else
{
this->kill();
this->post(new CameraEvent());
}
}
}
int main(int argc, char * argv[])
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kInfo);
int device = 0;
std::string path;
float rate = 0.0f;
std::string calibrationFile;
for(int i=1; i<argc; ++i)
{
if(strcmp(argv[i], "--rate") == 0)
{
++i;
if(i < argc)
{
rate = uStr2Float(argv[i]);
if(rate < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "--device") == 0)
{
++i;
if(i < argc)
{
device = std::atoi(argv[i]);
if(device < 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "--path") == 0)
{
++i;
if(i < argc)
{
path = argv[i];
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "--calibration") == 0)
{
++i;
if(i < argc)
{
calibrationFile = argv[i];
}
else
{
showUsage();
}
continue;
}
printf("Unrecognized option : %s\n", argv[i]);
showUsage();
}
if(path.empty())
{
UINFO("Using device %d", device);
}
else
{
UINFO("Using path %s", path.c_str());
}
rtabmap::Camera * camera = 0;
rtabmap::DBReader * dbReader = 0;
if(!path.empty())
{
if(UFile::exists(path))
{
if(UFile::getExtension(path).compare("db") == 0)
{
dbReader = new rtabmap::DBReader(path, rate);
}
else
{
camera = new rtabmap::CameraVideo(path, rate);
}
}
else if(UDirectory::exists(path))
{
camera = new rtabmap::CameraImages(path, rate);
}
else
{
UERROR("Path not valid! \"%s\"", path.c_str());
return -1;
}
}
else
{
camera = new rtabmap::CameraVideo(device, rate);
}
if(camera)
{
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(dbReader)
{
if(!dbReader->init())
{
delete dbReader;
UERROR("Cannot initialize the camera.");
return -1;
}
}
cv::Mat rgb;
rgb = camera?camera->takeImage().imageRaw():dbReader->getNextData().data().imageRaw();
cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window
while(!rgb.empty())
{
cv::imshow("Video", rgb); // show frame
int c = cv::waitKey(10); // wait 10 ms or for key stroke
if(c == 27)
break; // if ESC, break and quit
rgb = camera?camera->takeImage().imageRaw():dbReader->getNextData().data().imageRaw();
}
cv::destroyWindow("Video");
if(camera)
{
delete camera;
}
if(dbReader)
{
delete dbReader;
}
return 0;
}
OdometryEvent DBReader::getNextData()
{
OdometryEvent odom;
if(_dbDriver)
{
if(!this->isKilled() && _currentId != _ids.end())
{
int mapId;
SensorData data;
_dbDriver->getNodeData(*_currentId, data);
// info
Transform pose;
int weight;
std::string label;
double stamp;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp);
cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
if(!_odometryIgnored)
{
std::map<int, Link> links;
_dbDriver->loadLinks(*_currentId, links, Link::kNeighbor);
if(links.size())
{
// assume the first is the backward neighbor, take its variance
infMatrix = links.begin()->second.infMatrix();
}
}
else
{
pose.setNull();
}
int seq = *_currentId;
++_currentId;
if(data.imageCompressed().empty())
{
UWARN("No image loaded from the database for id=%d!", *_currentId);
}
// Frame rate
if(_frameRate < 0.0f)
{
if(stamp == 0)
{
UERROR("The option to use database stamps is set (framerate<0), but there are no stamps saved in the database! Aborting...");
this->kill();
}
else if(_previousMapID == mapId && _previousStamp > 0)
{
int sleepTime = 1000.0*(stamp-_previousStamp) - 1000.0*_timer.getElapsedTime();
if(sleepTime > 10000)
{
UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.",
sleepTime/1000, _previousStamp, stamp);
sleepTime = 10000;
}
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
// Add precision at the cost of a small overhead
while(_timer.getElapsedTime() < (stamp-_previousStamp)-0.000001)
{
//
}
double slept = _timer.getElapsedTime();
_timer.start();
UDEBUG("slept=%fs vs target=%fs", slept, stamp-_previousStamp);
}
_previousStamp = stamp;
_previousMapID = mapId;
}
else if(_frameRate>0.0f)
{
int sleepTime = (1000.0f/_frameRate - 1000.0f*_timer.getElapsedTime());
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
// Add precision at the cost of a small overhead
while(_timer.getElapsedTime() < 1.0/double(_frameRate)-0.000001)
{
//
}
double slept = _timer.getElapsedTime();
_timer.start();
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_frameRate));
}
if(!this->isKilled())
{
data.uncompressData();
data.setId(seq);
data.setStamp(stamp);
UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d",
data.laserScanRaw().empty()?0:1,
data.imageRaw().empty()?0:1,
data.depthOrRightRaw().empty()?0:1,
data.userDataRaw().empty()?0:1);
odom = OdometryEvent(data, pose, infMatrix.inv());
}
}
}
else
{
UERROR("Not initialized...");
}
return odom;
}
// return not null transform if odometry is correctly computed
Transform OdometryDVO::computeTransform(
SensorData & data,
const Transform & guess,
OdometryInfo * info)
{
Transform t;
#ifdef RTABMAP_DVO
UTimer timer;
if(data.imageRaw().empty() ||
data.imageRaw().rows != data.depthOrRightRaw().rows ||
data.imageRaw().cols != data.depthOrRightRaw().cols)
{
UERROR("Not supported input!");
return t;
}
if(!(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForReprojection()))
{
UERROR("Invalid camera model! Only single RGB-D camera supported by DVO. Try another odometry approach.");
return t;
}
if(dvo_ == 0)
{
dvo::DenseTracker::Config cfg = dvo::DenseTracker::getDefaultConfig();
dvo_ = new dvo::DenseTracker(cfg);
}
cv::Mat grey, grey_s16, depth_inpainted, depth_mask, depth_mono, depth_float;
if(data.imageRaw().type() != CV_32FC1)
{
if(data.imageRaw().type() == CV_8UC3)
{
cv::cvtColor(data.imageRaw(), grey, CV_BGR2GRAY);
}
else
{
grey = data.imageRaw();
}
grey.convertTo(grey_s16, CV_32F);
}
else
{
grey_s16 = data.imageRaw();
}
// make sure all zeros are NAN
if(data.depthRaw().type() == CV_32FC1)
{
depth_float = data.depthRaw();
for(int i=0; i<depth_float.rows; ++i)
{
for(int j=0; j<depth_float.cols; ++j)
{
float & d = depth_float.at<float>(i,j);
if(d == 0.0f)
{
d = NAN;
}
}
}
}
else if(data.depthRaw().type() == CV_16UC1)
{
depth_float = cv::Mat(data.depthRaw().size(), CV_32FC1);
for(int i=0; i<data.depthRaw().rows; ++i)
{
for(int j=0; j<data.depthRaw().cols; ++j)
{
float d = float(data.depthRaw().at<unsigned short>(i,j))/1000.0f;
depth_float.at<float>(i, j) = d==0.0f?NAN:d;
}
}
}
else
{
UFATAL("Unknown depth format!");
}
if(camera_ == 0)
{
dvo::core::IntrinsicMatrix intrinsics = dvo::core::IntrinsicMatrix::create(
data.cameraModels()[0].fx(),
data.cameraModels()[0].fy(),
data.cameraModels()[0].cx(),
data.cameraModels()[0].cy());
camera_ = new dvo::core::RgbdCameraPyramid(
data.cameraModels()[0].imageWidth(),
data.cameraModels()[0].imageHeight(),
intrinsics);
}
dvo::core::RgbdImagePyramid * current = new dvo::core::RgbdImagePyramid(*camera_, grey_s16, depth_float);
cv::Mat covariance;
if(reference_ == 0)
{
reference_ = current;
if(!lost_)
{
t.setIdentity();
}
covariance = cv::Mat::eye(6,6,CV_64FC1) * 9999.0;
}
else
{
dvo::DenseTracker::Result result;
dvo_->match(*reference_, *current, result);
t = Transform::fromEigen3d(result.Transformation);
if(result.Information(0,0) > 0.0 && result.Information(0,0) != 1.0)
{
lost_ = false;
cv::Mat information = cv::Mat::eye(6,6, CV_64FC1);
memcpy(information.data, result.Information.data(), 36*sizeof(double));
covariance = information.inv();
covariance *= 100.0; // to be in the same scale than loop closure detection
Transform currentMotion = t;
t = motionFromKeyFrame_.inverse() * t;
// TODO make parameters?
if(currentMotion.getNorm() > 0.01 || currentMotion.getAngle() > 0.01)
{
if(info)
{
info->keyFrameAdded = true;
}
// new keyframe
delete reference_;
reference_ = current;
motionFromKeyFrame_.setIdentity();
}
else
{
delete current;
motionFromKeyFrame_ = currentMotion;
}
}
else
{
lost_ = true;
delete reference_;
delete current;
reference_ = 0; // this will make restart from the next frame
motionFromKeyFrame_.setIdentity();
t.setNull();
covariance = cv::Mat::eye(6,6,CV_64FC1) * 9999.0;
UWARN("dvo failed to estimate motion, tracking will be reinitialized on next frame.");
}
}
const Transform & localTransform = data.cameraModels()[0].localTransform();
if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
{
// from camera frame to base frame
t = localTransform * t * localTransform.inverse();
}
if(info)
{
info->type = (int)kTypeDVO;
info->covariance = covariance;
}
UINFO("Odom update time = %fs", timer.elapsed());
#else
UERROR("RTAB-Map is not built with DVO support! Select another visual odometry approach.");
#endif
return t;
}
// return not null transform if odometry is correctly computed
Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo * info)
{
UTimer timer;
Transform output;
bool hasConverged = false;
double variance = 0;
unsigned int minPoints = 100;
if(!data.depth().empty())
{
if(data.depth().type() == CV_8UC1)
{
UERROR("ICP 3D cannot be done on stereo images!");
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
data.depth(),
data.fx(),
data.fy(),
data.cx(),
data.cy(),
_decimation,
this->getMaxDepth(),
_voxelSize,
_samples,
data.localTransform());
if(_pointToPlane)
{
pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
std::vector<int> indices;
newCloud = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(newCloud);
if(newCloudXYZ->size() != newCloud->size())
{
UWARN("removed nan normals...");
}
if(_previousCloudNormal->size() > minPoints && newCloud->size() > minPoints)
{
int correspondences = 0;
Transform transform = util3d::icpPointToPlane(newCloud,
_previousCloudNormal,
_maxCorrespondenceDistance,
_maxIterations,
&hasConverged,
&variance,
&correspondences);
// verify if there are enough correspondences
float correspondencesRatio = float(correspondences)/float(_previousCloudNormal->size()>newCloud->size()?_previousCloudNormal->size():newCloud->size());
if(!transform.isNull() && hasConverged &&
correspondencesRatio >= _correspondenceRatio)
{
output = transform;
_previousCloudNormal = newCloud;
}
else
{
UWARN("Transform not valid (hasConverged=%s variance = %f)",
hasConverged?"true":"false", variance);
}
}
else if(newCloud->size() > minPoints)
{
output.setIdentity();
_previousCloudNormal = newCloud;
}
}
else
{
//point to point
if(_previousCloud->size() > minPoints && newCloudXYZ->size() > minPoints)
{
int correspondences = 0;
Transform transform = util3d::icp(newCloudXYZ,
_previousCloud,
_maxCorrespondenceDistance,
_maxIterations,
&hasConverged,
&variance,
&correspondences);
// verify if there are enough correspondences
float correspondencesRatio = float(correspondences)/float(_previousCloud->size()>newCloudXYZ->size()?_previousCloud->size():newCloudXYZ->size());
if(!transform.isNull() && hasConverged &&
correspondencesRatio >= _correspondenceRatio)
{
output = transform;
_previousCloud = newCloudXYZ;
}
else
{
UWARN("Transform not valid (hasConverged=%s variance = %f)",
hasConverged?"true":"false", variance);
}
}
else if(newCloudXYZ->size() > minPoints)
{
output.setIdentity();
_previousCloud = newCloudXYZ;
}
}
}
else
{
UERROR("Depth is empty?!?");
}
if(info)
{
info->variance = variance;
}
UINFO("Odom update time = %fs hasConverged=%s variance=%f cloud=%d",
timer.elapsed(),
hasConverged?"true":"false",
variance,
(int)(_pointToPlane?_previousCloudNormal->size():_previousCloud->size()));
return output;
}
