CameraSeerSense.cpp
Go to the documentation of this file.
1 #include <rtabmap/core/camera/CameraSeerSense.h>
2 #include <rtabmap/utilite/UEventsManager.h>
3 
4 namespace rtabmap {
5 
6 bool CameraSeerSense::available()
7 {
8 #ifdef RTABMAP_XVSDK
9  return true;
10 #else
11  return false;
12 #endif
13 }
14 
15 CameraSeerSense::CameraSeerSense(bool computeOdometry, float imageRate, const Transform & localTransform) :
16  Camera(imageRate, localTransform)
17 #ifdef RTABMAP_XVSDK
18  ,
19  computeOdometry_(computeOdometry),
20  imuId_(0),
21  tofId_(0)
22 #endif
23 {
24 #ifdef RTABMAP_XVSDK
25  xv::setLogLevel(xv::LogLevel(ULogger::level()+1));
26 #endif
27 }
28 
29 CameraSeerSense::~CameraSeerSense()
30 {
31 #ifdef RTABMAP_XVSDK
32  if(imuId_)
33  device_->imuSensor()->unregisterCallback(imuId_);
34 
35  if(tofId_)
36  device_->tofCamera()->unregisterColorDepthImageCallback(tofId_);
37 
38  if(device_->slam())
39  device_->slam()->stop();
40 
41  if(device_->imuSensor())
42  device_->imuSensor()->stop();
43 
44  if(device_->colorCamera())
45  device_->colorCamera()->stop();
46 
47  if(device_->tofCamera())
48  device_->tofCamera()->stop();
49 
50  dataReady_.release();
51 #endif
52 }
53 
54 bool CameraSeerSense::init(const std::string & calibrationFolder, const std::string & cameraName)
55 {
56  UDEBUG("");
57 #ifdef RTABMAP_XVSDK
58  auto devices = xv::getDevices(3);
59  if(devices.empty())
60  {
61  UERROR("Timeout for SeerSense device detection.");
62  return false;
63  }
64  device_ = devices.begin()->second;
65 
66  UASSERT(device_->imuSensor());
67  device_->imuSensor()->start();
68  imuId_ = device_->imuSensor()->registerCallback([this](const xv::Imu & xvImu) {
69  if(xvImu.hostTimestamp > 0)
70  {
71  if(isInterIMUPublishing())
72  {
73  IMU imu(cv::Vec3d(xvImu.gyro[0], xvImu.gyro[1], xvImu.gyro[2]), cv::Mat::eye(3,3,CV_64FC1),
74  cv::Vec3d(xvImu.accel[0], xvImu.accel[1], xvImu.accel[2]), cv::Mat::eye(3,3,CV_64FC1),
75  this->getLocalTransform());
76  this->postInterIMU(imu, xvImu.hostTimestamp);
77  }
78  else
79  {
80  UScopeMutex lock(imuMutex_);
81  imuBuffer_.emplace_hint(imuBuffer_.end(), xvImu.hostTimestamp,
82  std::make_pair(cv::Vec3d(xvImu.gyro[0], xvImu.gyro[1], xvImu.gyro[2]), cv::Vec3d(xvImu.accel[0], xvImu.accel[1], xvImu.accel[2])));
83  }
84  }
85  });
86 
87  if(computeOdometry_)
88  {
89  UASSERT(device_->slam());
90  device_->slam()->start(xv::Slam::Mode::Mixed);
91  }
92 
93  auto frameRate = xv::TofCamera::Framerate::FPS_30;
94  if(this->getImageRate() > 25)
95  frameRate = xv::TofCamera::Framerate::FPS_30;
96  else if(this->getImageRate() > 20)
97  frameRate = xv::TofCamera::Framerate::FPS_25;
98  else if(this->getImageRate() > 15)
99  frameRate = xv::TofCamera::Framerate::FPS_20;
100  else if(this->getImageRate() > 10)
101  frameRate = xv::TofCamera::Framerate::FPS_15;
102  else if(this->getImageRate() > 5)
103  frameRate = xv::TofCamera::Framerate::FPS_10;
104  else if(this->getImageRate() > 0)
105  frameRate = xv::TofCamera::Framerate::FPS_5;
106 
107  UASSERT(device_->colorCamera());
108  device_->colorCamera()->setResolution(xv::ColorCamera::Resolution::RGB_640x480);
109  device_->colorCamera()->start();
110  UASSERT(device_->tofCamera());
111  device_->tofCamera()->setSonyTofSetting(xv::TofCamera::SonyTofLibMode::IQMIX_DF, xv::TofCamera::Resolution::QVGA, frameRate);
112  device_->tofCamera()->start();
113 
114  UASSERT(!device_->tofCamera()->calibration().empty());
115  auto xvTofCalib = device_->tofCamera()->calibration()[0];
116  UASSERT(!xvTofCalib.pdcm.empty());
117  cv::Mat D(1, xvTofCalib.pdcm[0].distor.size(), CV_64FC1, xvTofCalib.pdcm[0].distor.begin());
118  cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
119  cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
120  P.at<double>(0,0) = xvTofCalib.pdcm[0].fx;
121  P.at<double>(1,1) = xvTofCalib.pdcm[0].fy;
122  P.at<double>(0,2) = xvTofCalib.pdcm[0].u0;
123  P.at<double>(1,2) = xvTofCalib.pdcm[0].v0;
124  cv::Mat K = P.colRange(0, 3);
125  cameraModel_ = CameraModel(device_->id(), cv::Size(xvTofCalib.pdcm[0].w, xvTofCalib.pdcm[0].h), K, D, R, P,
126  this->getLocalTransform() * Transform(
127  xvTofCalib.pose.rotation()[0], xvTofCalib.pose.rotation()[1], xvTofCalib.pose.rotation()[2], xvTofCalib.pose.translation()[0],
128  xvTofCalib.pose.rotation()[3], xvTofCalib.pose.rotation()[4], xvTofCalib.pose.rotation()[5], xvTofCalib.pose.translation()[1],
129  xvTofCalib.pose.rotation()[6], xvTofCalib.pose.rotation()[7], xvTofCalib.pose.rotation()[8], xvTofCalib.pose.translation()[2]
130  )).scaled(0.5);
131  UASSERT(cameraModel_.isValidForRectification());
132  cameraModel_.initRectificationMap();
133 
134  lastData_ = std::pair<double, std::pair<cv::Mat, cv::Mat>>();
135  tofId_ = device_->tofCamera()->registerColorDepthImageCallback([this](const xv::DepthColorImage & xvDepthColorImage) {
136  if(xvDepthColorImage.hostTimestamp > 0)
137  {
138  cv::Mat color = cv::Mat::zeros(cameraModel_.imageSize(), CV_8UC3);
139  color.forEach<cv::Vec3b>([&](cv::Vec3b& pixel, const int position[]) -> void {
140  const auto rgb = reinterpret_cast<std::uint8_t const *>(xvDepthColorImage.data.get() + (position[0]*cameraModel_.imageWidth()+position[1]) * 7);
141  pixel = cv::Vec3b(rgb[2], rgb[1], rgb[0]);
142  });
143  cv::Mat depth = cv::Mat::zeros(cameraModel_.imageSize(), CV_32FC1);
144  depth.forEach<float>([&](float &pixel, const int position[]) -> void {
145  pixel = *reinterpret_cast<float const *>(xvDepthColorImage.data.get() + (position[0]*cameraModel_.imageWidth()+position[1]) * 7 + 3);
146  });
147  UScopeMutex lock(dataMutex_);
148  bool notify = !lastData_.first;
149  lastData_ = std::make_pair(xvDepthColorImage.hostTimestamp, std::make_pair(color, depth));
150  if(notify)
151  dataReady_.release();
152  }
153  });
154 
155  return true;
156 #else
157  UERROR("CameraSeerSense: RTAB-Map is not built with XVisio SDK support!");
158 #endif
159  return false;
160 }
161 
162 bool CameraSeerSense::isCalibrated() const
163 {
164  return true;
165 }
166 
167 std::string CameraSeerSense::getSerial() const
168 {
169 #ifdef RTABMAP_XVSDK
170  return device_->id();
171 #endif
172  return "";
173 }
174 
175 bool CameraSeerSense::odomProvided() const
176 {
177 #ifdef RTABMAP_XVSDK
178  return computeOdometry_;
179 #else
180  return false;
181 #endif
182 }
183 
184 bool CameraSeerSense::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
185 {
186 #ifdef RTABMAP_XVSDK
187  xv::Pose xvPose;
188  if(computeOdometry_ && device_->slam()->getPoseAt(xvPose, stamp))
189  {
190  pose = this->getLocalTransform() *
191  Transform(
192  xvPose.transform().rotation()[0], xvPose.transform().rotation()[1], xvPose.transform().rotation()[2], xvPose.transform().translation()[0],
193  xvPose.transform().rotation()[3], xvPose.transform().rotation()[4], xvPose.transform().rotation()[5], xvPose.transform().translation()[1],
194  xvPose.transform().rotation()[6], xvPose.transform().rotation()[7], xvPose.transform().rotation()[8], xvPose.transform().translation()[2]) *
195  this->getLocalTransform().inverse();
196  covariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.0005;
197  return true;
198  }
199 #endif
200  return false;
201 }
202 
203 SensorData CameraSeerSense::captureImage(SensorCaptureInfo * info)
204 {
205  SensorData data;
206 #ifdef RTABMAP_XVSDK
207  if(!dataReady_.acquire(1, 3000))
208  {
209  UERROR("Did not receive frame since 3 seconds...");
210  return data;
211  }
212 
213  dataMutex_.lock();
214  data = SensorData(
215  cameraModel_.rectifyImage(lastData_.second.first, cv::INTER_CUBIC),
216  cameraModel_.rectifyImage(lastData_.second.second, cv::INTER_NEAREST),
217  cameraModel_, this->getNextSeqID(), lastData_.first);
218  lastData_ = std::pair<double, std::pair<cv::Mat, cv::Mat>>();
219  dataMutex_.unlock();
220 
221  if(!isInterIMUPublishing())
222  {
223  cv::Vec3d gyro, acc;
224  std::map<double, std::pair<cv::Vec3d, cv::Vec3d>>::const_iterator iterA, iterB;
225 
226  imuMutex_.lock();
227  while(imuBuffer_.empty() || imuBuffer_.rbegin()->first < data.stamp())
228  {
229  imuMutex_.unlock();
230  uSleep(1);
231  imuMutex_.lock();
232  }
233 
234  iterB = imuBuffer_.lower_bound(data.stamp());
235  iterA = iterB;
236  if(iterA != imuBuffer_.begin())
237  iterA = --iterA;
238  if(iterA == iterB || data.stamp() == iterB->first)
239  {
240  gyro = iterB->second.first;
241  acc = iterB->second.second;
242  }
243  else if(data.stamp() > iterA->first && data.stamp() < iterB->first)
244  {
245  float t = (data.stamp()-iterA->first) / (iterB->first-iterA->first);
246  gyro = iterA->second.first + t*(iterB->second.first - iterA->second.first);
247  acc = iterA->second.second + t*(iterB->second.second - iterA->second.second);
248  }
249  imuBuffer_.erase(imuBuffer_.begin(), iterB);
250 
251  imuMutex_.unlock();
252  data.setIMU(IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), this->getLocalTransform()));
253  }
254 
255  xv::Pose xvPose;
256  if(computeOdometry_ && device_->slam()->getPoseAt(xvPose, data.stamp()))
257  {
258  info->odomPose = this->getLocalTransform() *
259  Transform(
260  xvPose.transform().rotation()[0], xvPose.transform().rotation()[1], xvPose.transform().rotation()[2], xvPose.transform().translation()[0],
261  xvPose.transform().rotation()[3], xvPose.transform().rotation()[4], xvPose.transform().rotation()[5], xvPose.transform().translation()[1],
262  xvPose.transform().rotation()[6], xvPose.transform().rotation()[7], xvPose.transform().rotation()[8], xvPose.transform().translation()[2]) *
263  this->getLocalTransform().inverse();
264  info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.0005;
265  }
266 #else
267  UERROR("CameraSeerSense: RTAB-Map is not built with XVisio SDK support!");
268 #endif
269  return data;
270 }
271 
272 } // namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
D
MatrixXcd D
rtabmap::CameraSeerSense::~CameraSeerSense
virtual ~CameraSeerSense()
Definition: CameraSeerSense.cpp:29
this
this
CameraSeerSense.h
rtabmap::SensorCaptureInfo
Definition: SensorCaptureInfo.h:36
rtabmap::IMU
Definition: IMU.h:19
data
int data[]
UScopeMutex
Definition: UMutex.h:157
rtabmap::CameraSeerSense::available
static bool available()
Definition: CameraSeerSense.cpp:6
info
else if n * info
UASSERT
#define UASSERT(condition)
rtabmap::Camera
Definition: Camera.h:43
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
glm::uint8_t
detail::uint8 uint8_t
Definition: fwd.hpp:908
K
K
ULogger::level
static ULogger::Level level()
Definition: ULogger.h:340
rtabmap::Transform
Definition: Transform.h:41
PointMatcherSupport::Parametrizable
position
Point3 position(const NavState &X, OptionalJacobian< 3, 9 > H)
UDEBUG
#define UDEBUG(...)
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
rtabmap::CameraSeerSense::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraSeerSense.cpp:54
t
Point2 t(10, 10)
rtabmap
Definition: CameraARCore.cpp:35
UEventsManager.h
UERROR
#define UERROR(...)
rtabmap::CameraSeerSense::CameraSeerSense
CameraSeerSense(bool computeOdometry=false, float imageRate=0.0f, const Transform &localTransform=Transform::getIdentity())
Definition: CameraSeerSense.cpp:15
R
R

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:07