46 0.0f, 0.0f, 1.0f, 0.0f,
47 -1.0f, 0.0f, 0.0f, 0.0f,
48 0.0f, -1.0f, 0.0f, 0.0f);
50 0.0f, -1.0f, 0.0f, 0.0f,
51 0.0f, 0.0f, -1.0f, 0.0f,
52 1.0f, 0.0f, 0.0f, 0.0f);
56 deviceTColorCamera_(
Transform::getIdentity()),
57 spinOncePreviousStamp_(0.0),
59 stampEpochOffset_(0.0),
60 smoothing_(smoothing),
143 bool ignoreFrame =
false;
214 cv::transpose(rgb,rgbt);
218 cv::transpose(depth,deptht);
226 model.cx()>0?model.imageWidth()-model.cx():0,
227 model.localTransform()*
rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
231 std::vector<cv::KeyPoint> keypoints = data.
keypoints();
232 for(
size_t i=0; i<keypoints.size(); ++i)
234 keypoints[i].pt.x = data.
keypoints()[i].pt.y;
235 keypoints[i].pt.y = rgb.rows - data.
keypoints()[i].pt.x;
246 cv::flip(depth,depth,0);
252 model.cx()>0?model.imageWidth()-model.cx():0,
253 model.cy()>0?model.imageHeight()-model.cy():0,
258 std::vector<cv::KeyPoint> keypoints = data.
keypoints();
259 for(
size_t i=0; i<keypoints.size(); ++i)
261 keypoints[i].pt.x = rgb.cols - data.
keypoints()[i].pt.x;
262 keypoints[i].pt.y = rgb.rows - data.
keypoints()[i].pt.y;
274 cv::flip(depth,depth,1);
280 model.cy()>0?model.imageHeight()-model.cy():0,
282 model.localTransform()*
rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
286 std::vector<cv::KeyPoint> keypoints = data.
keypoints();
287 for(
size_t i=0; i<keypoints.size(); ++i)
289 keypoints[i].pt.x = rgb.cols - data.
keypoints()[i].pt.y;
290 keypoints[i].pt.y = data.
keypoints()[i].pt.x;
312 info.
reg.
covariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.0001);
320 LOGI(
"Publish odometry message (variance=%f)", firstFrame?9999:0.0001);
327 LOGW(
"Odometry lost");
void setImageSize(const cv::Size &size)
0 degree rotation (natural orientation)
void addEnvSensor(int type, float value)
virtual SensorData captureImage(CameraInfo *info=0)
ScreenRotation colorCameraToDisplayRotation_
void setRGBDImage(const cv::Mat &rgb, const cv::Mat &depth, const CameraModel &model, bool clearPreviousData=true)
cv::Mat RTABMAP_EXP fastBilateralFiltering(const cv::Mat &depth, float sigmaS=15.0f, float sigmaR=0.05f, bool earlyDivision=false)
const std::vector< cv::KeyPoint > & keypoints() const
void setGPS(const GPS &gps)
void poseReceived(const Transform &pose)
const cv::Mat & imageRaw() const
void setData(const SensorData &data, const Transform &pose)
void post(UEvent *event, bool async=true) const
virtual void mainLoopBegin()
Transform deviceTColorCamera_
CameraMobile(bool smoothing=false)
UTimer spinOnceFrameRateTimer_
UTimer cameraStartedTime_
EnvSensors lastEnvSensors_
static const float bilateralFilteringSigmaS
const cv::Mat & depthOrRightRaw() const
virtual void capturePoseOnly()
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
void setEnvSensors(const EnvSensors &sensors)
void uSleep(unsigned int ms)
bool isValidForProjection() const
const std::vector< CameraModel > & cameraModels() const
static const rtabmap::Transform opticalRotation
static const float bilateralFilteringSigmaR
double spinOncePreviousStamp_
virtual bool isCalibrated() const
ULogger class and convenient macros.
void setStamp(double stamp)
void setGroundTruth(const Transform &pose)
static const rtabmap::Transform opticalRotationInv
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
const std::vector< cv::Point3f > & keypoints3D() const
void setGPS(const GPS &gps)
const double & stamp() const