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

0.15.1: CameraImages: added ground truth time diff, fixed memory leak when loading binary scans. CameraRGBDImages and CameraStereoImages: fixed start id. Feature2D: added grid rows and cols parameters (Kp/GridRows, Kp/GridCols, Vis/GridRows, Vis/GridCols). OdomInfo: publish bundle frames. OdometryORBSLAM2: added OdomORBSLAM2/Fps and OdomORBSLAM2/MaxFeatures parameters. Registration: added Reg/RepeatOnce parameter and removed variance normalization. For util2d::getDepth() and util3d::projectDepthTo3D(), maxZError parameter is now depthErrorRatio to be dependent of the sensor range. Database: save image width and height from stereo calibration. OptimizerG2O: fixed SBA optimization when using g2o built from ORBSLAM2 library. OptimizerGTSAM: to increase optimization stability, all rotations in information matrix are divided by 100000. Added rtabmap-report tool.

Browse Source
This commit is contained in:
matlabbe
2017-11-30 16:52:03 -05:00
parent 821c1c938e
commit 4452e637ad
46 changed files with 1892 additions and 802 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/SensorData.cpp
+42
View File
@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/SensorData.h"
#include "rtabmap/core/Compression.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/utilite/ULogger.h"
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
@@ -793,5 +794,46 @@ long SensorData::getMemoryUsed() const // Return memory usage in Bytes
_descriptors.total()*_descriptors.elemSize();
}
bool SensorData::isPointVisibleFromCameras(const cv::Point3f & pt) const
{
if(_cameraModels.size() >= 1)
{
for(unsigned int i=0; i<_cameraModels.size(); ++i)
{
if(_cameraModels[i].isValidForProjection() && !_cameraModels[i].localTransform().isNull())
{
cv::Point3f ptInCameraFrame = util3d::transformPoint(pt, _cameraModels[i].localTransform().inverse());
if(ptInCameraFrame.z > 0.0f)
{
int borderWidth = int(float(_cameraModels[i].imageWidth())* 0.2);
int u, v;
_cameraModels[i].reproject(ptInCameraFrame.x, ptInCameraFrame.y, ptInCameraFrame.z, u, v);
if(uIsInBounds(u, borderWidth, _cameraModels[i].imageWidth()-2*borderWidth) &&
uIsInBounds(v, borderWidth, _cameraModels[i].imageHeight()-2*borderWidth))
{
return true;
}
}
}
}
}
else if(_stereoCameraModel.isValidForProjection())
{
cv::Point3f ptInCameraFrame = util3d::transformPoint(pt, _stereoCameraModel.localTransform().inverse());
if(ptInCameraFrame.z > 0.0f)
{
int u, v;
_stereoCameraModel.left().reproject(ptInCameraFrame.x, ptInCameraFrame.y, ptInCameraFrame.z, u, v);
return uIsInBounds(u, 0, _stereoCameraModel.left().imageWidth()) &&
uIsInBounds(v, 0, _stereoCameraModel.left().imageHeight());
}
}
else
{
UERROR("no valid camera model!");
}
return false;
}
} // namespace rtabmap