42 #include <opencv2/imgproc/types_c.h>
43 #include <opencv2/stitching/detail/exposure_compensate.hpp>
48 #include <pcl/io/io.h>
67 double poseTimeOffset,
68 float poseScaleFactor,
96 double poseTimeOffset,
97 float poseScaleFactor,
110 double poseTimeOffset,
111 float poseScaleFactor,
115 _odomSensor(odomSensor),
117 _extrinsicsOdomToCamera(extrinsics *
CameraModel::opticalRotation()),
119 _poseTimeOffset(poseTimeOffset),
120 _poseScaleFactor(poseScaleFactor),
121 _poseWaitTime(poseWaitTime),
123 _stereoExposureCompensation(
false),
127 _stereoToDepth(
false),
128 _scanDeskewing(
false),
129 _scanFromDepth(
false),
130 _scanDownsampleStep(1),
133 _scanVoxelSize(0.0
f),
135 _scanNormalsRadius(0.0
f),
136 _scanForceGroundNormalsUp(
false),
139 _bilateralFiltering(
false),
140 _bilateralSigmaS(10),
141 _bilateralSigmaR(0.1),
143 _imuBaseFrameConversion(
false),
145 _depthAsMask(
Parameters::defaultVisDepthAsMask())
209 UERROR(
"Loaded distortion model \"%s\" is not valid!",
path.c_str());
270 bool forceGroundNormalsUp,
273 setScanParameters(fromDepth, downsampleStep, rangeMin, rangeMax, voxelSize, normalsK, normalsRadius, forceGroundNormalsUp?0.8
f:0.0
f, deskewing);
284 float groundNormalsUp,
332 double lidarStamp = 0.0;
333 double cameraStamp = 0.0;
337 if(
data.stamp() == 0.0)
339 UWARN(
"Could not capture scan!");
343 lidarStamp =
data.stamp();
347 if(cameraData.
stamp() == 0.0)
349 UWARN(
"Could not capture image!");
354 while(cameraData.
stamp() <
data.stamp() &&
365 cameraStamp = cameraData.
stamp();
368 UWARN(
"Could not get camera frame (%f) with stamp more recent than lidar frame (%f) after waiting for %f seconds.",
389 if(
data.stamp() == 0.0)
391 UWARN(
"Could not capture image!");
395 cameraStamp = cameraData.
stamp();
403 UDEBUG(
"Deskewing begin");
404 if(!
data.laserScanRaw().empty() &&
data.laserScanRaw().hasTime())
407 data.laserScanRaw().data().ptr<
float>(0,
data.laserScanRaw().size()-1)[
data.laserScanRaw().getTimeOffset()] -
408 data.laserScanRaw().data().ptr<
float>(0, 0)[
data.laserScanRaw().getTimeOffset()];
413 double firstStamp =
data.stamp() +
data.laserScanRaw().data().ptr<
float>(0, 0)[
data.laserScanRaw().getTimeOffset()];
414 double lastStamp =
data.stamp() +
data.laserScanRaw().data().ptr<
float>(0,
data.laserScanRaw().size()-1)[
data.laserScanRaw().getTimeOffset()];
445 info.timeDeskewing = timeDeskewing.
ticks();
448 data.setLaserScan(scanDeskewed);
451 else if(!
data.laserScanRaw().empty())
456 else if(!
data.laserScanRaw().empty())
458 UWARN(
"The input scan doesn't have time channel (scan format received=%s)!. Lidar won't be deskewed!",
data.laserScanRaw().formatName().c_str());
471 info.odomPose = pose;
472 info.odomCovariance = covariance;
480 if(cameraStamp != 0.0)
483 if(lidarStamp > 0.0 && lidarStamp != cameraStamp)
487 cameraCorrection =
info.odomPose.inverse() * pose;
491 UWARN(
"Could not get pose at stamp %f, the camera local motion against lidar won't be adjusted.", cameraStamp);
496 if(!
data.cameraModels().empty())
503 else if(!
data.stereoCameraModels().empty())
515 cv::Vec4d(
q.x(),
q.y(),
q.z(),
q.w()), cv::Mat(),
516 cv::Vec3d(), cv::Mat(),
517 cv::Vec3d(), cv::Mat(),
523 UWARN(
"Could not get pose at stamp %f", data.
stamp());
530 info.odomPose.setNull();
542 UWARN(
"no more data...");
564 UERROR(
"CameraFreenect2: Failed to kill normally the Freenect2 driver! The thread is locked "
565 "on waitForNewFrame() method of libfreenect2. This maybe caused by not linking on the right libusb. "
566 "Note that rtabmap should link on libusb of libfreenect2. "
567 "Tip before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
579 if(!
data.depthRaw().empty())
581 data.setRGBDImage(
data.imageRaw(), cv::Mat(),
data.cameraModels());
583 else if(!
data.rightRaw().empty())
585 std::vector<CameraModel> models;
586 for(
size_t i=0;
i<
data.stereoCameraModels().
size(); ++
i)
588 models.push_back(
data.stereoCameraModels()[
i].left());
590 data.setRGBDImage(
data.imageRaw(), cv::Mat(), models);
602 cv::Mat depth =
data.depthRaw().clone();
604 data.setRGBDImage(
data.imageRaw(), depth,
data.cameraModels());
608 UERROR(
"Distortion model size is %dx%d but depth image is %dx%d!",
610 data.depthRaw().cols,
data.depthRaw().rows);
626 if(!
data.depthRaw().empty() &&
629 UERROR(
"Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! "
637 if(
data.depthOrRightRaw().rows <= image.rows ||
data.depthOrRightRaw().cols <= image.cols)
644 while(
data.depthOrRightRaw().rows / depthDecimation > image.rows ||
645 data.depthOrRightRaw().cols / depthDecimation > image.cols ||
646 data.depthOrRightRaw().rows % depthDecimation != 0 ||
647 data.depthOrRightRaw().cols % depthDecimation != 0)
651 UDEBUG(
"depthDecimation=%d", depthDecimation);
655 std::vector<CameraModel> models =
data.cameraModels();
656 for(
unsigned int i=0;
i<models.size(); ++
i)
658 if(models[
i].isValidForProjection())
665 data.setRGBDImage(image, depthOrRight, models);
669 std::vector<StereoCameraModel> stereoModels =
data.stereoCameraModels();
670 for(
unsigned int i=0;
i<stereoModels.size(); ++
i)
672 if(stereoModels[
i].isValidForProjection())
677 if(!stereoModels.empty())
679 data.setStereoImage(image, depthOrRight, stereoModels);
682 std::vector<cv::KeyPoint> kpts =
data.keypoints();
684 for(
unsigned int i=0;
i<kpts.size(); ++
i)
689 kpts[
i].octave -= log2value;
691 data.setFeatures(kpts,
data.keypoints3D(),
data.descriptors());
698 if(
data.cameraModels().size() == 1)
703 cv::flip(
data.imageRaw(), tmpRgb, 1);
706 if(
data.cameraModels()[0].cx())
709 data.cameraModels()[0].fx(),
710 data.cameraModels()[0].fy(),
711 float(
data.imageRaw().cols) -
data.cameraModels()[0].cx(),
712 data.cameraModels()[0].cy(),
713 data.cameraModels()[0].localTransform(),
714 data.cameraModels()[0].Tx(),
715 data.cameraModels()[0].imageSize());
717 cv::Mat tmpDepth =
data.depthOrRightRaw();
718 if(!
data.depthRaw().empty())
720 cv::flip(
data.depthRaw(), tmpDepth, 1);
722 data.setRGBDImage(tmpRgb, tmpDepth, tmpModel);
727 UWARN(
"Mirroring is not implemented for multiple cameras or stereo...");
738 if(
data.imageRaw().type() == CV_8UC1)
740 cv::equalizeHist(
data.imageRaw(), image);
742 else if(
data.imageRaw().type() == CV_8UC3)
745 cv::cvtColor(
data.imageRaw(), image, CV_BGR2YCrCb);
746 cv::split(image, channels);
747 cv::equalizeHist(channels[0], channels[0]);
748 cv::merge(channels, 3, image);
749 cv::cvtColor(image, image, CV_YCrCb2BGR);
751 if(!
data.depthRaw().empty())
753 data.setRGBDImage(image,
data.depthRaw(),
data.cameraModels());
755 else if(!
data.rightRaw().empty())
758 if(
data.rightRaw().type() == CV_8UC1)
760 cv::equalizeHist(
data.rightRaw(), right);
762 else if(
data.rightRaw().type() == CV_8UC3)
765 cv::cvtColor(
data.rightRaw(), right, CV_BGR2YCrCb);
766 cv::split(right, channels);
767 cv::equalizeHist(channels[0], channels[0]);
768 cv::merge(channels, 3, right);
769 cv::cvtColor(right, right, CV_YCrCb2BGR);
771 data.setStereoImage(image, right,
data.stereoCameraModels()[0]);
776 cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0);
777 if(
data.imageRaw().type() == CV_8UC1)
779 clahe->apply(
data.imageRaw(), image);
781 else if(
data.imageRaw().type() == CV_8UC3)
784 cv::cvtColor(
data.imageRaw(), image, CV_BGR2YCrCb);
785 cv::split(image, channels);
786 clahe->apply(channels[0], channels[0]);
787 cv::merge(channels, 3, image);
788 cv::cvtColor(image, image, CV_YCrCb2BGR);
790 if(!
data.depthRaw().empty())
792 data.setRGBDImage(image,
data.depthRaw(),
data.cameraModels());
794 else if(!
data.rightRaw().empty())
797 if(
data.rightRaw().type() == CV_8UC1)
799 clahe->apply(
data.rightRaw(), right);
801 else if(
data.rightRaw().type() == CV_8UC3)
804 cv::cvtColor(
data.rightRaw(), right, CV_BGR2YCrCb);
805 cv::split(right, channels);
806 clahe->apply(channels[0], channels[0]);
807 cv::merge(channels, 3, right);
808 cv::cvtColor(right, right, CV_YCrCb2BGR);
810 data.setStereoImage(image, right,
data.stereoCameraModels()[0]);
818 if(
data.stereoCameraModels().size()==1)
820 #if CV_MAJOR_VERSION < 3
821 UWARN(
"Stereo exposure compensation not implemented for OpenCV version under 3.");
825 cv::Ptr<cv::detail::ExposureCompensator> compensator = cv::detail::ExposureCompensator::createDefault(cv::detail::ExposureCompensator::GAIN);
826 std::vector<cv::Point> topLeftCorners(2, cv::Point(0,0));
827 std::vector<cv::UMat> images;
828 std::vector<cv::UMat> masks(2, cv::UMat(
data.imageRaw().size(), CV_8UC1, cv::Scalar(255)));
829 images.push_back(
data.imageRaw().getUMat(cv::ACCESS_READ));
830 images.push_back(
data.rightRaw().getUMat(cv::ACCESS_READ));
831 compensator->feed(topLeftCorners, images, masks);
832 cv::Mat imgLeft =
data.imageRaw().clone();
833 compensator->apply(0, cv::Point(0,0), imgLeft, masks[0]);
834 cv::Mat imgRight =
data.rightRaw().clone();
835 compensator->apply(1, cv::Point(0,0), imgRight, masks[1]);
836 data.setStereoImage(imgLeft, imgRight,
data.stereoCameraModels()[0]);
837 cv::detail::GainCompensator * gainCompensator = (cv::detail::GainCompensator*)compensator.get();
838 UDEBUG(
"gains = %f %f ", gainCompensator->gains()[0], gainCompensator->gains()[1]);
839 if(
info)
info->timeStereoExposureCompensation =
timer.ticks();
844 UWARN(
"Stereo exposure compensation only is not implemented to multiple stereo cameras...");
848 if(
_stereoToDepth && !
data.imageRaw().empty() && !
data.stereoCameraModels().empty() &&
data.stereoCameraModels()[0].isValidForProjection() && !
data.rightRaw().empty())
850 if(
data.stereoCameraModels().size()==1)
856 data.stereoCameraModels()[0].left().fx(),
857 data.stereoCameraModels()[0].baseline());
860 data.stereoCameraModels()[0].left().fx(),
861 data.stereoCameraModels()[0].left().fy(),
862 data.stereoCameraModels()[0].left().cx(),
863 data.stereoCameraModels()[0].left().cy(),
864 data.stereoCameraModels()[0].localTransform(),
865 -
data.stereoCameraModels()[0].baseline()*
data.stereoCameraModels()[0].left().fx(),
866 data.stereoCameraModels()[0].left().imageSize());
872 UWARN(
"Stereo to depth is not implemented for multiple stereo cameras...");
876 data.cameraModels().size() &&
877 data.cameraModels().at(0).isValidForProjection() &&
878 !
data.depthRaw().empty())
881 if(
data.laserScanRaw().isEmpty())
885 pcl::IndicesPtr validIndices(
new std::vector<int>);
894 const Transform & baseToScan =
data.cameraModels()[0].localTransform();
895 if(validIndices->size())
901 maxPoints =
ratio * maxPoints;
903 else if(!cloud->is_dense)
905 pcl::PointCloud<pcl::PointXYZRGB>::Ptr denseCloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
906 pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
914 Eigen::Vector3f viewPoint(baseToScan.
x(), baseToScan.
y(), baseToScan.
z());
916 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudNormals(
new pcl::PointCloud<pcl::PointXYZRGBNormal>);
917 pcl::concatenateFields(*cloud, *normals, *cloudNormals);
931 UWARN(
"Option to create laser scan from depth image is enabled, but "
932 "there is already a laser scan in the captured sensor data. Scan from "
933 "depth will not be created.");
936 else if(!
data.laserScanRaw().isEmpty())
946 if(
data.imu().angularVelocity()[0] == 0 &&
947 data.imu().angularVelocity()[1] == 0 &&
948 data.imu().angularVelocity()[2] == 0 &&
949 data.imu().linearAcceleration()[0] == 0 &&
950 data.imu().linearAcceleration()[1] == 0 &&
951 data.imu().linearAcceleration()[2] == 0)
953 UWARN(
"IMU's acc and gyr values are null! Please disable IMU filtering.");
977 cv::Vec4d(qx,qy,
qz,qw), cv::Mat::eye(3,3,CV_64FC1),
982 UDEBUG(
"%f %f %f %f (gyro=%f %f %f, acc=%f %f %f, %fs)",
983 data.imu().orientation()[0],
984 data.imu().orientation()[1],
985 data.imu().orientation()[2],
986 data.imu().orientation()[3],
987 data.imu().angularVelocity()[0],
988 data.imu().angularVelocity()[1],
989 data.imu().angularVelocity()[2],
990 data.imu().linearAcceleration()[0],
991 data.imu().linearAcceleration()[1],
992 data.imu().linearAcceleration()[2],
999 UDEBUG(
"Detecting features");
1000 cv::Mat grayScaleImg =
data.imageRaw();
1001 if(
data.imageRaw().channels() > 1)
1004 cv::cvtColor(grayScaleImg, tmp, cv::COLOR_BGR2GRAY);
1011 if(
data.imageRaw().rows %
data.depthRaw().rows == 0 &&
1012 data.imageRaw().cols %
data.depthRaw().cols == 0 &&
1013 data.imageRaw().rows/
data.depthRaw().rows ==
data.imageRaw().cols/
data.depthRaw().cols)
1019 UWARN(
"%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.",
1020 Parameters::kVisDepthAsMask().
c_str(),
1021 data.imageRaw().rows,
data.imageRaw().cols,
1022 data.depthRaw().rows,
data.depthRaw().cols);
1027 cv::Mat descriptors;
1028 std::vector<cv::Point3f> keypoints3D;
1029 if(!keypoints.empty())
1032 if(!keypoints.empty())
1038 data.setFeatures(keypoints, keypoints3D, descriptors);