i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Integration of CLAMS depth calibration #114

Browse Source
This commit is contained in:
matlabbe
2016-09-19 14:01:06 -04:00
parent 861cc437f4
commit 3434b95d68
30 changed files with 2867 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/CameraInfo.h
+4
View File
@@ -45,6 +45,8 @@ public:
timeMirroring(0.0f),
timeImageDecimation(0.0f),
timeScanFromDepth(0.0f),
timeUndistortDepth(0.0f),
timeTotal(0.0f),
odomCovariance(cv::Mat::eye(6,6,CV_64FC1))
{
}
@@ -58,6 +60,8 @@ public:
float timeMirroring;
float timeImageDecimation;
float timeScanFromDepth;
float timeUndistortDepth;
float timeTotal;
Transform odomPose;
cv::Mat odomCovariance;
};
corelib/include/rtabmap/core/CameraModel.h
+9 -1
View File
@@ -76,7 +76,8 @@ public:
void initRectificationMap();
bool isValidForProjection() const {return fx()>0.0 && fy()>0.0;}
bool isValidForProjection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0;}
bool isValidForReprojection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0 && imageWidth()>0 && imageHeight()>0;}
bool isValidForRectification() const
{
return imageSize_.width>0 &&
@@ -124,6 +125,13 @@ public:
cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const;
cv::Mat rectifyDepth(const cv::Mat & raw) const;
// Project 2D pixel to 3D (in /camera_link frame)
void project(float u, float v, float depth, float & x, float & y, float & z) const;
// Reproject 3D point (in /camera_link frame) to pixel
void reproject(float x, float y, float z, float & u, float & v) const;
void reproject(float x, float y, float z, int & u, int & v) const;
bool inFrame(int u, int v) const;
private:
std::string name_;
cv::Size imageSize_;
corelib/include/rtabmap/core/CameraThread.h
+8 -1
View File
@@ -33,6 +33,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UThread.h>
#include <rtabmap/utilite/UEventsSender.h>
namespace clams
{
class DiscreteDepthDistortionModel;
}
namespace rtabmap
{
@@ -56,6 +61,8 @@ public:
void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
void setImageDecimation(int decimation) {_imageDecimation = decimation;}
void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
void setImageRate(float imageRate);
void setDistortionModel(const std::string & path);
void setScanFromDepth(
bool enabled,
@@ -74,7 +81,6 @@ public:
//getters
bool isPaused() const {return !this->isRunning();}
bool isCapturing() const {return this->isRunning();}
void setImageRate(float imageRate);
Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted
@@ -95,6 +101,7 @@ private:
float _scanVoxelSize;
int _scanNormalsK;
StereoDense * _stereoDense;
clams::DiscreteDepthDistortionModel * _distortionModel;
};
} // namespace rtabmap
corelib/include/rtabmap/core/clams/discrete_depth_distortion_model.h
+130
View File
@@ -0,0 +1,130 @@
/*
Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the <organization> nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
RTAB-Map integration: Mathieu Labbe
*/
#ifndef DISCRETE_DEPTH_DISTORTION_MODEL_H
#define DISCRETE_DEPTH_DISTORTION_MODEL_H
#include <assert.h>
#include <vector>
#include <Eigen/Core>
#include <opencv2/opencv.hpp>
#include <rtabmap/utilite/UMutex.h>
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
namespace clams
{
class RTABMAP_EXP DiscreteFrustum
{
public:
DiscreteFrustum(int smoothing = 1, double bin_depth = 1.0);
//! z value, not distance to origin.
//! thread-safe.
void addExample(double ground_truth, double measurement);
int index(double z) const;
void undistort(double* z) const;
void interpolatedUndistort(double* z) const;
void serialize(std::ostream& out) const;
void deserialize(std::istream& in);
protected:
double max_dist_;
int num_bins_;
double bin_depth_;
Eigen::VectorXf counts_;
Eigen::VectorXf total_numerators_;
Eigen::VectorXf total_denominators_;
Eigen::VectorXf multipliers_;
friend class DiscreteDepthDistortionModel;
};
class RTABMAP_EXP DiscreteDepthDistortionModel
{
public:
DiscreteDepthDistortionModel() :
width_(0),
height_(0),
bin_width_(0),
bin_height_(0),
bin_depth_(0),
num_bins_x_(0),
num_bins_y_(0),
training_samples_(0)
{}
virtual ~DiscreteDepthDistortionModel();
DiscreteDepthDistortionModel(int width, int height, int bin_width = 8, int bin_height = 6, double bin_depth = 2.0, int smoothing = 1);
DiscreteDepthDistortionModel(const DiscreteDepthDistortionModel& other);
DiscreteDepthDistortionModel& operator=(const DiscreteDepthDistortionModel& other);
void undistort(cv::Mat & depth) const;
//! Returns the number of training examples it used from this pair.
//! Thread-safe.
size_t accumulate(const cv::Mat& ground_truth, const cv::Mat& measurement);
void addExample(int v, int u, double ground_truth, double measurement);
void save(const std::string& path) const;
void load(const std::string& path);
void serialize(std::ostream& out) const;
void deserialize(std::istream& in);
cv::Mat visualize(const std::string& path = "") const;
int getWidth() const {return width_;}
int getHeight() const {return height_;}
size_t getTrainingSamples() const {return training_samples_;}
bool isValid() const
{
return !frustums_.empty();
}
protected:
//! Image width.
int width_;
//! Image height.
int height_;
//! Width of each bin in pixels.
int bin_width_;
//! Height of each bin in pixels.
int bin_height_;
//! Depth of each bin in meters.
double bin_depth_;
int num_bins_x_;
int num_bins_y_;
//! frustums_[y][x]
std::vector< std::vector<DiscreteFrustum*> > frustums_;
size_t training_samples_;
void deleteFrustums();
DiscreteFrustum& frustum(int y, int x);
const DiscreteFrustum& frustum(int y, int x) const;
UMutex mutex_;
};
} // namespace clams
#endif // DISCRETE_DEPTH_DISTORTION_MODEL_H
corelib/include/rtabmap/core/clams/frame_projector.h
+96
View File
@@ -0,0 +1,96 @@
/*
Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the <organization> nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
RTAB-Map integration: Mathieu Labbe
*/
#ifndef FRAME_PROJECTOR_H
#define FRAME_PROJECTOR_H
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <opencv2/core/core.hpp>
#include <rtabmap/core/CameraModel.h>
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#define MAX_MULT 1.3
#define MIN_MULT 0.7
namespace clams
{
//! "Projective" point comes from the OpenNI terminology, and refers to (u, v, z), i.e.
//! pixel id and depth value. Here I've added color, too, so that this represents everything
//! that is known about a pixel in an RBGD camera.
class ProjectivePoint
{
public:
ProjectivePoint() :
u_(0),
v_(0),
z_(0.0f) {}
int u_;
int v_;
float z_; // in meters
};
//! This is essentially a pinhole camera model for an RGBD sensor, with
//! some extra functions added on for use during calibration.
class RTABMAP_EXP FrameProjector
{
public:
// For storing z values in meters. This is not Euclidean distance.
typedef std::vector< std::vector< std::vector<double> > > RangeIndex;
FrameProjector(const rtabmap::CameraModel & model);
RangeIndex cloudToRangeIndex(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pcd) const;
//! transform is applied to the map, then projected into a depth index.
//! The best depth estimate from the map corresponding to the measurement depth frame
//! will be returned.
cv::Mat estimateMapDepth(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & map,
const rtabmap::Transform & transform,
const cv::Mat & measurement,
double coneRadius = 0.02,
double coneStdevThresh = 0.03) const;
pcl::PointXYZ project(const ProjectivePoint& ppt) const;
ProjectivePoint reproject(const pcl::PointXYZ& pt) const;
protected:
bool coneFit(const cv::Size& imageSize, const RangeIndex& rindex,
int uc, int vc, double radius, double measurement_depth,
double* mean, double* stdev) const;
private:
rtabmap::CameraModel model_;
};
} // namespace clams
#endif // FRAME_PROJECTOR_H
corelib/include/rtabmap/core/clams/slam_calibrator.h
+52
View File
@@ -0,0 +1,52 @@
/*
Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the <organization> nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
RTAB-Map integration: Mathieu Labbe
*/
#ifndef SLAM_CALIBRATOR_H
#define SLAM_CALIBRATOR_H
#include <rtabmap/core/clams/discrete_depth_distortion_model.h>
#include <rtabmap/core/SensorData.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
namespace clams
{
DiscreteDepthDistortionModel RTABMAP_EXP calibrate(
const std::map<int, rtabmap::SensorData> & sequence,
const std::map<int, rtabmap::Transform> & trajectory,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & map,
double coneRadius = 0.02,
double coneStdevThresh = 0.03);
}
#endif // SLAM_CALIBRATOR_H
corelib/src/CMakeLists.txt
+6
View File
@@ -74,6 +74,12 @@ SET(SRC_FILES
FlannIndex.cpp
sqlite3/sqlite3.c
#clams stuff
clams/discrete_depth_distortion_model_helpers.cpp
clams/discrete_depth_distortion_model.cpp
clams/frame_projector.cpp
clams/slam_calibrator.cpp
)
IF(OpenCV_VERSION_MAJOR EQUAL 2)
corelib/src/CameraModel.cpp
+61 -4
View File
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UMath.h>
#include <opencv2/imgproc/imgproc.hpp>
namespace rtabmap {
@@ -75,9 +76,19 @@ CameraModel::CameraModel(
{
UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str());
UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str());
UASSERT_MSG(cx > 0.0 || imageSize.width>0, uFormat("cx=%f imageSize.width=%d", cx, imageSize.width).c_str());
UASSERT_MSG(cy > 0.0 || imageSize.height>0, uFormat("cy=%f imageSize.height=%d", cy, imageSize.height).c_str());
UASSERT(!localTransform.isNull());
if(cx<=0.0)
{
cx = double(imageSize.width)/2.0-0.5;
}
if(cy<=0.0)
{
cy = double(imageSize.height)/2.0-0.5;
}
if(Tx != 0.0)
{
P_ = cv::Mat::eye(3, 4, CV_64FC1),
@@ -110,9 +121,19 @@ CameraModel::CameraModel(
{
UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str());
UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str());
UASSERT_MSG(cx > 0.0 || imageSize.width>0, uFormat("cx=%f imageSize.width=%d", cx, imageSize.width).c_str());
UASSERT_MSG(cy > 0.0 || imageSize.height>0, uFormat("cy=%f imageSize.height=%d", cy, imageSize.height).c_str());
UASSERT(!localTransform.isNull());
if(cx<=0.0)
{
cx = double(imageSize.width)/2.0-0.5;
}
if(cy<=0.0)
{
cy = double(imageSize.height)/2.0-0.5;
}
if(Tx != 0.0)
{
P_ = cv::Mat::eye(3, 4, CV_64FC1),
@@ -501,4 +522,40 @@ cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const
}
}
// resulting 3D point is in /camera_link frame
void CameraModel::project(float u, float v, float depth, float & x, float & y, float & z) const
{
if(depth > 0.0f)
{
// Fill in XYZ
x = (u - cx()) * depth / fx();
y = (v - cy()) * depth / fy();
z = depth;
}
else
{
x = y = z = std::numeric_limits<float>::quiet_NaN();
}
}
// 3D point is in /camera_link frame
void CameraModel::reproject(float x, float y, float z, float & u, float & v) const
{
UASSERT(z!=0.0f);
float invZ = 1.0f/z;
u = (fx()*x)*invZ + cx();
v = (fy()*y)*invZ + cy();
}
void CameraModel::reproject(float x, float y, float z, int & u, int & v) const
{
UASSERT(z!=0.0f);
float invZ = 1.0f/z;
u = (fx()*x)*invZ + cx();
v = (fy()*y)*invZ + cy();
}
bool CameraModel::inFrame(int u, int v) const
{
return uIsInBounds(u, 0, imageWidth()) && uIsInBounds(v, 0, imageHeight());
}
} /* namespace rtabmap */
corelib/src/CameraThread.cpp
+49 -1
View File
@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_surface.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/StereoDense.h"
#include "rtabmap/core/clams/discrete_depth_distortion_model.h"
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/ULogger.h>
@@ -56,7 +57,8 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
_scanMinDepth(0.0f),
_scanVoxelSize(0.0f),
_scanNormalsK(0),
_stereoDense(new StereoBM(parameters))
_stereoDense(new StereoBM(parameters)),
_distortionModel(0)
{
UASSERT(_camera != 0);
}
@@ -69,6 +71,10 @@ CameraThread::~CameraThread()
{
delete _camera;
}
if(_distortionModel)
{
delete _distortionModel;
}
delete _stereoDense;
}
@@ -80,8 +86,29 @@ void CameraThread::setImageRate(float imageRate)
}
}
void CameraThread::setDistortionModel(const std::string & path)
{
if(_distortionModel)
{
delete _distortionModel;
_distortionModel = 0;
}
if(!path.empty())
{
_distortionModel = new clams::DiscreteDepthDistortionModel();
_distortionModel->load(path);
if(!_distortionModel->isValid())
{
UERROR("Loaded distortion model \"%s\" is not valid!", path.c_str());
delete _distortionModel;
_distortionModel = 0;
}
}
}
void CameraThread::mainLoop()
{
UTimer totalTime;
UDEBUG("");
CameraInfo info;
SensorData data = _camera->takeImage(&info);
@@ -92,6 +119,26 @@ void CameraThread::mainLoop()
{
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);
}
info.timeUndistortDepth = timer.ticks();
}
if(_imageDecimation>1 && !data.imageRaw().empty())
{
UDEBUG("");
@@ -228,6 +275,7 @@ void CameraThread::mainLoop()
}
info.cameraName = _camera->getSerial();
info.timeTotal = totalTime.ticks();
this->post(new CameraEvent(data, info));
}
else if(!this->isKilled())
corelib/src/clams/discrete_depth_distortion_model.cpp
+344
View File
@@ -0,0 +1,344 @@
/*
Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the <organization> nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
RTAB-Map integration: Mathieu Labbe
*/
#include "rtabmap/core/clams/discrete_depth_distortion_model.h"
#include "rtabmap/core/clams/frame_projector.h"
#include <rtabmap/utilite/ULogger.h>
#include "eigen_extensions/eigen_extensions.h"
using namespace std;
using namespace Eigen;
namespace clams
{
DiscreteFrustum::DiscreteFrustum(int smoothing, double bin_depth) :
max_dist_(10),
bin_depth_(bin_depth)
{
num_bins_ = ceil(max_dist_ / bin_depth_);
counts_ = VectorXf::Ones(num_bins_) * smoothing;
total_numerators_ = VectorXf::Ones(num_bins_) * smoothing;
total_denominators_ = VectorXf::Ones(num_bins_) * smoothing;
multipliers_ = VectorXf::Ones(num_bins_);
}
void DiscreteFrustum::addExample(double ground_truth, double measurement)
{
double mult = ground_truth / measurement;
if(mult > MAX_MULT || mult < MIN_MULT)
return;
int idx = min(num_bins_ - 1, (int)floor(measurement / bin_depth_));
UASSERT(idx >= 0);
total_numerators_(idx) += ground_truth * ground_truth;
total_denominators_(idx) += ground_truth * measurement;
++counts_(idx);
multipliers_(idx) = total_numerators_(idx) / total_denominators_(idx);
}
inline int DiscreteFrustum::index(double z) const
{
return min(num_bins_ - 1, (int)floor(z / bin_depth_));
}
inline void DiscreteFrustum::undistort(double* z) const
{
*z *= multipliers_.coeffRef(index(*z));
}
void DiscreteFrustum::interpolatedUndistort(double* z) const
{
int idx = index(*z);
double start = bin_depth_ * idx;
int idx1;
if(*z - start < bin_depth_ / 2)
idx1 = idx;
else
idx1 = idx + 1;
int idx0 = idx1 - 1;
if(idx0 < 0 || idx1 >= num_bins_ || counts_(idx0) < 50 || counts_(idx1) < 50) {
undistort(z);
return;
}
double z0 = (idx0 + 1) * bin_depth_ - bin_depth_ * 0.5;
double coeff1 = (*z - z0) / bin_depth_;
double coeff0 = 1.0 - coeff1;
double mult = coeff0 * multipliers_.coeffRef(idx0) + coeff1 * multipliers_.coeffRef(idx1);
*z *= mult;
}
void DiscreteFrustum::serialize(std::ostream& out) const
{
eigen_extensions::serializeScalar(max_dist_, out);
eigen_extensions::serializeScalar(num_bins_, out);
eigen_extensions::serializeScalar(bin_depth_, out);
eigen_extensions::serialize(counts_, out);
eigen_extensions::serialize(total_numerators_, out);
eigen_extensions::serialize(total_denominators_, out);
eigen_extensions::serialize(multipliers_, out);
}
void DiscreteFrustum::deserialize(std::istream& in)
{
eigen_extensions::deserializeScalar(in, &max_dist_);
eigen_extensions::deserializeScalar(in, &num_bins_);
eigen_extensions::deserializeScalar(in, &bin_depth_);
eigen_extensions::deserialize(in, &counts_);
eigen_extensions::deserialize(in, &total_numerators_);
eigen_extensions::deserialize(in, &total_denominators_);
eigen_extensions::deserialize(in, &multipliers_);
}
DiscreteDepthDistortionModel::DiscreteDepthDistortionModel(const DiscreteDepthDistortionModel& other)
{
*this = other;
}
DiscreteDepthDistortionModel& DiscreteDepthDistortionModel::operator=(const DiscreteDepthDistortionModel& other)
{
width_ = other.width_;
height_ = other.height_;
bin_width_ = other.bin_width_;
bin_height_ = other.bin_height_;
bin_depth_ = other.bin_depth_;
num_bins_x_ = other.num_bins_x_;
num_bins_y_ = other.num_bins_y_;
training_samples_ = other.training_samples_;
frustums_ = other.frustums_;
for(size_t i = 0; i < frustums_.size(); ++i)
for(size_t j = 0; j < frustums_[i].size(); ++j)
frustums_[i][j] = new DiscreteFrustum(*other.frustums_[i][j]);
return *this;
}
DiscreteDepthDistortionModel::DiscreteDepthDistortionModel(int width, int height,
int bin_width, int bin_height,
double bin_depth,
int smoothing) :
width_(width),
height_(height),
bin_width_(bin_width),
bin_height_(bin_height),
bin_depth_(bin_depth)
{
UASSERT(width_ % bin_width_ == 0);
UASSERT(height_ % bin_height_ == 0);
num_bins_x_ = width_ / bin_width_;
num_bins_y_ = height_ / bin_height_;
frustums_.resize(num_bins_y_);
for(size_t i = 0; i < frustums_.size(); ++i) {
frustums_[i].resize(num_bins_x_, NULL);
for(size_t j = 0; j < frustums_[i].size(); ++j)
frustums_[i][j] = new DiscreteFrustum(smoothing, bin_depth);
}
training_samples_ = 0;
}
void DiscreteDepthDistortionModel::deleteFrustums()
{
for(size_t y = 0; y < frustums_.size(); ++y)
for(size_t x = 0; x < frustums_[y].size(); ++x)
if(frustums_[y][x])
delete frustums_[y][x];
training_samples_ = 0;
}
DiscreteDepthDistortionModel::~DiscreteDepthDistortionModel()
{
deleteFrustums();
}
void DiscreteDepthDistortionModel::undistort(cv::Mat & depth) const
{
UASSERT(width_ == depth.cols);
UASSERT(height_ ==depth.rows);
UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
if(depth.type() == CV_32FC1)
{
#pragma omp parallel for
for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) {
float & z = depth.at<float>(v, u);
if(z == 0.0f)
continue;
double zf = z;
frustum(v, u).interpolatedUndistort(&zf);
z = zf;
}
}
}
else
{
#pragma omp parallel for
for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) {
unsigned short & z = depth.at<unsigned short>(v, u);
if(z == 0)
continue;
double zf = z * 0.001;
frustum(v, u).interpolatedUndistort(&zf);
z = zf*1000;
}
}
}
}
void DiscreteDepthDistortionModel::addExample(int v, int u, double ground_truth, double measurement)
{
frustum(v, u).addExample(ground_truth, measurement);
}
size_t DiscreteDepthDistortionModel::accumulate(const cv::Mat& ground_truth,
const cv::Mat& measurement)
{
UASSERT(width_ == ground_truth.cols);
UASSERT(height_ == ground_truth.rows);
UASSERT(width_ == measurement.cols);
UASSERT(height_ == measurement.rows);
UASSERT(ground_truth.type() == CV_16UC1 || ground_truth.type() == CV_32FC1);
UASSERT(measurement.type() == CV_16UC1 || measurement.type() == CV_32FC1);
bool isGroundTruthInMM = ground_truth.type()==CV_16UC1;
bool isMeasurementInMM = measurement.type()==CV_16UC1;
size_t num_training_examples = 0;
for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) {
float gt = isGroundTruthInMM?float(ground_truth.at<unsigned short>(v,u))*0.001:ground_truth.at<float>(v,u);
if(gt == 0)
continue;
float meas = isMeasurementInMM?float(measurement.at<unsigned short>(v,u))*0.001:measurement.at<float>(v,u);
if(meas == 0)
continue;
UScopeMutex sm(mutex_);
frustum(v, u).addExample(gt, meas);
++num_training_examples;
}
}
training_samples_ += num_training_examples;
return num_training_examples;
}
void DiscreteDepthDistortionModel::load(const std::string& path)
{
ifstream f;
f.open(path.c_str());
if(!f.is_open()) {
cerr << "Failed to open " << path << endl;
assert(f.is_open());
}
deserialize(f);
f.close();
}
void DiscreteDepthDistortionModel::save(const std::string& path) const
{
ofstream f;
f.open(path.c_str());
if(!f.is_open()) {
cerr << "Failed to open " << path << endl;
assert(f.is_open());
}
serialize(f);
f.close();
}
void DiscreteDepthDistortionModel::serialize(std::ostream& out) const
{
out << "DiscreteDepthDistortionModel v01" << endl;
eigen_extensions::serializeScalar(width_, out);
eigen_extensions::serializeScalar(height_, out);
eigen_extensions::serializeScalar(bin_width_, out);
eigen_extensions::serializeScalar(bin_height_, out);
eigen_extensions::serializeScalar(bin_depth_, out);
eigen_extensions::serializeScalar(num_bins_x_, out);
eigen_extensions::serializeScalar(num_bins_y_, out);
eigen_extensions::serializeScalar(training_samples_, out);
for(int y = 0; y < num_bins_y_; ++y)
for(int x = 0; x < num_bins_x_; ++x)
frustums_[y][x]->serialize(out);
}
void DiscreteDepthDistortionModel::deserialize(std::istream& in)
{
string buf;
getline(in, buf);
assert(buf == "DiscreteDepthDistortionModel v01");
eigen_extensions::deserializeScalar(in, &width_);
eigen_extensions::deserializeScalar(in, &height_);
eigen_extensions::deserializeScalar(in, &bin_width_);
eigen_extensions::deserializeScalar(in, &bin_height_);
eigen_extensions::deserializeScalar(in, &bin_depth_);
eigen_extensions::deserializeScalar(in, &num_bins_x_);
eigen_extensions::deserializeScalar(in, &num_bins_y_);
eigen_extensions::deserializeScalar(in, &training_samples_);
deleteFrustums();
frustums_.resize(num_bins_y_);
for(size_t y = 0; y < frustums_.size(); ++y) {
frustums_[y].resize(num_bins_x_, NULL);
for(size_t x = 0; x < frustums_[y].size(); ++x) {
frustums_[y][x] = new DiscreteFrustum;
frustums_[y][x]->deserialize(in);
}
}
}
DiscreteFrustum& DiscreteDepthDistortionModel::frustum(int y, int x)
{
UASSERT(x >= 0 && x < width_);
UASSERT(y >= 0 && y < height_);
int xidx = x / bin_width_;
int yidx = y / bin_height_;
return (*frustums_[yidx][xidx]);
}
const DiscreteFrustum& DiscreteDepthDistortionModel::frustum(int y, int x) const
{
UASSERT(x >= 0 && x < width_);
UASSERT(y >= 0 && y < height_);
int xidx = x / bin_width_;
int yidx = y / bin_height_;
return (*frustums_[yidx][xidx]);
}
} // namespace clams
corelib/src/clams/discrete_depth_distortion_model_helpers.cpp
+207
View File
@@ -0,0 +1,207 @@
/*
Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the <organization> nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
RTAB-Map integration: Mathieu Labbe
*/
#include <rtabmap/core/clams/discrete_depth_distortion_model.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UDirectory.h>
using namespace std;
using namespace Eigen;
namespace clams
{
cv::Mat DiscreteDepthDistortionModel::visualize(const std::string& dir) const
{
bool writeFiles = !dir.empty() && UDirectory::exists(dir);
const DiscreteFrustum& reference_frustum = *frustums_[0][0];
int num_layers = reference_frustum.num_bins_;
// -- Set up for combined imagery.
int horiz_divider = 10;
int vert_divider = 20;
cv::Mat3b overview(cv::Size(width_ * 2 + horiz_divider, height_ * num_layers + vert_divider * (num_layers + 2)), cv::Vec3b(0, 0, 0));
vector<int> pub_layers;
for(int i = 0; i < num_layers; ++i)
pub_layers.push_back(i);
// pub_layers.push_back(1);
// pub_layers.push_back(2);
// pub_layers.push_back(3);
cv::Mat3b pub(cv::Size(width_, height_ * pub_layers.size() + vert_divider * (pub_layers.size() + 2)), cv::Vec3b(255, 255, 255));
for(int i = 0; i < num_layers; ++i) {
// -- Determine the path to save the image for this layer.
char buffer[50];
float mindepth = reference_frustum.bin_depth_ * i;
float maxdepth = reference_frustum.bin_depth_ * (i + 1);
sprintf(buffer, "%05.2f-%05.2f", mindepth, maxdepth);
ostringstream oss;
oss << dir << "/multipliers_" << buffer << ".png";
// -- Compute the multipliers visualization for this layer.
// Multiplier of 1 is black, >1 is red, <1 is blue. Think redshift.
cv::Mat3b mult(cv::Size(width_, height_), cv::Vec3b(0, 0, 0));
for(int y = 0; y < mult.rows; ++y) {
for(int x = 0; x < mult.cols; ++x) {
const DiscreteFrustum& frustum = *frustums_[y / bin_height_][x / bin_width_];
float val = frustum.multipliers_(i);
if(val > 1)
mult(y, x)[2] = min(255., 255 * (val - 1.0) / 0.25);
if(val < 1)
mult(y, x)[0] = min(255., 255 * (1.0 - val) / 0.25);
}
}
if(writeFiles)
{
cv::imwrite(oss.str(), mult);
UINFO("Written \"%s\"", oss.str().c_str());
}
// -- Compute the counts visualization for this layer.
// 0 is black, 100 is white.
cv::Mat3b count(cv::Size(width_, height_), cv::Vec3b(0, 0, 0));
for(int y = 0; y < count.rows; ++y) {
for(int x = 0; x < count.cols; ++x) {
const DiscreteFrustum& frustum = *frustums_[y / bin_height_][x / bin_width_];
uchar val = min(255., (double)(255 * frustum.counts_(i) / 100));
count(y, x)[0] = val;
count(y, x)[1] = val;
count(y, x)[2] = val;
}
}
oss.str("");
oss << dir << "/counts_" << buffer << ".png";
if(writeFiles)
{
cv::imwrite(oss.str(), count);
UINFO("Written \"%s\"", oss.str().c_str());
}
// -- Make images showing the two, side-by-side.
cv::Mat3b combined(cv::Size(width_ * 2 + horiz_divider, height_), cv::Vec3b(0, 0, 0));
for(int y = 0; y < combined.rows; ++y) {
for(int x = 0; x < combined.cols; ++x) {
if(x < count.cols)
combined(y, x) = count(y, x);
else if(x > count.cols + horiz_divider)
combined(y, x) = mult(y, x - count.cols - horiz_divider);
}
}
oss.str("");
oss << dir << "/combined_" << buffer << ".png";
if(writeFiles)
{
cv::imwrite(oss.str(), combined);
UINFO("Written \"%s\"", oss.str().c_str());
}
// -- Append to the overview image.
for(int y = 0; y < combined.rows; ++y)
for(int x = 0; x < combined.cols; ++x)
overview(y + i * (combined.rows + vert_divider) + vert_divider, x) = combined(y, x);
// -- Compute the publication multipliers visualization for this layer.
// Multiplier of 1 is white, >1 is red, <1 is blue. Think redshift.
cv::Mat3b pubmult(cv::Size(width_, height_), cv::Vec3b(255, 255, 255));
for(int y = 0; y < pubmult.rows; ++y) {
for(int x = 0; x < pubmult.cols; ++x) {
const DiscreteFrustum& frustum = *frustums_[y / bin_height_][x / bin_width_];
float val = frustum.multipliers_(i);
if(val > 1) {
pubmult(y, x)[0] = 255 - min(255., 255 * (val - 1.0) / 0.1);
pubmult(y, x)[1] = 255 - min(255., 255 * (val - 1.0) / 0.1);
}
if(val < 1) {
pubmult(y, x)[1] = 255 - min(255., 255 * (1.0 - val) / 0.1);
pubmult(y, x)[2] = 255 - min(255., 255 * (1.0 - val) / 0.1);
}
}
}
// -- Append to publication image.
for(size_t j = 0; j < pub_layers.size(); ++j)
if(pub_layers[j] == i)
for(int y = 0; y < pubmult.rows; ++y)
for(int x = 0; x < pubmult.cols; ++x)
pub(y + j * (pubmult.rows + vert_divider) + vert_divider, x) = pubmult(y, x);
}
// -- Add a white bar at the top and bottom for reference.
for(int y = 0; y < overview.rows; ++y)
if(y < vert_divider || y > overview.rows - vert_divider)
for(int x = 0; x < overview.cols; ++x)
overview(y, x) = cv::Vec3b(255, 255, 255);
// -- Save overview image.
ostringstream oss;
oss << dir << "/overview.png";
if(writeFiles)
{
cv::imwrite(oss.str(), overview);
UINFO("Written \"%s\"", oss.str().c_str());
}
// -- Save a small version for easy loading.
cv::Mat3b overview_scaled;
cv::resize(overview, overview_scaled, cv::Size(), 0.2, 0.2, cv::INTER_CUBIC);
oss.str("");
oss << dir << "/overview_scaled.png";
if(writeFiles)
{
cv::imwrite(oss.str(), overview_scaled);
UINFO("Written \"%s\"", oss.str().c_str());
}
// -- Save publication image.
oss.str("");
oss << dir << "/pub";
// for(size_t i = 0; i < pub_layers.size(); ++i)
// oss << "-" << setw(2) << setfill('0') << pub_layers[i];
oss << ".png";
if(writeFiles)
{
cv::imwrite(oss.str(), pub);
UINFO("Written \"%s\"", oss.str().c_str());
}
UASSERT(overview.rows == pub.rows);
cv::Mat3b targetImage(overview.rows, overview.cols/2 + pub.cols);
cv::Mat roiA(targetImage, cv::Rect( 0, 0, overview.cols/2, overview.rows ));
cv::Mat(overview, cv::Rect( 0, 0, overview.cols/2, overview.rows )).copyTo(roiA);
cv::Mat roiB( targetImage, cvRect( overview.cols/2, 0, pub.cols, pub.rows ) );
pub.copyTo(roiB);
return targetImage;
}
} // namespace clams
corelib/src/clams/eigen_extensions/eigen_extensions.h
+312
View File
@@ -0,0 +1,312 @@
#ifndef EIGEN_EXTENSIONS_H
#define EIGEN_EXTENSIONS_H
#include <Eigen/Eigen>
//#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#include <Eigen/Sparse>
//#include <boost/filesystem.hpp>
#include <stdint.h>
#include <fstream>
#include <iostream>
//#include <gzstream/gzstream.h>
namespace eigen_extensions {
inline void stdToEig(const std::vector<double>& std, Eigen::VectorXd* eig)
{
eig->resize(std.size());
for(size_t i = 0; i < std.size(); ++i)
eig->coeffRef(i) = std[i];
}
inline double stdev(const Eigen::VectorXd& vec)
{
double mean = vec.sum() / (double)vec.rows();
double total = 0;
for(int i = 0; i < vec.rows(); ++i)
total += (vec.coeffRef(i) - mean) * (vec.coeffRef(i) - mean);
double var = total / (double)vec.rows();
return sqrt(var);
}
template<class S, int T, int U>
void save(const Eigen::Matrix<S, T, U>& mat, const std::string& filename);
template<class S, int T, int U>
void load(const std::string& filename, Eigen::Matrix<S, T, U>* mat);
template<class ScalarType, int Options, class IndexType>
void save(const Eigen::SparseMatrix<ScalarType, Options, IndexType>& mat, const std::string& filename);
template<class ScalarType, int Options, class IndexType>
void load(const std::string& filename, Eigen::SparseMatrix<ScalarType, Options, IndexType>* mat);
template<class S, int T, int U>
void saveASCII(const Eigen::Matrix<S, T, U>& mat, const std::string& filename);
template<class S, int T, int U>
void loadASCII(const std::string& filename, Eigen::Matrix<S, T, U>* mat);
template<class S, int T, int U>
void serialize(const Eigen::Matrix<S, T, U>& mat, std::ostream& strm);
template<class S, int T, int U>
void deserialize(std::istream& strm, Eigen::Matrix<S, T, U>* mat);
template<class S, int T, int U>
void serializeASCII(const Eigen::Matrix<S, T, U>& mat, std::ostream& strm);
template<class S, int T, int U>
void deserializeASCII(std::istream& strm, Eigen::Matrix<S, T, U>* mat);
// -- SparseMatrix serialization.
template<class ScalarType, int Options, class IndexType>
void serialize(const Eigen::SparseMatrix<ScalarType, Options, IndexType>& mat, std::ostream& strm);
template<class ScalarType, int Options, class IndexType>
void deserialize(std::istream& strm, Eigen::SparseMatrix<ScalarType, Options, IndexType>* mat);
// -- Scalar serialization
// TODO: Can you name these {de,}serialize() and still have the right
// functions get called when serializing matrices?
template<class T>
void serializeScalar(T val, std::ostream& strm);
template<class T>
void deserializeScalar(std::istream& strm, T* val);
/************************************************************
* Template implementations
************************************************************/
template<class S, int T, int U>
void serialize(const Eigen::Matrix<S, T, U>& mat, std::ostream& strm)
{
int bytes = sizeof(S);
int rows = mat.rows();
int cols = mat.cols();
strm.write((char*)&bytes, sizeof(int));
strm.write((char*)&rows, sizeof(int));
strm.write((char*)&cols, sizeof(int));
strm.write((const char*)mat.data(), sizeof(S) * rows * cols);
}
template<class S, int T, int U>
void deserialize(std::istream& strm, Eigen::Matrix<S, T, U>* mat)
{
int bytes;
int rows;
int cols;
strm.read((char*)&bytes, sizeof(int));
strm.read((char*)&rows, sizeof(int));
strm.read((char*)&cols, sizeof(int));
assert(bytes == sizeof(S));
S *buf = (S*) malloc(sizeof(S) * rows * cols);
strm.read((char*)buf, sizeof(S) * rows * cols);
*mat = Eigen::Map< Eigen::Matrix<S, T, U> >(buf, rows, cols);
free(buf);
}
/*
template<class S, int T, int U>
void save(const Eigen::Matrix<S, T, U>& mat, const std::string& filename)
{
assert(filename.size() > 3);
if(filename.substr(filename.size() - 3, 3).compare(".gz") == 0) {
ogzstream file(filename.c_str());
assert(file);
serialize(mat, file);
file.close();
}
else {
assert(boost::filesystem::extension(filename).compare(".eig") == 0);
std::ofstream file(filename.c_str());
assert(file);
serialize(mat, file);
file.close();
}
}
template<class S, int T, int U>
void load(const std::string& filename, Eigen::Matrix<S, T, U>* mat)
{
assert(filename.size() > 3);
if(filename.substr(filename.size() - 3, 3).compare(".gz") == 0) {
igzstream file(filename.c_str());
assert(file);
deserialize(file, mat);
file.close();
}
else {
assert(boost::filesystem::extension(filename).compare(".eig") == 0);
std::ifstream file(filename.c_str());
assert(file);
deserialize(file, mat);
file.close();
}
}
*/
template<class ScalarType, int Options, class IndexType>
void serialize(const Eigen::SparseMatrix<ScalarType, Options, IndexType>& mat, std::ostream& strm)
{
int bytes = sizeof(ScalarType);
int type = Options;
int outer = mat.outerSize();
int inner = mat.innerSize();
int nnz = mat.nonZeros();
strm.write((char*)&bytes, sizeof(int));
strm.write((char*)&type, sizeof(int));
strm.write((char*)&outer, sizeof(int));
strm.write((char*)&inner, sizeof(int));
strm.write((char*)&nnz, sizeof(int));
typedef typename Eigen::SparseMatrix<ScalarType, Options, IndexType>::InnerIterator InnerIterator;
for(IndexType i = 0; i < mat.outerSize(); ++i) {
int num = 0;
for(InnerIterator it(mat, i); it; ++it)
++num;
strm.write((const char*)&num, sizeof(num));
for(InnerIterator it(mat, i); it; ++it) {
int idx = it.index();
ScalarType buf = it.value();
strm.write((const char*)&idx, sizeof(idx));
strm.write((const char*)&buf, sizeof(buf));
}
}
}
template<class ScalarType, int Options, class IndexType>
void deserialize(std::istream& strm, Eigen::SparseMatrix<ScalarType, Options, IndexType>* mat)
{
int bytes;
int options;
int outer;
int inner;
int nnz;
strm.read((char*)&bytes, sizeof(int));
strm.read((char*)&options, sizeof(int));
strm.read((char*)&outer, sizeof(int));
strm.read((char*)&inner, sizeof(int));
strm.read((char*)&nnz, sizeof(int));
assert(bytes == sizeof(ScalarType));
assert(options == Options);
if(mat->IsRowMajor)
mat->resize(outer, inner);
else
mat->resize(inner, outer);
mat->reserve(nnz);
ScalarType buf;
for(int i = 0; i < mat->outerSize(); ++i) {
mat->startVec(i);
int num;
strm.read((char*)&num, sizeof(int));
int idx;
for(int j = 0; j < num; ++j) {
strm.read((char*)&idx, sizeof(idx));
strm.read((char*)&buf, sizeof(buf));
mat->insertBackByOuterInner(i, idx) = buf;
}
}
mat->finalize();
}
/*
template<class ScalarType, int Options, class IndexType>
void save(const Eigen::SparseMatrix<ScalarType, Options, IndexType>& mat, const std::string& filename)
{
assert(boost::filesystem::extension(filename).compare(".eig") == 0);
std::ofstream file(filename.c_str());
assert(file);
serialize(mat, file);
file.close();
}
template<class ScalarType, int Options, class IndexType>
void load(const std::string& filename, Eigen::SparseMatrix<ScalarType, Options, IndexType>* mat)
{
assert(filename.size() > 3);
assert(boost::filesystem::extension(filename).compare(".eig") == 0);
std::ifstream file(filename.c_str());
assert(file);
deserialize(file, mat);
file.close();
}
*/
template<class S, int T, int U>
void serializeASCII(const Eigen::Matrix<S, T, U>& mat, std::ostream& strm)
{
int old_precision = strm.precision();
strm.precision(16);
strm << "% " << mat.rows() << " " << mat.cols() << std::endl;
strm << mat << std::endl;
strm.precision(old_precision);
}
template<class S, int T, int U>
void deserializeASCII(std::istream& strm, Eigen::Matrix<S, T, U>* mat)
{
// -- Read the header.
std::string line;
while(line.length() == 0) getline(strm, line);
assert(line[0] == '%');
std::istringstream iss(line.substr(1));
int rows;
int cols;
iss >> rows;
iss >> cols;
// -- Read in the data.
*mat = Eigen::Matrix<S, T, U>(rows, cols);
for(int y = 0; y < rows; ++y) {
getline(strm, line);
std::istringstream iss(line);
for(int x = 0; x < cols; ++x) {
iss >> mat->coeffRef(y, x);
}
}
}
template<class S, int T, int U>
void saveASCII(const Eigen::Matrix<S, T, U>& mat, const std::string& filename)
{
assert(filename.substr(filename.size() - 8).compare(".eig.txt") == 0);
std::ofstream file;
file.open(filename.c_str());
assert(file);
serializeASCII(mat, file);
file.close();
}
template<class S, int T, int U>
void loadASCII(const std::string& filename, Eigen::Matrix<S, T, U>* mat)
{
assert(filename.substr(filename.size() - 8).compare(".eig.txt") == 0);
std::ifstream file;
file.open(filename.c_str());
if(!file)
std::cerr << "File " << filename << " could not be opened. Dying badly." << std::endl;
assert(file);
deserializeASCII(file, mat);
file.close();
}
template<class T>
void serializeScalar(T val, std::ostream& strm)
{
strm.write((char*)&val, sizeof(T));
}
template<class T>
void deserializeScalar(std::istream& strm, T* val)
{
strm.read((char*)val, sizeof(T));
}
}
#endif // EIGEN_EXTENSIONS_H
corelib/src/clams/frame_projector.cpp
+241
View File
@@ -0,0 +1,241 @@
/*
Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the <organization> nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
RTAB-Map integration: Mathieu Labbe
*/
#include "rtabmap/core/clams/frame_projector.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
using namespace std;
using namespace Eigen;
namespace clams
{
FrameProjector::FrameProjector(const rtabmap::CameraModel & model) :
model_(model)
{
UASSERT(model.isValidForReprojection());
}
// pcd is in /map frame
FrameProjector::RangeIndex FrameProjector::cloudToRangeIndex(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pcd) const
{
int height = model_.imageHeight();
int width = model_.imageWidth();
RangeIndex ind;
if((int)ind.size() != height)
ind.resize(height);
for(size_t y = 0; y < ind.size(); ++y)
if((int)ind[y].size() != width)
ind[y].resize(width);
for(size_t y = 0; y < ind.size(); ++y) {
for(size_t x = 0; x < ind[y].size(); ++x) {
ind[y][x].clear();
ind[y][x].reserve(10);
}
}
rtabmap::Transform t = model_.localTransform().inverse();
ProjectivePoint ppt;
for(size_t i = 0; i < pcd->size(); ++i) {
if(!isFinite(pcd->at(i)))
continue;
ppt = reproject(rtabmap::util3d::transformPoint(pcd->at(i), t));
if(ppt.z_ == 0 || !(ppt.u_ >= 0 && ppt.v_ >= 0 && ppt.u_ < width && ppt.v_ < height))
continue;
ind[ppt.v_][ppt.u_].push_back(ppt.z_);
}
return ind;
}
pcl::PointXYZ FrameProjector::project(const ProjectivePoint& ppt) const
{
UASSERT(ppt.u_ >= 0 && ppt.v_ >= 0 && ppt.u_ < model_.imageWidth() && ppt.v_ < model_.imageHeight());
pcl::PointXYZ pt;
model_.project(ppt.u_, ppt.v_, ppt.z_, pt.x, pt.y, pt.z);
return pt;
}
ProjectivePoint FrameProjector::reproject(const pcl::PointXYZ & pt) const
{
UASSERT(isFinite(pt));
ProjectivePoint ppt;
if(pt.z > 0)
{
model_.reproject(pt.x, pt.y, pt.z, ppt.u_, ppt.v_);
ppt.z_ = pt.z;
}
return ppt;
}
cv::Mat FrameProjector::estimateMapDepth(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & map,
const rtabmap::Transform & transform,
const cv::Mat& measurement,
double coneRadius,
double coneStdevThresh) const
{
cv::Mat estimate = cv::Mat::zeros(measurement.size(), CV_32FC1);
// -- Get the depth index.
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed = rtabmap::util3d::transformPointCloud(map, transform);
RangeIndex rindex = cloudToRangeIndex(transformed);
// -- Compute the edge-of-map mask.
cv::Mat naive_mapframe = rtabmap::util3d::projectCloudToCamera(model_.imageSize(), model_.K(), transformed, model_.localTransform());
const cv::Mat& measurement_depth = measurement;
const cv::Mat& naive_mapdepth = naive_mapframe;
cv::Mat1b mask(measurement_depth.rows, measurement_depth.cols);
mask = 0;
for(int y = 0; y < mask.rows; ++y)
for(int x = 0; x < mask.cols; ++x)
if(naive_mapdepth.at<float>(y, x) != 0)
mask(y, x) = 255;
cv::dilate(mask, mask, cv::Mat(), cv::Point(-1, -1), 4);
cv::erode(mask, mask, cv::Mat(), cv::Point(-1, -1), 15);
bool isInMM = measurement_depth.type() == CV_16UC1;
// -- Main loop: for all points in the image...
ProjectivePoint ppt;
for(ppt.v_ = 0; ppt.v_ < measurement_depth.rows; ++ppt.v_) {
for(ppt.u_ = 0; ppt.u_ < measurement_depth.cols; ++ppt.u_) {
float value = isInMM?float(measurement_depth.at<unsigned short>(ppt.v_, ppt.u_))*0.001f:measurement_depth.at<float>(ppt.v_, ppt.u_);
// -- Reject points with no data.
if(value == 0)
continue;
if(naive_mapdepth.at<float>(ppt.v_, ppt.u_) == 0)
continue;
// -- Reject points on the edge of the map.
if(mask(ppt.v_, ppt.u_) == 0)
continue;
// -- Find nearby points in the cone to get a good estimate of the map depth.
double mean = 0;
double stdev = 0;
//double stdev_thresh = numeric_limits<double>::max();
bool valid = coneFit(
naive_mapdepth.size(),
rindex,
ppt.u_,
ppt.v_,
coneRadius,
value,
&mean,
&stdev);
if(!valid)
continue;
if(stdev > coneStdevThresh)
continue;
estimate.at<float>(ppt.v_, ppt.u_) = mean;
}
}
return estimate;
}
bool FrameProjector::coneFit(const cv::Size& imageSize, const RangeIndex& rindex,
int uc, int vc, double radius, double measurement_depth,
double* mean, double* stdev) const
{
pcl::PointXYZ pt_center, pt_ul, pt_lr;
ProjectivePoint ppt, ppt_ul, ppt_lr;
ppt.u_ = uc;
ppt.v_ = vc;
ppt.z_ = measurement_depth;
pt_center = project(ppt);
pt_ul = pt_center;
pt_lr = pt_center;
pt_ul.x -= radius;
pt_ul.y -= radius;
pt_lr.x += radius;
pt_lr.y += radius;
ppt_ul = reproject(pt_ul);
ppt_lr = reproject(pt_lr);
if(ppt_ul.z_ == 0 || !(ppt_ul.u_ >= 0 && ppt_ul.v_ >= 0 && ppt_ul.u_ < imageSize.width && ppt_ul.v_ < imageSize.height))
return false;
if(ppt_lr.z_ == 0 || !(ppt_lr.u_ >= 0 && ppt_lr.v_ >= 0 && ppt_lr.u_ < imageSize.width && ppt_lr.v_ < imageSize.height))
return false;
int min_u = ppt_ul.u_;
int max_u = ppt_lr.u_;
int min_v = ppt_ul.v_;
int max_v = ppt_lr.v_;
*mean = 0;
double num = 0;
for(ppt.u_ = min_u; ppt.u_ <= max_u; ++ppt.u_) {
for(ppt.v_ = min_v; ppt.v_ <= max_v; ++ppt.v_) {
const vector<double>& vals = rindex[ppt.v_][ppt.u_];
for(size_t i = 0; i < vals.size(); ++i) {
double mult = vals[i] / measurement_depth;
if(mult > MIN_MULT && mult < MAX_MULT) {
*mean += vals[i];
++num;
}
}
}
}
if(num == 0)
return false;
*mean /= num;
double var = 0;
for(ppt.u_ = min_u; ppt.u_ <= max_u; ++ppt.u_) {
for(ppt.v_ = min_v; ppt.v_ <= max_v; ++ppt.v_) {
const vector<double>& vals = rindex[ppt.v_][ppt.u_];
for(size_t i = 0; i < vals.size(); ++i) {
double mult = vals[i] / measurement_depth;
if(mult > MIN_MULT && mult < MAX_MULT)
var += (vals[i] - *mean) * (vals[i] - *mean);
}
}
}
var /= num;
*stdev = sqrt(var);
return true;
}
} // namespace clams
corelib/src/clams/slam_calibrator.cpp
+78
View File
@@ -0,0 +1,78 @@
/*
Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the <organization> nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
RTAB-Map integration: Mathieu Labbe
*/
#include "rtabmap/core/clams/slam_calibrator.h"
#include "rtabmap/core/clams/frame_projector.h"
#include <rtabmap/utilite/ULogger.h>
using namespace std;
using namespace Eigen;
namespace clams
{
DiscreteDepthDistortionModel calibrate(
const std::map<int, rtabmap::SensorData> & sequence,
const std::map<int, rtabmap::Transform> & trajectory,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & map,
double coneRadius,
double coneStdevThresh)
{
DiscreteDepthDistortionModel model;
if(!sequence.empty())
{
const cv::Size & imageSize = sequence.begin()->second.cameraModels()[0].imageSize();
model = DiscreteDepthDistortionModel(imageSize.width, imageSize.height);
// -- For all selected frames, accumulate training examples
// in the distortion model.
size_t counts;
#pragma omp parallel for
for(std::map<int, rtabmap::Transform>::const_iterator iter = trajectory.begin(); iter != trajectory.end(); ++iter)
{
size_t idx = iter->first;
std::map<int, rtabmap::SensorData>::const_iterator ster = sequence.find(idx);
if(ster!=sequence.end())
{
cv::Mat depthImage;
ster->second.uncompressDataConst(0, &depthImage);
cv::Mat mapDepth;
FrameProjector projector(ster->second.cameraModels()[0]);
mapDepth = projector.estimateMapDepth(map, iter->second.inverse(), depthImage, coneRadius, coneStdevThresh);
counts = model.accumulate(mapDepth, depthImage);
}
}
UINFO("counts=%d", (int)counts);
}
return model;
}
} // namespace clams
corelib/src/util3d.cpp
+18 -13
View File
@@ -1356,9 +1356,9 @@ cv::Point3f projectDisparityTo3D(
// Register point cloud to camera (return registered depth image)
cv::Mat projectCloudToCamera(
const cv::Size & imageSize,
const cv::Mat & cameraMatrixK, // /base_link -> /camera_link
const cv::Mat & cameraMatrixK,
const cv::Mat & laserScan, // assuming laser scan points are already in /base_link coordinate
const rtabmap::Transform & cameraTransform)
const rtabmap::Transform & cameraTransform) // /base_link -> /camera_link
{
UASSERT(!cameraTransform.isNull());
UASSERT(!laserScan.empty());
@@ -1421,9 +1421,9 @@ cv::Mat projectCloudToCamera(
cv::Mat projectCloudToCamera(
const cv::Size & imageSize,
const cv::Mat & cameraMatrixK, // /base_link -> /camera_link
const cv::Mat & cameraMatrixK,
const pcl::PointCloud<pcl::PointXYZ>::Ptr laserScan, // assuming points are already in /base_link coordinate
const rtabmap::Transform & cameraTransform)
const rtabmap::Transform & cameraTransform) // /base_link -> /camera_link
{
UASSERT(!cameraTransform.isNull());
UASSERT(!laserScan->empty());
@@ -1446,17 +1446,22 @@ cv::Mat projectCloudToCamera(
// re-project in camera frame
float z = ptScan.z;
float invZ = 1.0f/z;
int dx = (fx*ptScan.x)*invZ + cx;
int dy = (fy*ptScan.y)*invZ + cy;
if(z > 0.0f && uIsInBounds(dx, 0, registered.cols) && uIsInBounds(dy, 0, registered.rows))
if(z > 0.0f)
{
++count;
float &zReg = registered.at<float>(dy, dx);
if(zReg == 0 || z < zReg)
float invZ = 1.0f/z;
int dx = (fx*ptScan.x)*invZ + cx;
if(uIsInBounds(dx, 0, registered.cols))
{
zReg = z;
int dy = (fy*ptScan.y)*invZ + cy;
if(uIsInBounds(dy, 0, registered.rows))
{
++count;
float &zReg = registered.at<float>(dy, dx);
if(zReg == 0 || z < zReg)
{
zReg = z;
}
}
}
}
}
guilib/include/rtabmap/gui/MainWindow.h
+5
View File
@@ -69,6 +69,7 @@ class TwistGridWidget;
class ExportCloudsDialog;
class ExportScansDialog;
class PostProcessingDialog;
class DepthCalibrationDialog;
class DataRecorder;
class OctoMap;
@@ -121,6 +122,7 @@ protected:
private slots:
void changeState(MainWindow::State state);
void beep();
void cancelProgress();
void configGUIModified();
void saveConfigGUI();
void newDatabase();
@@ -141,6 +143,7 @@ private slots:
void exportImages();
void exportOctomap();
void postProcessing();
void depthCalibration();
void deleteMemory();
void openWorkingDirectory();
void updateEditMenu();
@@ -264,6 +267,7 @@ private:
ExportCloudsDialog * _exportCloudsDialog;
ExportScansDialog * _exportScansDialog;
PostProcessingDialog * _postProcessingDialog;
DepthCalibrationDialog * _depthCalibrationDialog;
DataRecorder * _dataRecorder;
QSet<int> _lastIds;
@@ -330,6 +334,7 @@ private:
QVector<int> _loopClosureIds;
bool _firstCall;
bool _progressCanceled;
};
}
guilib/include/rtabmap/gui/PreferencesDialog.h
+4 -1
View File
@@ -215,8 +215,9 @@ public:
QString getSourceDriverStr() const;
QString getSourceDevice() const;
bool getSourceDatabaseStampsUsed() const;
bool isSourceDatabaseStampsUsed() const;
bool isSourceRGBDColorOnly() const;
QString getSourceDistortionModel() const;
int getSourceImageDecimation() const;
bool isSourceStereoDepthGenerated() const;
bool isSourceScanFromDepth() const;
@@ -292,6 +293,7 @@ private slots:
void setupTreeView();
void updateBasicParameter();
void openDatabaseViewer();
void visualizeDistortionModel();
void selectSourceDatabase();
void selectCalibrationPath();
void selectSourceImagesStamps();
@@ -305,6 +307,7 @@ private slots:
void selectSourceVideoPath();
void selectSourceStereoVideoPath();
void selectSourceStereoVideoPath2();
void selectSourceDistortionModel();
void selectSourceOniPath();
void selectSourceOni2Path();
void selectSourceSvoPath();
guilib/include/rtabmap/gui/ProgressDialog.h
+5
View File
@@ -53,6 +53,10 @@ public:
int maximumSteps() const;
void setMaximumSteps(int steps);
void setAutoClose(bool on, int delayedClosingTimeMsec = -1);
void setCancelButtonVisible(bool visible);
signals:
void canceled();
protected:
virtual void closeEvent(QCloseEvent * event);
@@ -71,6 +75,7 @@ private:
QTextEdit * _detailedText;
QProgressBar * _progressBar;
QPushButton * _closeButton;
QPushButton * _cancelButton;
QCheckBox * _closeWhenDoneCheckBox;
QString _endMessage;
int _delayedClosingTime; // sec
guilib/src/CMakeLists.txt
+4
View File
@@ -27,6 +27,7 @@ SET(headers_ui
../include/${PROJECT_PREFIX}/gui/GraphViewer.h
./CreateSimpleCalibrationDialog.h
./ParametersToolBox.h
./DepthCalibrationDialog.h
)
SET(uis
@@ -42,6 +43,7 @@ SET(uis
./ui/exportScansDialog.ui
./ui/calibrationDialog.ui
./ui/createSimpleCalibrationDialog.ui
./ui/depthCalibrationDialog.ui
)
SET(qrc
@@ -91,6 +93,8 @@ SET(SRC_FILES
./GraphViewer.cpp
./CreateSimpleCalibrationDialog.cpp
./ParametersToolBox.cpp
./DepthCalibrationDialog.cpp
${moc_srcs}
${moc_uis}
${srcs_qrc}
guilib/src/DepthCalibrationDialog.cpp
+450
View File
@@ -0,0 +1,450 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <DepthCalibrationDialog.h>
#include "ui_depthCalibrationDialog.h"
#include "rtabmap/gui/ProgressDialog.h"
#include "rtabmap/gui/CloudViewer.h"
#include "rtabmap/gui/ImageView.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UThread.h"
#include "rtabmap/utilite/UCv2Qt.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/clams/slam_calibrator.h"
#include "rtabmap/core/clams/frame_projector.h"
#include <QPushButton>
#include <QDir>
#include <QFileInfo>
#include <QUrl>
#include <QtGui/QDesktopServices>
#include <QMessageBox>
#include <QFileDialog>
namespace rtabmap {
DepthCalibrationDialog::DepthCalibrationDialog(QWidget *parent) :
QDialog(parent),
_canceled(false),
_model(0)
{
_ui = new Ui_DepthCalibrationDialog();
_ui->setupUi(this);
connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
connect(_ui->buttonBox->button(QDialogButtonBox::Save), SIGNAL(clicked()), this, SLOT(saveModel()));
connect(_ui->buttonBox->button(QDialogButtonBox::Ok), SIGNAL(clicked()), this, SLOT(accept()));
_ui->buttonBox->button(QDialogButtonBox::Ok)->setText("Calibrate");
restoreDefaults();
connect(_ui->spinBox_decimation, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_maxDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_minDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_coneRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_coneStdDevThresh, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_laserScan, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
_ui->buttonBox->button(QDialogButtonBox::Ok)->setFocus();
_progressDialog = new ProgressDialog(this);
_progressDialog->setVisible(false);
_progressDialog->setAutoClose(true, 2);
_progressDialog->setMinimumWidth(600);
_progressDialog->setCancelButtonVisible(true);
connect(_progressDialog, SIGNAL(canceled()), this, SLOT(cancel()));
}
DepthCalibrationDialog::~DepthCalibrationDialog()
{
delete _ui;
if(_model)
{
delete _model;
}
}
void DepthCalibrationDialog::saveSettings(QSettings & settings, const QString & group) const
{
if(!group.isEmpty())
{
settings.beginGroup(group);
}
settings.setValue("decimation", _ui->spinBox_decimation->value());
settings.setValue("max_depth", _ui->doubleSpinBox_maxDepth->value());
settings.setValue("min_depth", _ui->doubleSpinBox_minDepth->value());
settings.setValue("voxel",_ui->doubleSpinBox_voxelSize->value());
settings.setValue("cone_radius",_ui->doubleSpinBox_coneRadius->value());
settings.setValue("cone_stddev_thresh",_ui->doubleSpinBox_coneStdDevThresh->value());
settings.setValue("laser_scan",_ui->checkBox_laserScan->isChecked());
if(!group.isEmpty())
{
settings.endGroup();
}
}
void DepthCalibrationDialog::loadSettings(QSettings & settings, const QString & group)
{
if(!group.isEmpty())
{
settings.beginGroup(group);
}
_ui->spinBox_decimation->setValue(settings.value("decimation", _ui->spinBox_decimation->value()).toInt());
_ui->doubleSpinBox_maxDepth->setValue(settings.value("max_depth", _ui->doubleSpinBox_maxDepth->value()).toDouble());
_ui->doubleSpinBox_minDepth->setValue(settings.value("min_depth", _ui->doubleSpinBox_minDepth->value()).toDouble());
_ui->doubleSpinBox_voxelSize->setValue(settings.value("voxel", _ui->doubleSpinBox_voxelSize->value()).toDouble());
_ui->doubleSpinBox_coneRadius->setValue(settings.value("cone_radius", _ui->doubleSpinBox_coneRadius->value()).toDouble());
_ui->doubleSpinBox_coneStdDevThresh->setValue(settings.value("cone_stddev_thresh", _ui->doubleSpinBox_coneStdDevThresh->value()).toDouble());
_ui->checkBox_laserScan->setChecked(settings.value("laser_scan", _ui->checkBox_laserScan->isChecked()).toBool());
if(!group.isEmpty())
{
settings.endGroup();
}
}
void DepthCalibrationDialog::restoreDefaults()
{
_ui->spinBox_decimation->setValue(1);
_ui->doubleSpinBox_maxDepth->setValue(3.5);
_ui->doubleSpinBox_minDepth->setValue(0);
_ui->doubleSpinBox_voxelSize->setValue(0.01);
_ui->doubleSpinBox_coneRadius->setValue(0.02);
_ui->doubleSpinBox_coneStdDevThresh->setValue(0.1); // 0.03
_ui->checkBox_laserScan->setChecked(false);
_ui->checkBox_resetModel->setChecked(true);
}
void DepthCalibrationDialog::saveModel()
{
if(_model && _model->getTrainingSamples())
{
QString path = QFileDialog::getSaveFileName(this, tr("Save distortion model to ..."), _workingDirectory+QDir::separator()+"distortion_model.bin", tr("Distortion model (*.bin)"));
if(!path.isEmpty())
{
//
// Save depth calibration
//
cv::Mat results = _model->visualize(ULogger::level() == ULogger::kDebug?_workingDirectory.toStdString():"");
_model->save(path.toStdString());
if(!results.empty())
{
QString name = QString(path).replace(".bin", ".png", Qt::CaseInsensitive);
cv::imwrite(name.toStdString(), results);
QDesktopServices::openUrl(QUrl::fromLocalFile(name));
}
QMessageBox::information(this, tr("Depth Calibration"), tr("Distortion model saved to \"%1\"!").arg(path));
}
}
}
void DepthCalibrationDialog::cancel()
{
_canceled = true;
_progressDialog->appendText(tr("Canceled!"));
}
void DepthCalibrationDialog::calibrate(
const std::map<int, Transform> & poses,
const QMap<int, Signature> & cachedSignatures,
const QString & workingDirectory,
const ParametersMap & parameters)
{
_canceled = false;
_workingDirectory = workingDirectory;
_ui->buttonBox->button(QDialogButtonBox::Save)->setEnabled(_model && _model->getTrainingSamples()>0);
if(_model)
{
_ui->label_trainingSamples->setNum((int)_model->getTrainingSamples());
}
if(this->exec() == QDialog::Accepted)
{
if(_model && _ui->checkBox_resetModel->isChecked())
{
delete _model;
_model = 0;
}
_progressDialog->setMaximumSteps(poses.size()*2 + 3);
if(_ui->doubleSpinBox_voxelSize->value() > 0.0)
{
_progressDialog->setMaximumSteps(_progressDialog->maximumSteps()+1);
}
_progressDialog->resetProgress();
_progressDialog->show();
std::map<int, rtabmap::SensorData> sequence;
// Create the map
pcl::PointCloud<pcl::PointXYZ>::Ptr map(new pcl::PointCloud<pcl::PointXYZ>);
int index=1;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end() && !_canceled; ++iter, ++index)
{
int points = 0;
if(!iter->second.isNull())
{
pcl::IndicesPtr indices(new std::vector<int>);
if(cachedSignatures.contains(iter->first))
{
const Signature & s = cachedSignatures.find(iter->first).value();
SensorData data = s.sensorData();
if(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForReprojection())
{
cv::Mat image, depth, laserScan;
data.uncompressData(&image, &depth, _ui->checkBox_laserScan->isChecked()?&laserScan:0);
if(!image.empty() && !depth.empty())
{
UASSERT(iter->first == data.id());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
if(_ui->checkBox_laserScan->isChecked())
{
cloud = util3d::laserScanToPointCloud(laserScan);
indices->resize(cloud->size());
for(unsigned int i=0; i<indices->size(); ++i)
{
indices->at(i) = i;
}
}
else
{
cloud = util3d::cloudFromSensorData(
data,
_ui->spinBox_decimation->value(),
_ui->doubleSpinBox_maxDepth->value(),
_ui->doubleSpinBox_minDepth->value(),
indices.get(),
parameters);
}
if(indices->size())
{
if(_ui->doubleSpinBox_voxelSize->value() > 0.0)
{
cloud = util3d::voxelize(cloud, indices, _ui->doubleSpinBox_voxelSize->value());
}
cloud = util3d::transformPointCloud(cloud, iter->second);
points+=cloud->size();
*map += *cloud;
sequence.insert(std::make_pair(iter->first, data));
cv::Size size = data.cameraModels()[0].imageSize();
if(_model &&
(_model->getWidth()!=size.width ||
_model->getHeight()!=size.height))
{
QString msg = tr("Depth images (%1x%2) in the map don't have the "
"same size then in the current model (%3x%4). You may want "
"to check \"Reset previous model\" before trying again.")
.arg(size.width).arg(size.height)
.arg(_model->getWidth()).arg(_model->getHeight());
QMessageBox::warning(this, tr("Depth Calibration"), msg);
_progressDialog->appendText(msg, Qt::darkRed);
_progressDialog->setAutoClose(false);
return;
}
}
}
}
else
{
_progressDialog->appendText(tr("Not suitable camera model found for node %1, ignoring this node!").arg(iter->first), Qt::darkYellow);
_progressDialog->setAutoClose(false);
}
}
else
{
UERROR("Cloud %d not found in cache!", iter->first);
}
}
else
{
UERROR("transform is null!?");
}
if(points>0)
{
_progressDialog->appendText(tr("Generated cloud %1 with %2 points (%3/%4).")
.arg(iter->first).arg(points).arg(index).arg(poses.size()));
}
else
{
_progressDialog->appendText(tr("Ignored cloud %1 (%2/%3).").arg(iter->first).arg(index).arg(poses.size()));
}
_progressDialog->incrementStep();
QApplication::processEvents();
}
if(!_canceled && map->size() && sequence.size())
{
if(_ui->doubleSpinBox_voxelSize->value() > 0.0)
{
_progressDialog->appendText(tr("Voxel filtering (%1 m) of the merged point cloud (%2 points)")
.arg(_ui->doubleSpinBox_voxelSize->value())
.arg(map->size()));
QApplication::processEvents();
QApplication::processEvents();
map = util3d::voxelize(map, _ui->doubleSpinBox_voxelSize->value());
_progressDialog->incrementStep();
}
if(ULogger::level() == ULogger::kDebug)
{
//
// Show 3D map with frustums
//
QDialog * window = new QDialog(this->parentWidget()?this->parentWidget():this, Qt::Window);
window->setAttribute(Qt::WA_DeleteOnClose, true);
window->setWindowTitle(tr("Map"));
window->setMinimumWidth(800);
window->setMinimumHeight(600);
CloudViewer * viewer = new CloudViewer(window);
viewer->setCameraLockZ(false);
viewer->setFrustumShown(true);
QVBoxLayout *layout = new QVBoxLayout();
layout->addWidget(viewer);
window->setLayout(layout);
connect(window, SIGNAL(finished(int)), viewer, SLOT(clear()));
window->show();
uSleep(500);
_progressDialog->appendText(tr("Viewing the cloud (%1 points and %2 poses)...").arg(map->size()).arg(sequence.size()));
_progressDialog->incrementStep();
viewer->addCloud("map", map);
Transform opticalRot(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
for(std::map<int, SensorData>::iterator iter=sequence.begin(); iter!=sequence.end(); ++iter)
{
Transform baseToCamera = iter->second.cameraModels()[0].localTransform()*opticalRot.inverse();
viewer->addOrUpdateFrustum(uFormat("frustum%d",iter->first), poses.at(iter->first) * baseToCamera, 0.2);
}
_progressDialog->appendText(tr("Viewing the cloud (%1 points and %2 poses)... done.").arg(map->size()).arg(sequence.size()));
viewer->update();
}
_progressDialog->appendText(tr("CLAMS depth calibration..."));
QApplication::processEvents();
QApplication::processEvents();
QDialog * dialog = new QDialog(this->parentWidget()?this->parentWidget():this, Qt::Window);
dialog->setAttribute(Qt::WA_DeleteOnClose, true);
dialog->setWindowTitle(tr("Original/Map"));
dialog->setMinimumWidth(sequence.begin()->second.cameraModels()[0].imageWidth());
ImageView * imageView1 = new ImageView(dialog);
imageView1->setMinimumSize(320, 240);
ImageView * imageView2 = new ImageView(dialog);
imageView2->setMinimumSize(320, 240);
QVBoxLayout * vlayout = new QVBoxLayout();
vlayout->setMargin(0);
vlayout->addWidget(imageView1, 1);
vlayout->addWidget(imageView2, 1);
dialog->setLayout(vlayout);
if(ULogger::level() == ULogger::kDebug)
{
dialog->show();
}
//clams::DiscreteDepthDistortionModel model = clams::calibrate(sequence, poses, map);
const cv::Size & imageSize = sequence.begin()->second.cameraModels()[0].imageSize();
if(_model == 0)
{
_model = new clams::DiscreteDepthDistortionModel(imageSize.width, imageSize.height);
}
UASSERT(_model->getWidth() == imageSize.width && _model->getHeight() == imageSize.height);
// -- For all selected frames, accumulate training examples
// in the distortion model.
size_t counts;
index = 0;
for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin(); iter != poses.end() && !_canceled; ++iter)
{
size_t idx = iter->first;
std::map<int, rtabmap::SensorData>::const_iterator ster = sequence.find(idx);
if(ster!=sequence.end())
{
cv::Mat depthImage;
ster->second.uncompressDataConst(0, &depthImage);
cv::Mat mapDepth;
clams::FrameProjector projector(ster->second.cameraModels()[0]);
mapDepth = projector.estimateMapDepth(
map,
iter->second.inverse(),
depthImage,
_ui->doubleSpinBox_coneRadius->value(),
_ui->doubleSpinBox_coneStdDevThresh->value());
if(ULogger::level() == ULogger::kDebug)
{
imageView1->setImage(uCvMat2QImage(depthImage));
imageView2->setImage(uCvMat2QImage(mapDepth));
}
counts = _model->accumulate(mapDepth, depthImage);
_progressDialog->appendText(tr("Added %1 training examples from node %2 (%3/%4).").arg(counts).arg(iter->first).arg(++index).arg(sequence.size()));
}
_progressDialog->incrementStep();
QApplication::processEvents();
}
_progressDialog->appendText(tr("CLAMS depth calibration... done!"));
QApplication::processEvents();
if(!_canceled)
{
this->saveModel();
}
}
else
{
QMessageBox::warning(this, tr("Depth Calibration"), tr("The resulting map is empty!"));
}
_progressDialog->setValue(_progressDialog->maximumSteps());
}
}
} /* namespace rtabmap */
guilib/src/DepthCalibrationDialog.h
+83
View File
@@ -0,0 +1,83 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef GUILIB_SRC_DEPTHCALIBRATIONDIALOG_H_
#define GUILIB_SRC_DEPTHCALIBRATIONDIALOG_H_
#include <QDialog>
#include <QMap>
#include <QtCore/QSettings>
#include <rtabmap/core/Signature.h>
#include <rtabmap/core/Parameters.h>
class Ui_DepthCalibrationDialog;
namespace clams {
class DiscreteDepthDistortionModel;
}
namespace rtabmap {
class ProgressDialog;
class DepthCalibrationDialog : public QDialog
{
Q_OBJECT
public:
DepthCalibrationDialog(QWidget * parent = 0);
virtual ~DepthCalibrationDialog();
void saveSettings(QSettings & settings, const QString & group = "") const;
void loadSettings(QSettings & settings, const QString & group = "");
void calibrate(const std::map<int, Transform> & poses,
const QMap<int, Signature> & cachedSignatures,
const QString & workingDirectory,
const ParametersMap & parameters);
signals:
void configChanged();
public slots:
void restoreDefaults();
private slots:
void saveModel();
void cancel();
private:
Ui_DepthCalibrationDialog * _ui;
ProgressDialog * _progressDialog;
bool _canceled;
clams::DiscreteDepthDistortionModel * _model;
QString _workingDirectory;
};
} /* namespace rtabmap */
#endif /* GUILIB_SRC_DEPTHCALIBRATIONDIALOG_H_ */
guilib/src/ExportCloudsDialog.cpp
+99 -4
View File
@@ -41,6 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/Graph.h"
#include "rtabmap/core/GainCompensator.h"
#include "rtabmap/core/clams/discrete_depth_distortion_model.h"
#include <pcl/conversions.h>
#include <pcl/io/pcd_io.h>
@@ -58,7 +59,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
QDialog(parent)
QDialog(parent),
_canceled(false)
{
_ui = new Ui_ExportCloudsDialog();
_ui->setupUi(this);
@@ -77,6 +79,8 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
connect(_ui->spinBox_decimation, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_maxDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_minDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->lineEdit_distortionModel, SIGNAL(textChanged(const QString &)), this, SIGNAL(configChanged()));
connect(_ui->toolButton_distortionModel, SIGNAL(clicked()), this, SLOT(selectDistortionModel()));
connect(_ui->groupBox_filtering, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_filteringRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
@@ -120,6 +124,8 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
_progressDialog->setVisible(false);
_progressDialog->setAutoClose(true, 2);
_progressDialog->setMinimumWidth(600);
_progressDialog->setCancelButtonVisible(true);
connect(_progressDialog, SIGNAL(canceled()), this, SLOT(cancel()));
#ifdef DISABLE_VTK
_ui->doubleSpinBox_meshDecimationFactor->setEnabled(false);
@@ -148,6 +154,12 @@ void ExportCloudsDialog::updateTexturingAvailability()
_ui->label_meshDecimation->setEnabled(_ui->doubleSpinBox_meshDecimationFactor->isEnabled());
}
void ExportCloudsDialog::cancel()
{
_canceled = true;
_progressDialog->appendText(tr("Canceled!"));
}
void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & group) const
{
if(!group.isEmpty())
@@ -162,6 +174,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
settings.setValue("regenerate_decimation", _ui->spinBox_decimation->value());
settings.setValue("regenerate_max_depth", _ui->doubleSpinBox_maxDepth->value());
settings.setValue("regenerate_min_depth", _ui->doubleSpinBox_minDepth->value());
settings.setValue("regenerate_distortion_model", _ui->lineEdit_distortionModel->text());
settings.setValue("filtering", _ui->groupBox_filtering->isChecked());
@@ -225,6 +238,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
_ui->spinBox_decimation->setValue(settings.value("regenerate_decimation", _ui->spinBox_decimation->value()).toInt());
_ui->doubleSpinBox_maxDepth->setValue(settings.value("regenerate_max_depth", _ui->doubleSpinBox_maxDepth->value()).toDouble());
_ui->doubleSpinBox_minDepth->setValue(settings.value("regenerate_min_depth", _ui->doubleSpinBox_minDepth->value()).toDouble());
_ui->lineEdit_distortionModel->setText(settings.value("regenerate_distortion_model", _ui->lineEdit_distortionModel->text()).toString());
_ui->groupBox_filtering->setChecked(settings.value("filtering", _ui->groupBox_filtering->isChecked()).toBool());
_ui->doubleSpinBox_filteringRadius->setValue(settings.value("filtering_radius", _ui->doubleSpinBox_filteringRadius->value()).toDouble());
@@ -285,6 +299,7 @@ void ExportCloudsDialog::restoreDefaults()
_ui->spinBox_decimation->setValue(1);
_ui->doubleSpinBox_maxDepth->setValue(4);
_ui->doubleSpinBox_minDepth->setValue(0);
_ui->lineEdit_distortionModel->setText("");
_ui->groupBox_filtering->setChecked(false);
_ui->doubleSpinBox_filteringRadius->setValue(0.02);
@@ -339,6 +354,20 @@ void ExportCloudsDialog::updateReconstructionFlavor()
_ui->groupBox_organized->setVisible(_ui->comboBox_pipeline->currentIndex() == 0);
}
void ExportCloudsDialog::selectDistortionModel()
{
QString dir = _ui->lineEdit_distortionModel->text();
if(dir.isEmpty())
{
dir = _workingDirectory;
}
QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Distortion model (*.bin)"));
if(path.size())
{
_ui->lineEdit_distortionModel->setText(path);
}
}
void ExportCloudsDialog::setSaveButton()
{
_ui->buttonBox->button(QDialogButtonBox::Ok)->setVisible(false);
@@ -423,7 +452,7 @@ void ExportCloudsDialog::exportClouds(
saveClouds(workingDirectory, poses, clouds, _ui->checkBox_binary->isChecked());
}
}
else
else if(!_canceled)
{
_progressDialog->setAutoClose(false);
}
@@ -605,6 +634,8 @@ bool ExportCloudsDialog::getExportedClouds(
std::map<int, pcl::PolygonMesh::Ptr> & meshes,
std::map<int, pcl::TextureMesh::Ptr> & textureMeshes)
{
_canceled = false;
_workingDirectory = workingDirectory;
enableRegeneration(cachedSignatures.size());
if(this->exec() == QDialog::Accepted)
{
@@ -650,6 +681,11 @@ bool ExportCloudsDialog::getExportedClouds(
cachedClouds,
parameters);
if(_canceled)
{
return false;
}
if(clouds.empty())
{
_progressDialog->setAutoClose(false);
@@ -686,6 +722,10 @@ bool ExportCloudsDialog::getExportedClouds(
_progressDialog->appendText(tr("Cloud %1 has gain %2").arg(jter->first).arg(gain));
_progressDialog->incrementStep();
QApplication::processEvents();
if(_canceled)
{
return false;
}
}
}
}
@@ -721,6 +761,10 @@ bool ExportCloudsDialog::getExportedClouds(
_progressDialog->appendText(tr("Assembled cloud %1, total=%2 (%3/%4).").arg(iter->first).arg(assembledCloud->size()).arg(++i).arg(clouds.size()));
_progressDialog->incrementStep();
QApplication::processEvents();
if(_canceled)
{
return false;
}
}
pcl::copyPointCloud(*assembledCloud, *rawAssembledCloud);
@@ -740,6 +784,11 @@ bool ExportCloudsDialog::getExportedClouds(
clouds.insert(std::make_pair(0, std::make_pair(assembledCloud, pcl::IndicesPtr(new std::vector<int>))));
}
if(_canceled)
{
return false;
}
std::map<int, Transform> viewPoints = poses;
if(_ui->groupBox_mls->isChecked())
{
@@ -787,6 +836,10 @@ bool ExportCloudsDialog::getExportedClouds(
_progressDialog->appendText(tr("Smoothing (MLS) the cloud (%1 points)...").arg(cloudWithNormals->size()));
QApplication::processEvents();
if(_canceled)
{
return false;
}
cloudWithNormals = util3d::mls(
cloudWithoutNormals,
@@ -832,6 +885,10 @@ bool ExportCloudsDialog::getExportedClouds(
_progressDialog->incrementStep();
QApplication::processEvents();
if(_canceled)
{
return false;
}
}
//used for organized texturing below
@@ -968,6 +1025,10 @@ bool ExportCloudsDialog::getExportedClouds(
_progressDialog->incrementStep();
QApplication::processEvents();
if(_canceled)
{
return false;
}
}
if(_ui->checkBox_assemble->isChecked() && mergedClouds->size())
@@ -1015,7 +1076,6 @@ bool ExportCloudsDialog::getExportedClouds(
_progressDialog->incrementStep();
QApplication::processEvents();
}
}
else
{
@@ -1073,10 +1133,19 @@ bool ExportCloudsDialog::getExportedClouds(
_progressDialog->incrementStep();
QApplication::processEvents();
if(_canceled)
{
return false;
}
}
}
}
if(_canceled)
{
return false;
}
// texture mesh
UDEBUG("texture mapping=%d", _ui->checkBox_textureMapping->isChecked()?1:0);
if(_ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked())
@@ -1228,6 +1297,10 @@ bool ExportCloudsDialog::getExportedClouds(
_progressDialog->appendText(tr("TextureMesh %1 created [cameras=%2] (%3/%4).").arg(iter->first).arg(cameraPoses.size()).arg(++i).arg(meshes.size()));
_progressDialog->incrementStep();
QApplication::processEvents();
if(_canceled)
{
return false;
}
}
if(textureMeshes.size() > 1 && _ui->checkBox_assemble->isChecked())
@@ -1260,7 +1333,7 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr previousCloud;
pcl::IndicesPtr previousIndices;
Transform previousPose;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter, ++index)
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end() && !_canceled; ++iter, ++index)
{
int points = 0;
int totalIndices = 0;
@@ -1278,6 +1351,16 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
d.uncompressData(&image, &depth, 0);
if(!image.empty() && !depth.empty())
{
if(!_ui->lineEdit_distortionModel->text().isEmpty() &&
QFileInfo(_ui->lineEdit_distortionModel->text()).exists())
{
clams::DiscreteDepthDistortionModel model;
model.load(_ui->lineEdit_distortionModel->text().toStdString());
depth = depth.clone();// make sure we are not modifying data in cached signatures.
model.undistort(depth);
d.setDepthOrRightRaw(depth);
}
UASSERT(iter->first == d.id());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithoutNormals;
cloudWithoutNormals = util3d::cloudRGBFromSensorData(
@@ -1555,6 +1638,10 @@ void ExportCloudsDialog::saveClouds(
}
_progressDialog->incrementStep();
QApplication::processEvents();
if(_canceled)
{
return;
}
}
}
}
@@ -1708,6 +1795,10 @@ void ExportCloudsDialog::saveMeshes(
}
_progressDialog->incrementStep();
QApplication::processEvents();
if(_canceled)
{
return;
}
}
}
}
@@ -1847,6 +1938,10 @@ void ExportCloudsDialog::saveTextureMeshes(
}
_progressDialog->incrementStep();
QApplication::processEvents();
if(_canceled)
{
return;
}
}
}
}
guilib/src/ExportCloudsDialog.h
+4
View File
@@ -87,8 +87,10 @@ public slots:
private slots:
void updateReconstructionFlavor();
void selectDistortionModel();
void updateMLSGrpVisibility();
void updateTexturingAvailability();
void cancel();
private:
std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > getClouds(
@@ -118,6 +120,8 @@ private:
private:
Ui_ExportCloudsDialog * _ui;
ProgressDialog * _progressDialog;
QString _workingDirectory;
bool _canceled;
};
}
guilib/src/MainWindow.cpp
+74 -17
View File
@@ -65,6 +65,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "ExportScansDialog.h"
#include "AboutDialog.h"
#include "PostProcessingDialog.h"
#include "DepthCalibrationDialog.h"
#include <QtGui/QCloseEvent>
#include <QtGui/QPixmap>
@@ -165,7 +166,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_rawLikelihoodCurve(0),
_autoScreenCaptureOdomSync(false),
_autoScreenCaptureRAM(false),
_firstCall(true)
_firstCall(true),
_progressCanceled(false)
{
UDEBUG("");
@@ -186,6 +188,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_exportScansDialog->setObjectName("ExportScansDialog");
_postProcessingDialog = new PostProcessingDialog(this);
_postProcessingDialog->setObjectName("PostProcessingDialog");
_depthCalibrationDialog = new DepthCalibrationDialog(this);
_depthCalibrationDialog->setObjectName("DepthCalibrationDialog");
_ui = new Ui_mainWindow();
UDEBUG("Setup ui...");
@@ -235,6 +239,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_preferencesDialog->loadWindowGeometry(_exportCloudsDialog);
_preferencesDialog->loadWindowGeometry(_exportScansDialog);
_preferencesDialog->loadWindowGeometry(_postProcessingDialog);
_preferencesDialog->loadWindowGeometry(_depthCalibrationDialog);
_preferencesDialog->loadWindowGeometry(_aboutDialog);
setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());
@@ -281,6 +286,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_initProgressDialog = new ProgressDialog(this);
_initProgressDialog->setWindowTitle(tr("Progress dialog"));
_initProgressDialog->setMinimumWidth(800);
connect(_initProgressDialog, SIGNAL(canceled()), this, SLOT(cancelProgress()));
connect(_ui->widget_mapVisibility, SIGNAL(visibilityChanged(int, bool)), this, SLOT(updateNodeVisibility(int, bool)));
@@ -375,6 +381,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
connect(_ui->actionTrigger_a_new_map, SIGNAL(triggered()), this, SLOT(triggerNewMap()));
connect(_ui->actionData_recorder, SIGNAL(triggered()), this, SLOT(dataRecorder()));
connect(_ui->actionPost_processing, SIGNAL(triggered()), this, SLOT(postProcessing()));
connect(_ui->actionDepth_Calibration, SIGNAL(triggered()), this, SLOT(depthCalibration()));
_ui->actionPause->setShortcut(Qt::Key_Space);
_ui->actionSave_GUI_config->setShortcut(QKeySequence::Save);
@@ -458,6 +465,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
connect(_exportCloudsDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
connect(_exportScansDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
connect(_postProcessingDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
connect(_depthCalibrationDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
connect(_ui->toolBar->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
connect(_ui->toolBar, SIGNAL(orientationChanged(Qt::Orientation)), this, SLOT(configGUIModified()));
connect(statusBarAction, SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
@@ -518,6 +526,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_preferencesDialog->loadWidgetState(_exportCloudsDialog);
_preferencesDialog->loadWidgetState(_exportScansDialog);
_preferencesDialog->loadWidgetState(_postProcessingDialog);
_preferencesDialog->loadWidgetState(_depthCalibrationDialog);
if(_ui->statsToolBox->findChildren<StatItem*>().size() == 0)
{
@@ -848,12 +857,13 @@ void MainWindow::processCameraInfo(const rtabmap::CameraInfo & info)
{
_firstStamp = info.stamp;
}
_ui->statsToolBox->updateStat("Camera/Time total/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeTotal*1000.0f);
_ui->statsToolBox->updateStat("Camera/Time capturing/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeCapture*1000.0f);
_ui->statsToolBox->updateStat("Camera/Time undistort depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeUndistortDepth*1000.0f);
_ui->statsToolBox->updateStat("Camera/Time decimation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeImageDecimation*1000.0f);
_ui->statsToolBox->updateStat("Camera/Time disparity/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeDisparity*1000.0f);
_ui->statsToolBox->updateStat("Camera/Time mirroring/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeMirroring*1000.0f);
_ui->statsToolBox->updateStat("Camera/Time scan_from_depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeScanFromDepth*1000.0f);
_ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeScanFromDepth*1000.0f);
}
void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataIgnored)
@@ -1907,6 +1917,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
{
_ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
_ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
_ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
}
_processingStatistics = false;
@@ -2170,6 +2181,10 @@ void MainWindow::updateMapCloud(
if(poses.size() < 200 || i % 100 == 0)
{
QApplication::processEvents();
if(_progressCanceled)
{
break;
}
}
}
}
@@ -3254,6 +3269,8 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
{
_initProgressDialog->appendText("Updating the 3D map cloud...");
_initProgressDialog->incrementStep();
_initProgressDialog->setCancelButtonVisible(true);
_progressCanceled = false;
QApplication::processEvents();
std::map<int, Transform> poses = event.getPoses();
alignPosesToGroundTruth(poses, groundTruth);
@@ -3277,10 +3294,13 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
{
_ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
_ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
_ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
}
_processingDownloadedMap = false;
}
_initProgressDialog->setValue(_initProgressDialog->maximumSteps());
_initProgressDialog->setCancelButtonVisible(false);
_progressCanceled = false;
}
void MainWindow::processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent & event)
@@ -3379,7 +3399,7 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
{
if(dynamic_cast<DBReader*>(_camera->camera()) != 0)
{
_camera->setImageRate( _preferencesDialog->getSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate());
_camera->setImageRate( _preferencesDialog->isSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate());
}
else
{
@@ -3755,6 +3775,12 @@ void MainWindow::beep()
QApplication::beep();
}
void MainWindow::cancelProgress()
{
_progressCanceled = true;
_initProgressDialog->appendText(tr("Canceled!"));
}
void MainWindow::configGUIModified()
{
this->setWindowModified(true);
@@ -3774,6 +3800,7 @@ void MainWindow::saveConfigGUI()
_preferencesDialog->saveWidgetState(_exportCloudsDialog);
_preferencesDialog->saveWidgetState(_exportScansDialog);
_preferencesDialog->saveWidgetState(_postProcessingDialog);
_preferencesDialog->saveWidgetState(_depthCalibrationDialog);
_preferencesDialog->saveWidgetState(_ui->graphicsView_graphView);
_preferencesDialog->saveSettings();
this->saveFigures();
@@ -4012,7 +4039,7 @@ void MainWindow::startDetection()
float detectionRate = uStr2Float(parameters.at(Parameters::kRtabmapDetectionRate()));
int bufferingSize = uStr2Float(parameters.at(Parameters::kRtabmapImageBufferSize()));
if(((detectionRate!=0.0f && detectionRate <= inputRate) || (detectionRate > 0.0f && inputRate == 0.0f)) &&
(_preferencesDialog->getSourceDriver() != PreferencesDialog::kSrcDatabase || !_preferencesDialog->getSourceDatabaseStampsUsed()))
(_preferencesDialog->getSourceDriver() != PreferencesDialog::kSrcDatabase || !_preferencesDialog->isSourceDatabaseStampsUsed()))
{
int button = QMessageBox::question(this,
tr("Incompatible frame rates!"),
@@ -4028,7 +4055,7 @@ void MainWindow::startDetection()
}
}
if(bufferingSize != 0 &&
(_preferencesDialog->getSourceDriver() != PreferencesDialog::kSrcDatabase || !_preferencesDialog->getSourceDatabaseStampsUsed()))
(_preferencesDialog->getSourceDriver() != PreferencesDialog::kSrcDatabase || !_preferencesDialog->isSourceDatabaseStampsUsed()))
{
int button = QMessageBox::question(this,
tr("Some images may be skipped!"),
@@ -4090,6 +4117,10 @@ void MainWindow::startDetection()
_preferencesDialog->getSourceScanFromDepthMaxDepth(),
_preferencesDialog->getSourceScanVoxelSize(),
_preferencesDialog->getSourceScanNormalsK());
if(_preferencesDialog->getSourceType() == PreferencesDialog::kSrcRGBD)
{
_camera->setDistortionModel(_preferencesDialog->getSourceDistortionModel().toStdString());
}
//Create odometry thread if rgbd slam
if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()))
@@ -4468,6 +4499,8 @@ void MainWindow::postProcessing()
_initProgressDialog->clear();
_initProgressDialog->show();
_initProgressDialog->appendText("Post-processing beginning!");
_initProgressDialog->setCancelButtonVisible(true);
_progressCanceled = false;
int totalSteps = 0;
if(refineNeighborLinks)
@@ -4498,7 +4531,7 @@ void MainWindow::postProcessing()
UDEBUG("");
UASSERT(detectLoopClosureIterations>0);
for(int n=0; n<detectLoopClosureIterations; ++n)
for(int n=0; n<detectLoopClosureIterations && !_progressCanceled; ++n)
{
_initProgressDialog->appendText(tr("Looking for more loop closures, clustering poses... (iteration=%1/%2, radius=%3 m angle=%4 degrees)")
.arg(n+1).arg(detectLoopClosureIterations).arg(clusterRadius).arg(clusterAngle));
@@ -4513,7 +4546,7 @@ void MainWindow::postProcessing()
int i=0;
std::set<int> addedLinks;
for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter, ++i)
for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end() && !_progressCanceled; ++iter, ++i)
{
int from = iter->first;
int to = iter->second;
@@ -4552,9 +4585,6 @@ void MainWindow::postProcessing()
}
else
{
_initProgressDialog->incrementStep();
QApplication::processEvents();
Signature signatureFrom = _cachedSignatures[from];
Signature signatureTo = _cachedSignatures[to];
@@ -4579,6 +4609,7 @@ void MainWindow::postProcessing()
}
}
}
_initProgressDialog->incrementStep();
}
_initProgressDialog->appendText(tr("Iteration %1/%2: Detected %3 loop closures!")
.arg(n+1).arg(detectLoopClosureIterations).arg(addedLinks.size()/2));
@@ -4610,7 +4641,7 @@ void MainWindow::postProcessing()
_initProgressDialog->appendText(tr("Total new loop closures detected=%1").arg(loopClosuresAdded));
}
if(refineNeighborLinks || refineLoopClosureLinks)
if(!_progressCanceled && (refineNeighborLinks || refineLoopClosureLinks))
{
UDEBUG("");
if(refineLoopClosureLinks)
@@ -4623,7 +4654,7 @@ void MainWindow::postProcessing()
RegistrationIcp regIcp(parameters);
int i=0;
for(std::multimap<int, Link>::iterator iter = _currentLinksMap.begin(); iter!=_currentLinksMap.end(); ++iter, ++i)
for(std::multimap<int, Link>::iterator iter = _currentLinksMap.begin(); iter!=_currentLinksMap.end() && !_progressCanceled; ++iter, ++i)
{
int type = iter->second.type();
@@ -4712,7 +4743,7 @@ void MainWindow::postProcessing()
_initProgressDialog->appendText(tr("Optimizing graph with updated links... done!"));
_initProgressDialog->incrementStep();
if(sba)
if(!_progressCanceled && sba)
{
UASSERT(Optimizer::isAvailable(sbaType));
_initProgressDialog->appendText(tr("SBA (%1 nodes, %2 constraints, %3 iterations)...")
@@ -4759,10 +4790,28 @@ void MainWindow::postProcessing()
_initProgressDialog->setValue(_initProgressDialog->maximumSteps());
_initProgressDialog->appendText("Post-processing finished!");
_initProgressDialog->setCancelButtonVisible(false);
_progressCanceled = false;
delete optimizer;
}
void MainWindow::depthCalibration()
{
if(_currentPosesMap.size() && _cachedSignatures.size())
{
_depthCalibrationDialog->calibrate(
_currentPosesMap,
_cachedSignatures,
_preferencesDialog->getWorkingDirectory(),
_preferencesDialog->getAllParameters());
}
else
{
QMessageBox::warning(this, tr("Depth Calibration"), tr("No data in cache. Try to refresh the cache."));
}
}
void MainWindow::deleteMemory()
{
QMessageBox::StandardButton button;
@@ -5157,6 +5206,7 @@ void MainWindow::clearTheCache()
_ui->actionPost_processing->setEnabled(false);
_ui->actionSave_point_cloud->setEnabled(false);
_ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false);
_ui->actionDepth_Calibration->setEnabled(false);
_ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(false);
_ui->actionView_scans->setEnabled(false);
_ui->actionExport_octomap->setEnabled(false);
@@ -6008,7 +6058,7 @@ void MainWindow::changeState(MainWindow::State newState)
}
}
actions = _ui->menuFile->actions();
if(actions.size()==16)
if(actions.size()==17)
{
if(actions.at(2)->isSeparator())
{
@@ -6018,9 +6068,9 @@ void MainWindow::changeState(MainWindow::State newState)
{
UWARN("Menu File separators have not the same order.");
}
if(actions.at(12)->isSeparator())
if(actions.at(13)->isSeparator())
{
actions.at(12)->setVisible(!monitoring);
actions.at(13)->setVisible(!monitoring);
}
else
{
@@ -6083,6 +6133,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionExport_octomap->setEnabled(false);
#endif
_ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
_ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
_ui->actionDownload_all_clouds->setEnabled(false);
_ui->actionDownload_graph->setEnabled(false);
_ui->menuSelect_source->setEnabled(true);
@@ -6144,6 +6195,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionExport_octomap->setEnabled(false);
#endif
_ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
_ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
_ui->actionDownload_all_clouds->setEnabled(true);
_ui->actionDownload_graph->setEnabled(true);
_ui->menuSelect_source->setEnabled(true);
@@ -6190,6 +6242,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionView_scans->setEnabled(false);
_ui->actionExport_octomap->setEnabled(false);
_ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false);
_ui->actionDepth_Calibration->setEnabled(false);
_ui->actionDownload_all_clouds->setEnabled(false);
_ui->actionDownload_graph->setEnabled(false);
_ui->menuSelect_source->setEnabled(false);
@@ -6233,6 +6286,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionView_scans->setEnabled(false);
_ui->actionExport_octomap->setEnabled(false);
_ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false);
_ui->actionDepth_Calibration->setEnabled(false);
_ui->actionDownload_all_clouds->setEnabled(false);
_ui->actionDownload_graph->setEnabled(false);
_state = kDetecting;
@@ -6267,6 +6321,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionExport_octomap->setEnabled(false);
#endif
_ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
_ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
_ui->actionDownload_all_clouds->setEnabled(true);
_ui->actionDownload_graph->setEnabled(true);
_state = kPaused;
@@ -6297,6 +6352,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionView_scans->setEnabled(false);
_ui->actionExport_octomap->setEnabled(false);
_ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false);
_ui->actionDepth_Calibration->setEnabled(false);
_ui->actionDelete_memory->setEnabled(true);
_ui->actionDownload_all_clouds->setEnabled(true);
_ui->actionDownload_graph->setEnabled(true);
@@ -6332,6 +6388,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionExport_octomap->setEnabled(false);
#endif
_ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
_ui->actionDepth_Calibration->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
_ui->actionDelete_memory->setEnabled(true);
_ui->actionDownload_all_clouds->setEnabled(true);
_ui->actionDownload_graph->setEnabled(true);
guilib/src/PreferencesDialog.cpp
+74 -1
View File
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <QtCore/QSettings>
#include <QtCore/QDir>
#include <QtCore/QTimer>
#include <QUrl>
#include <QFileDialog>
#include <QInputDialog>
@@ -42,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <QProgressDialog>
#include <QScrollBar>
#include <QStatusBar>
#include <QDesktopServices>
#include <QtGui/QCloseEvent>
#include "ui_preferencesDialog.h"
@@ -60,6 +62,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Optimizer.h"
#include "rtabmap/core/OptimizerG2O.h"
#include "rtabmap/core/DBReader.h"
#include "rtabmap/core/clams/discrete_depth_distortion_model.h"
#include "rtabmap/gui/LoopClosureViewer.h"
#include "rtabmap/gui/CameraViewer.h"
@@ -70,6 +73,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "ExportScansDialog.h"
#include "PostProcessingDialog.h"
#include "CreateSimpleCalibrationDialog.h"
#include "DepthCalibrationDialog.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
@@ -505,8 +509,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->pushButton_calibrate_simple, SIGNAL(clicked()), this, SLOT(calibrateSimple()));
connect(_ui->toolButton_openniOniPath, SIGNAL(clicked()), this, SLOT(selectSourceOniPath()));
connect(_ui->toolButton_openni2OniPath, SIGNAL(clicked()), this, SLOT(selectSourceOni2Path()));
connect(_ui->toolButton_source_distortionModel, SIGNAL(clicked()), this, SLOT(selectSourceDistortionModel()));
connect(_ui->toolButton_distortionModel, SIGNAL(clicked()), this, SLOT(visualizeDistortionModel()));
connect(_ui->lineEdit_openniOniPath, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_openni2OniPath, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_source_distortionModel, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->groupBox_scanFromDepth, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_cameraScanFromDepth_decimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1336,6 +1343,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->lineEdit_cameraRGBDImages_path_rgb->setText("");
_ui->lineEdit_cameraRGBDImages_path_depth->setText("");
_ui->doubleSpinBox_cameraRGBDImages_scale->setValue(1.0);
_ui->lineEdit_source_distortionModel->setText("");
_ui->source_comboBox_image_type->setCurrentIndex(kSrcDC1394-kSrcDC1394);
_ui->lineEdit_cameraStereoImages_path_left->setText("");
@@ -1639,6 +1647,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
settings.beginGroup("rgbd");
_ui->comboBox_cameraRGBD->setCurrentIndex(settings.value("driver", _ui->comboBox_cameraRGBD->currentIndex()).toInt());
_ui->checkbox_rgbd_colorOnly->setChecked(settings.value("rgbdColorOnly", _ui->checkbox_rgbd_colorOnly->isChecked()).toBool());
_ui->lineEdit_source_distortionModel->setText(settings.value("distortion_model", _ui->lineEdit_source_distortionModel->text()).toString());
settings.endGroup(); // rgbd
settings.beginGroup("stereo");
@@ -2066,6 +2075,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.beginGroup("rgbd");
settings.setValue("driver", _ui->comboBox_cameraRGBD->currentIndex());
settings.setValue("rgbdColorOnly", _ui->checkbox_rgbd_colorOnly->isChecked());
settings.setValue("distortion_model", _ui->lineEdit_source_distortionModel->text());
settings.endGroup(); // rgbd
settings.beginGroup("stereo");
@@ -2566,6 +2576,7 @@ void PreferencesDialog::saveWidgetState(const QWidget * widget)
const PostProcessingDialog * postProcessingDialog = qobject_cast<const PostProcessingDialog *>(widget);
const GraphViewer * graphViewer = qobject_cast<const GraphViewer *>(widget);
const CalibrationDialog * calibrationDialog = qobject_cast<const CalibrationDialog *>(widget);
const DepthCalibrationDialog * depthCalibrationDialog = qobject_cast<const DepthCalibrationDialog *>(widget);
if(cloudViewer)
{
@@ -2595,6 +2606,10 @@ void PreferencesDialog::saveWidgetState(const QWidget * widget)
{
calibrationDialog->saveSettings(settings);
}
else if(depthCalibrationDialog)
{
depthCalibrationDialog->saveSettings(settings);
}
else
{
UERROR("Widget \"%s\" cannot be exported in config file.", widget->objectName().toStdString().c_str());
@@ -2621,6 +2636,7 @@ void PreferencesDialog::loadWidgetState(QWidget * widget)
PostProcessingDialog * postProcessingDialog = qobject_cast<PostProcessingDialog *>(widget);
GraphViewer * graphViewer = qobject_cast<GraphViewer *>(widget);
CalibrationDialog * calibrationDialog = qobject_cast<CalibrationDialog *>(widget);
DepthCalibrationDialog * depthCalibrationDialog = qobject_cast<DepthCalibrationDialog *>(widget);
if(cloudViewer)
{
@@ -2650,6 +2666,10 @@ void PreferencesDialog::loadWidgetState(QWidget * widget)
{
calibrationDialog->loadSettings(settings);
}
else if(depthCalibrationDialog)
{
depthCalibrationDialog->loadSettings(settings);
}
else
{
UERROR("Widget \"%s\" cannot be loaded from config file.", widget->objectName().toStdString().c_str());
@@ -2788,6 +2808,31 @@ void PreferencesDialog::openDatabaseViewer()
}
}
void PreferencesDialog::visualizeDistortionModel()
{
if(!_ui->lineEdit_source_distortionModel->text().isEmpty() &&
QFileInfo(_ui->lineEdit_source_distortionModel->text()).exists())
{
clams::DiscreteDepthDistortionModel model;
model.load(_ui->lineEdit_source_distortionModel->text().toStdString());
if(model.isValid())
{
cv::Mat img = model.visualize();
QString name = QFileInfo(_ui->lineEdit_source_distortionModel->text()).baseName()+".png";
cv::imwrite(name.toStdString(), img);
QDesktopServices::openUrl(QUrl::fromLocalFile(name));
}
else
{
QMessageBox::warning(this, tr("Distortion Model"), tr("Model loaded from \"%1\" is not valid!").arg(_ui->lineEdit_source_distortionModel->text()));
}
}
else
{
QMessageBox::warning(this, tr("Distortion Model"), tr("File \"%1\" doesn't exist!").arg(_ui->lineEdit_source_distortionModel->text()));
}
}
void PreferencesDialog::selectCalibrationPath()
{
QString dir = _ui->lineEdit_calibrationFile->text();
@@ -2972,6 +3017,20 @@ void PreferencesDialog::selectSourceStereoVideoPath2()
}
}
void PreferencesDialog::selectSourceDistortionModel()
{
QString dir = _ui->lineEdit_source_distortionModel->text();
if(dir.isEmpty())
{
dir = getWorkingDirectory();
}
QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->lineEdit_source_distortionModel->text(), tr("Distortion model (*.bin)"));
if(!path.isEmpty())
{
_ui->lineEdit_source_distortionModel->setText(path);
}
}
void PreferencesDialog::selectSourceOniPath()
{
QString dir = _ui->lineEdit_openniOniPath->text();
@@ -4054,7 +4113,7 @@ Transform PreferencesDialog::getLaserLocalTransform() const
return t;
}
bool PreferencesDialog::getSourceDatabaseStampsUsed() const
bool PreferencesDialog::isSourceDatabaseStampsUsed() const
{
return _ui->source_checkBox_useDbStamps->isChecked();
}
@@ -4062,6 +4121,10 @@ bool PreferencesDialog::isSourceRGBDColorOnly() const
{
return _ui->checkbox_rgbd_colorOnly->isChecked();
}
QString PreferencesDialog::getSourceDistortionModel() const
{
return _ui->lineEdit_source_distortionModel->text();
}
int PreferencesDialog::getSourceImageDecimation() const
{
return _ui->spinBox_source_imageDecimation->value();
@@ -4577,6 +4640,11 @@ void PreferencesDialog::testOdometry()
_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value(),
_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
_ui->spinBox_cameraImages_scanNormalsK->value());
if(this->getSourceType() == PreferencesDialog::kSrcRGBD &&
!_ui->lineEdit_source_distortionModel->text().isEmpty())
{
cameraThread.setDistortionModel(_ui->lineEdit_source_distortionModel->text().toStdString());
}
UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent");
UEventsManager::createPipe(&odomThread, odomViewer, "OdometryEvent");
UEventsManager::createPipe(odomViewer, &odomThread, "OdometryResetEvent");
@@ -4612,6 +4680,11 @@ void PreferencesDialog::testCamera()
_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value(),
_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
_ui->spinBox_cameraImages_scanNormalsK->value());
if(this->getSourceType() == PreferencesDialog::kSrcRGBD &&
!_ui->lineEdit_source_distortionModel->text().isEmpty())
{
cameraThread.setDistortionModel(_ui->lineEdit_source_distortionModel->text().toStdString());
}
UEventsManager::createPipe(&cameraThread, window, "CameraEvent");
cameraThread.start();
guilib/src/ProgressDialog.cpp
+11
View File
@@ -53,12 +53,16 @@ ProgressDialog::ProgressDialog(QWidget *parent, Qt::WindowFlags flags) :
_detailedText->setLineWrapMode(QTextEdit::NoWrap);
_closeButton = new QPushButton(this);
_closeButton->setText("Close");
_cancelButton = new QPushButton(this);
_cancelButton->setText("Cancel");
_cancelButton-> setVisible(false);
_closeWhenDoneCheckBox = new QCheckBox(this);
_closeWhenDoneCheckBox->setChecked(true);
_closeWhenDoneCheckBox->setText("Close when done.");
_endMessage = "Finished!";
this->clear();
connect(_closeButton, SIGNAL(clicked()), this, SLOT(close()));
connect(_cancelButton, SIGNAL(clicked()), this, SIGNAL(canceled()));
QVBoxLayout * layout = new QVBoxLayout(this);
layout->addWidget(_text);
@@ -67,6 +71,8 @@ ProgressDialog::ProgressDialog(QWidget *parent, Qt::WindowFlags flags) :
QHBoxLayout * hLayout = new QHBoxLayout();
layout->addLayout(hLayout);
hLayout->addWidget(_closeWhenDoneCheckBox);
hLayout->addStretch();
hLayout->addWidget(_cancelButton);
hLayout->addWidget(_closeButton);
this->setLayout(layout);
@@ -87,6 +93,11 @@ void ProgressDialog::setAutoClose(bool on, int delayedClosingTimeSec)
_closeWhenDoneCheckBox->setChecked(on);
}
void ProgressDialog::setCancelButtonVisible(bool visible)
{
_cancelButton->setVisible(visible);
}
void ProgressDialog::appendText(const QString & text, const QColor & color)
{
_text->setText(text);
guilib/src/ui/depthCalibrationDialog.ui
+333
View File
@@ -0,0 +1,333 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>DepthCalibrationDialog</class>
<widget class="QDialog" name="DepthCalibrationDialog">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>524</width>
<height>449</height>
</rect>
</property>
<property name="windowTitle">
<string>Depth Calibration</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<item>
<widget class="QLabel" name="label_111">
<property name="text">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;CLAMS approach is used for depth calibration. Please visit &lt;a href=&quot;http://www.alexteichman.com/octo/clams/&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;CLAMS website&lt;/span&gt;&lt;/a&gt; for tips about how to get a good map for depth calibration. If you want to process multiple mapping sessions, uncheck &amp;quot;Reset previous model&amp;quot;. If logger's level is debug, 3D map and generated depth images will be shown during the calibration.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="textFormat">
<enum>Qt::RichText</enum>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
<item row="5" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_minDepth">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>1</number>
</property>
<property name="maximum">
<double>100.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_voxel">
<property name="text">
<string>Voxel size. Set 0 to disable.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.010000000000000</double>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_coneRadius">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>0.010000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.020000000000000</double>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QCheckBox" name="checkBox_laserScan">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_132">
<property name="text">
<string>Maximum depth (0 means no limit).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_maxDepth">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>1</number>
</property>
<property name="maximum">
<double>100.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>4.000000000000000</double>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_133">
<property name="text">
<string>Minimum depth.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_voxel_2">
<property name="text">
<string>Cone radius.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_voxel_3">
<property name="text">
<string>Cone std dev threshold.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_coneStdDevThresh">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>0.010000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.030000000000000</double>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="checkBox_resetModel">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_110">
<property name="text">
<string>Use 3D laser scans for 3D map.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_109">
<property name="text">
<string>Reset previous model.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_108">
<property name="text">
<string>Decimation (1-2-4-8-...).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QSpinBox" name="spinBox_decimation">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>32</number>
</property>
<property name="value">
<number>1</number>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<widget class="QLabel" name="label_112">
<property name="text">
<string>Current model:</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_trainingSamples">
<property name="text">
<string>0</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_114">
<property name="text">
<string>training samples</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QDialogButtonBox" name="buttonBox">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="standardButtons">
<set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok|QDialogButtonBox::RestoreDefaults|QDialogButtonBox::Save</set>
</property>
</widget>
</item>
</layout>
</widget>
<resources/>
<connections>
<connection>
<sender>buttonBox</sender>
<signal>rejected()</signal>
<receiver>DepthCalibrationDialog</receiver>
<slot>reject()</slot>
<hints>
<hint type="sourcelabel">
<x>316</x>
<y>260</y>
</hint>
<hint type="destinationlabel">
<x>286</x>
<y>274</y>
</hint>
</hints>
</connection>
</connections>
</ui>
guilib/src/ui/exportCloudsDialog.ui
+49 -29
View File
@@ -25,7 +25,7 @@
<x>0</x>
<y>0</y>
<width>773</width>
<height>1648</height>
<height>1689</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_13">
@@ -150,31 +150,8 @@
</property>
<layout class="QVBoxLayout" name="verticalLayout_14">
<item>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
<item row="0" column="0">
<widget class="QSpinBox" name="spinBox_decimation">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>32</number>
</property>
<property name="value">
<number>1</number>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_108">
<property name="text">
<string>3D cloud decimation (1-2-4-8-...).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0">
<layout class="QGridLayout" name="gridLayout" columnstretch="0,0,1">
<item row="1" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_maxDepth">
<property name="suffix">
<string> m</string>
@@ -193,7 +170,40 @@
</property>
</widget>
</item>
<item row="1" column="1">
<item row="0" column="2">
<widget class="QLabel" name="label_108">
<property name="text">
<string>3D cloud decimation (1-2-4-8-...).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QSpinBox" name="spinBox_decimation">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>32</number>
</property>
<property name="value">
<number>1</number>
</property>
</widget>
</item>
<item row="3" column="2">
<widget class="QLabel" name="label_134">
<property name="text">
<string>Path to a depth distortion model to apply (output from depth calibration).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="label_132">
<property name="text">
<string>3D cloud maximum depth (0 means no limit).</string>
@@ -203,7 +213,7 @@
</property>
</widget>
</item>
<item row="2" column="1">
<item row="2" column="2">
<widget class="QLabel" name="label_133">
<property name="text">
<string>3D cloud minimum depth.</string>
@@ -213,7 +223,7 @@
</property>
</widget>
</item>
<item row="2" column="0">
<item row="2" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_minDepth">
<property name="suffix">
<string> m</string>
@@ -232,6 +242,16 @@
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLineEdit" name="lineEdit_distortionModel"/>
</item>
<item row="3" column="0">
<widget class="QToolButton" name="toolButton_distortionModel">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
guilib/src/ui/mainWindow.ui
+6
View File
@@ -56,6 +56,7 @@
<addaction name="menuExport_poses"/>
<addaction name="actionExport_images_RGB_jpg_Depth_png"/>
<addaction name="actionExport_cameras_in_Bundle_format_out"/>
<addaction name="actionDepth_Calibration"/>
<addaction name="separator"/>
<addaction name="actionClose_database"/>
<addaction name="separator"/>
@@ -1345,6 +1346,11 @@
<string>RealSense</string>
</property>
</action>
<action name="actionDepth_Calibration">
<property name="text">
<string>Depth calibration...</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>
guilib/src/ui/preferencesDialog.ui
+51 -7
View File
@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-490</y>
<y>-179</y>
<width>673</width>
<height>2496</height>
</rect>
@@ -2357,7 +2357,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item>
<widget class="QStackedWidget" name="stackedWidget_src">
<property name="currentIndex">
<number>1</number>
<number>0</number>
</property>
<widget class="QWidget" name="page_41">
<layout class="QVBoxLayout" name="verticalLayout_64">
@@ -2384,7 +2384,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_55" columnstretch="0,0,1">
<layout class="QGridLayout" name="gridLayout_55" columnstretch="0,0,0,1,0">
<item row="0" column="1">
<widget class="QComboBox" name="comboBox_cameraRGBD">
<property name="sizeAdjustPolicy">
@@ -2432,7 +2432,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</item>
</widget>
</item>
<item row="0" column="2">
<item row="0" column="3">
<widget class="QLabel" name="label_228">
<property name="text">
<string>Driver</string>
@@ -2442,7 +2442,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="3" column="2">
<item row="1" column="3">
<widget class="QLabel" name="label_229">
<property name="text">
<string>Only RGB images are published.</string>
@@ -2452,7 +2452,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="3" column="1">
<item row="1" column="1">
<widget class="QCheckBox" name="checkbox_rgbd_colorOnly">
<property name="text">
<string/>
@@ -2462,12 +2462,56 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QToolButton" name="toolButton_source_distortionModel">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item row="2" column="3">
<widget class="QLabel" name="label_341">
<property name="text">
<string>Distortion model (output from depth calibration).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLineEdit" name="lineEdit_source_distortionModel">
<property name="minimumSize">
<size>
<width>100</width>
<height>0</height>
</size>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QToolButton" name="toolButton_distortionModel">
<property name="toolTip">
<string>Open database viewer</string>
</property>
<property name="text">
<string/>
</property>
<property name="icon">
<iconset resource="../GuiLib.qrc">
<normaloff>:/images/mag_glass.png</normaloff>:/images/mag_glass.png</iconset>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QStackedWidget" name="stackedWidget_rgbd">
<property name="currentIndex">
<number>2</number>
<number>1</number>
</property>
<widget class="QWidget" name="page_32">
<layout class="QVBoxLayout" name="verticalLayout_63">