mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 01:20:25 +08:00
iOS various updates (#1340)
* Added Data Recording Mode. Added option to filter ARKit localization jumps. * Implemented max acc relocalization filtering (working on iOS) * Fixed android build, added re-localization max acceleration parameter * ARCore Java: Fixed pose of depth not available at stamp requested * Android log fix * Added libLAS support * ios: added LAS support * updated dep install script to skip libraries already installed * CameraMobile: Fixed origin not updated if updateOnRender() is used * Default max opt error increased to 2x to reduce number of loop closures rejected. OptimizerGTSAM: updated gravity noise model to use same sigma for both parameters. * Updated license * bump 0.21.7 * updated license date
This commit is contained in:
matlabbe
43
corelib/include/rtabmap/core/LASWriter.h
Normal file
43
corelib/include/rtabmap/core/LASWriter.h
Normal file
@@ -0,0 +1,43 @@
|
||||
/*
|
||||
Copyright (c) 2010-2024, 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 CORELIB_INCLUDE_RTABMAP_CORE_LASWRITER_H_
|
||||
#define CORELIB_INCLUDE_RTABMAP_CORE_LASWRITER_H_
|
||||
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
int saveLASFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud, const std::vector<int> & cameraIds = std::vector<int>());
|
||||
int saveLASFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const std::vector<int> & cameraIds = std::vector<int>(), const std::vector<float> & intensities = std::vector<float>());
|
||||
int saveLASFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud, const std::vector<int> & cameraIds = std::vector<int>());
|
||||
|
||||
}
|
||||
|
||||
|
||||
#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LASWRITER_H_ */
|
||||
@@ -539,6 +539,22 @@ IF(PDAL_FOUND)
|
||||
ENDIF(PDAL_VERSION VERSION_LESS "1.7")
|
||||
ENDIF(PDAL_FOUND)
|
||||
|
||||
IF(libLAS_FOUND)
|
||||
SET(INCLUDE_DIRS
|
||||
${INCLUDE_DIRS}
|
||||
${libLAS_INCLUDE_DIRS}
|
||||
)
|
||||
SET(LIBRARIES
|
||||
${LIBRARIES}
|
||||
${libLAS_LIBRARIES}
|
||||
)
|
||||
SET(SRC_FILES
|
||||
${SRC_FILES}
|
||||
LASWriter.cpp
|
||||
)
|
||||
ENDIF(libLAS_FOUND)
|
||||
|
||||
|
||||
IF(CudaSift_FOUND)
|
||||
#target
|
||||
SET(LIBRARIES
|
||||
|
||||
169
corelib/src/LASWriter.cpp
Normal file
169
corelib/src/LASWriter.cpp
Normal file
@@ -0,0 +1,169 @@
|
||||
/*
|
||||
Copyright (c) 2010-2024, 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 <rtabmap/core/LASWriter.h>
|
||||
#include <rtabmap/utilite/UFile.h>
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UStl.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
|
||||
#include <fstream>
|
||||
#include <liblas/liblas.hpp>
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
int saveLASFile(const std::string & filePath,
|
||||
const pcl::PointCloud<pcl::PointXYZ> & cloud,
|
||||
const std::vector<int> & cameraIds)
|
||||
{
|
||||
UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
|
||||
uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
|
||||
|
||||
std::ofstream ofs;
|
||||
ofs.open(filePath, std::ios::out | std::ios::binary);
|
||||
|
||||
liblas::Header header;
|
||||
header.SetScale(0.001, 0.001, 0.001);
|
||||
|
||||
header.SetCompressed(uToLowerCase(UFile::getExtension(filePath)) == "laz");
|
||||
|
||||
try {
|
||||
liblas::Writer writer(ofs, header);
|
||||
|
||||
for(size_t i=0; i<cloud.size(); ++i)
|
||||
{
|
||||
liblas::Point point(&header);
|
||||
point.SetCoordinates(cloud.at(i).x, cloud.at(i).y, cloud.at(i).z);
|
||||
if(!cameraIds.empty())
|
||||
{
|
||||
point.SetPointSourceID(cameraIds.at(i));
|
||||
}
|
||||
|
||||
writer.WritePoint(point);
|
||||
}
|
||||
}
|
||||
catch(liblas::configuration_error & e)
|
||||
{
|
||||
UERROR("\"laz\" format not available, use \"las\" instead: %s", e.what());
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0; //success
|
||||
}
|
||||
|
||||
int saveLASFile(const std::string & filePath,
|
||||
const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
|
||||
const std::vector<int> & cameraIds,
|
||||
const std::vector<float> & intensities)
|
||||
{
|
||||
UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
|
||||
uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
|
||||
UASSERT_MSG(intensities.empty() || intensities.size() == cloud.size(),
|
||||
uFormat("intensities=%d cloud=%d", (int)intensities.size(), (int)cloud.size()).c_str());
|
||||
|
||||
UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
|
||||
uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
|
||||
|
||||
std::ofstream ofs;
|
||||
ofs.open(filePath, std::ios::out | std::ios::binary);
|
||||
|
||||
liblas::Header header;
|
||||
|
||||
header.SetScale(0.001, 0.001, 0.001);
|
||||
header.SetCompressed(uToLowerCase(UFile::getExtension(filePath)) == "laz");
|
||||
|
||||
try {
|
||||
liblas::Writer writer(ofs, header);
|
||||
|
||||
for(size_t i=0; i<cloud.size(); ++i)
|
||||
{
|
||||
liblas::Point point(&header);
|
||||
point.SetCoordinates(cloud.at(i).x, cloud.at(i).y, cloud.at(i).z);
|
||||
liblas::Color color(cloud.at(i).r*65535/255, cloud.at(i).g*65535/255, cloud.at(i).b*65535/255);
|
||||
point.SetColor(color);
|
||||
if(!cameraIds.empty())
|
||||
{
|
||||
point.SetPointSourceID(cameraIds.at(i));
|
||||
}
|
||||
if(!intensities.empty())
|
||||
{
|
||||
point.SetIntensity(intensities.at(i));
|
||||
}
|
||||
|
||||
writer.WritePoint(point);
|
||||
}
|
||||
}
|
||||
catch(liblas::configuration_error & e)
|
||||
{
|
||||
UERROR("\"laz\" format not available, use \"las\" instead: %s", e.what());
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0; //success
|
||||
}
|
||||
|
||||
int saveLASFile(const std::string & filePath,
|
||||
const pcl::PointCloud<pcl::PointXYZI> & cloud,
|
||||
const std::vector<int> & cameraIds)
|
||||
{
|
||||
UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
|
||||
uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
|
||||
|
||||
std::ofstream ofs;
|
||||
ofs.open(filePath, std::ios::out | std::ios::binary);
|
||||
|
||||
liblas::Header header;
|
||||
header.SetScale(0.001, 0.001, 0.001);
|
||||
|
||||
header.SetCompressed(uToLowerCase(UFile::getExtension(filePath)) == "laz");
|
||||
|
||||
try {
|
||||
liblas::Writer writer(ofs, header);
|
||||
|
||||
for(size_t i=0; i<cloud.size(); ++i)
|
||||
{
|
||||
liblas::Point point(&header);
|
||||
point.SetCoordinates(cloud.at(i).x, cloud.at(i).y, cloud.at(i).z);
|
||||
if(!cameraIds.empty())
|
||||
{
|
||||
point.SetPointSourceID(cameraIds.at(i));
|
||||
}
|
||||
|
||||
writer.WritePoint(point);
|
||||
}
|
||||
}
|
||||
catch(liblas::configuration_error & e)
|
||||
{
|
||||
UERROR("\"laz\" format not available, use \"las\" instead: %s", e.what());
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0; //success
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -685,6 +685,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
|
||||
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
|
||||
#else
|
||||
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
|
||||
#endif
|
||||
str = "With libLAS:";
|
||||
#ifdef RTABMAP_LIBLAS
|
||||
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
|
||||
#else
|
||||
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
|
||||
#endif
|
||||
str = "With CudaSift:";
|
||||
#ifdef RTABMAP_CUDASIFT
|
||||
|
||||
@@ -217,7 +217,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
|
||||
{
|
||||
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(
|
||||
(gtsam::Vector(6) <<
|
||||
(hasGravityConstraints?2:1e-2), (hasGravityConstraints?2:1e-2), hasGPSPrior?1e-2:std::numeric_limits<double>::min(), // roll, pitch, fixed yaw if there are no priors
|
||||
(hasGravityConstraints?2:1e-2), (hasGravityConstraints?2:1e-2), (hasGPSPrior?1e-2:std::numeric_limits<double>::min()), // roll, pitch, fixed yaw if there are no priors
|
||||
(hasGPSPrior?2:1e-2), hasGPSPrior?2:1e-2, hasGPSPrior?2:1e-2 // xyz
|
||||
).finished());
|
||||
graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
|
||||
@@ -473,7 +473,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
|
||||
{
|
||||
Vector3 r = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation().xyz();
|
||||
gtsam::Unit3 nG = gtsam::Rot3::RzRyRx(r.x(), r.y(), 0).rotate(gtsam::Unit3(0,0,-1));
|
||||
gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigmas(gtsam::Vector2(gravitySigma(), 10));
|
||||
gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigmas(gtsam::Vector2(gravitySigma(), gravitySigma()));
|
||||
graph.add(Pose3GravityFactor(iter->first, nG, model, Unit3(0,0,1)));
|
||||
lastAddedConstraints_.push_back(ConstraintToFactor(iter->first, iter->first, -1));
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user