From f6cca3313a1acd6b19e862797ce3741aa834b8d7 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 16 Nov 2016 10:13:16 -0500 Subject: [PATCH] Tango: Using tango_support library to get depth to color transform (#143), poses are in device frames and camera local transform is from device to camera (including optical rotation), "720" Mode is now called "HD Mode" --- app/android/AndroidManifest.xml.in | 2 +- .../com/introlab/rtabmap/AboutDialog.class | Bin 0 -> 3188 bytes .../introlab/rtabmap/RTABMapActivity.class | Bin 0 -> 8720 bytes .../bin/com/introlab/rtabmap/RTABMapLib.class | Bin 0 -> 4341 bytes .../bin/com/introlab/rtabmap/Renderer.class | Bin 0 -> 2130 bytes .../rtabmap/TangoInitializationHelper.class | Bin 0 -> 3585 bytes app/android/jni/CameraTango.cpp | 188 ++++++++---------- app/android/jni/CameraTango.h | 8 +- app/android/jni/RTABMapApp.cpp | 6 +- app/android/jni/util.h | 25 ++- app/android/res/menu/optionmenu.xml | 2 +- .../com/introlab/rtabmap/RTABMapActivity.java | 2 +- cmake_modules/FindTango.cmake | 9 +- 13 files changed, 114 insertions(+), 128 deletions(-) create mode 100644 app/android/bin/com/introlab/rtabmap/AboutDialog.class create mode 100644 app/android/bin/com/introlab/rtabmap/RTABMapActivity.class create mode 100644 app/android/bin/com/introlab/rtabmap/RTABMapLib.class create mode 100644 app/android/bin/com/introlab/rtabmap/Renderer.class create mode 100644 app/android/bin/com/introlab/rtabmap/TangoInitializationHelper.class diff --git a/app/android/AndroidManifest.xml.in b/app/android/AndroidManifest.xml.in index fead7ee3..383be3ce 100644 --- a/app/android/AndroidManifest.xml.in +++ b/app/android/AndroidManifest.xml.in @@ -2,7 +2,7 @@ diff --git a/app/android/bin/com/introlab/rtabmap/AboutDialog.class b/app/android/bin/com/introlab/rtabmap/AboutDialog.class new file mode 100644 index 0000000000000000000000000000000000000000..be4f777b7da956ca2f98f1f3f84c1488f6dfe4ce GIT binary patch literal 3188 zcmeHJTW`}a6h7{{rOgT(gK->CQ$P0QfPLF-EuSOt`L}b&IQ*m2vn-S z-!5(wDAuLR445P^boPa8sKY&~xfE7MN!w$7@XRvHTYJpn zen%>8MHI_iS`HOLYRhI;fA+tJQQ<1daT|ZO?LUXb;z>CKOG}Gd`l!Rqaw40*h-!72 zQru-3ORj~&^|Cd(lK<9Q4@nVC;}|L{05LJUDI;jO!E zdJu)sS3J_vY_+j?R(B=1L&%RD*PF}D2BJcS49=qxromAn%_!L1Q3*YLQ-;c6u|%c^ z3f$!=M4@UL!jU52fo4K)aHgno_Kpm=hc#{N$_6|jU?lfxz$5(7FOe5dl*sW8umpb1 zg8(z!78W}J+yw$-Sb3uvtPR6~CjnS=sSBapI;xl@eN+hYh7XT8Drl%eKhMGSXUYQb3GWQ+eav9WlFd z?Gz}Fw!Ghu@`mz(?|kAMI0wG-nSX$Pgu|WL-AJ;>mXc#m&H;b0?#|4e-`qR*&h_kX z|MBm?{0#t}hCdjPB5>8_-jd^og1fY_BtqKoXnUz*7>zHe1wIg1lHj^DVHsx&!-V!VbRvhZ`7J6g$_n^`_ zuWAd%{Hk-8A)L9%F&@!m+6maIBZAO!JRBpp38cNUzbX3(jCsY7c}c@8F~1`_aO$WU zsnT84ik{1D<`Xy{&EP?ZR@i7321^o_Beti2>~&DzRXjz@z=07TXIuf<XiQWJ3L zRYs-P$v~`ca^CLMpO0Y5?a=XX*@6&U#85Lm%c?UJV0>QGox@2lHX~7+mA)qiB%PC9 zym-=a!RuQ#=^pn?y3ae|3Tt#QaVAaE@fj6SC+6L(XYr7_5)6|dkUq-v!ctNU3SK+h zCvX;3Xn=WUnCby%UGgk>uB%M!IyS@F;A5aL@6adnP$%l2z2^8%cpd9ccA-XKB#-%N z0M?bMZXr^;m4=&Ss{`n!!xSGc*(Kx7pZe+0VOBjWKqt9D4}dyVlXt0}e10e!a&mcjiZt4Lj~|fz>efC$=g_VF zg!S`ykLNz~!wu@O0oeDWTi#_tIL$$j|37L&o@|r1G*K8E{og)WL#DxydsylCKE*du z%|`at+WY#)`0y~tU>!>1PynR-=CX&G3HjnaC9BRx$7?WQNzWyfC~2n}MYaXoapy?5 z?I4Zt}yj2^?G?CtvXu8-Vy@ zW)jckG`v7y{dnF$(gw+d<3om|ANP}+VTA=97uk=AY_Vt-eZ2pL$~H7w4oQ~J;0eKp zPwJwGBwki=j7oK!`xfum+l5`6jNnb5uQ(WK$18}waN@s81g}BOmq}& zRYmWEEo4q+(41m%RsP`bYH^{KhL?uggW<9pZR%MuPhGb`?VU6@1fCtd&qtIO%$Jz3 zE+QYK+?T#w%ocU|mIjZ&>fptOsaePv9$Y!`iNLsGsqdMS)tRF+5Z&p{?843Ex0$`8 z7_vMUtj0v3xUrFJQO5dQi-k#Y5y)m2hC3!{K*IXUz}-M^Tuz&-j?3iR3W29|XIaBM zrr{L={_*&VdK+fDhdyX?+>cXUGTK$1eWuZ?_jF+-a-xd9#Xq90tWm5L6cY*@FPt6o{DF{Pw-#K zfmdn+en#LC^)0N^;G;Ny8lH{i zbFo~GLDXeoS-(4NhQS-LR-+HQD{66+PVfU z1vWH$a%}2e9NFQlAb|#fK1^<|2qAQO5W44R_wY`p_B99qd$8Yw z(6!;N2H_PA0)a2U7qtjq(jdI4K{x|n#tcZoSKzBC0p7yD3{1h-;C1*qd=tJ6--YkP T58=n~Q+N}80l$LZ!0-M8TN~>Q literal 0 HcmV?d00001 diff --git a/app/android/bin/com/introlab/rtabmap/RTABMapLib.class b/app/android/bin/com/introlab/rtabmap/RTABMapLib.class new file mode 100644 index 0000000000000000000000000000000000000000..de2bf9e1fb06388087279f25342e2729e87e4571 GIT binary patch literal 4341 zcmchZ|5F=96u{phkS|i86s*>2uU2VNYocwf3Y0bkLenOIB+!xHHkXBL=W=`Q?P2Mk z+XXzGTO&=rZr9xZQfm9c{tnitvP9wcvwXe&2%evTXAd zWGt8&%5EqnRT@6D#(v-P6c40(%-c2^^o7f`ke=-;*>ZU|ShBO3W{2DQ#OJnAP%y>{ zWtRnk?J&=i+HP@sprc?{wJic$cx|E39rpyc4!Po9ShefB!V%gILZ2&4fEQ>LI$A2bm(E#ekeQ>0q)>$ z7P^{B7wmc%XtRS-=)cWh^%WD9*-T9y#Re5+bESW@B}a>6p-)z0L~Si?7M(>l(>Mt< z@5Tt>mR7=xGe!C88Xl4f`b?Io)P+>NU!g3d;nEaLz_bMmx!SO*du}Y7s-LOhL2QNH z7FSJ%g>1oGO*+ioXG)mo{^Gdqh`@rIwR3uH*@Cq6%8E0Ms`9y!@`D3JN7C>SY9HNV z*kKk-NuPV`44a$n&@o$ZReDWqS=r#~SU9{aJr5g`x6x>>)-QtP{6QK% zvtaXlqZcY03sTbCVPSyTx@5-Dri*qp_i#Q<1zd;zrec0aI2KIgs#SAhlQ<)=9HX&S zwIG91W!B}2;Rwq`5>pt644ux#aik}MUnB(p#HH$tbRE zUyE*EyeGPaP*4%B=B6{T8c}pzi9=nM%FIa|xmdIVeqRJ4D%$C2MYK;Bj7TZea^saR zgHZJysf4b(Yo>VEQ)FB=#k9*EsW7QM_o5itt0?Ss?(2?8Qk1?0lsD0ryd!K4>0;J# zURz-FnDr($joE65*Mrqwns=DOGs2)OxE<4pF}+z$!($8F(RCKxI`3?#V9DY2gpr(< zM!=3Sjw}voylzZ@Zy6*97EEKF^^TGb4}yMORr97kkGO7Vrb8Sb^SS)lNU`uyq$xFX zqr)f9FYr(hn;z`%NWA88!2o-H8)r;YbkRY)z$;kWxLT9al(0GRDmHGmAw%VGtS6M3 z8U7_Gm|2*DpRxV=ZJL6{8-EgdNyUuRMHEjHuk`?K0vf}=9srB@YXT|c=nWiI;%O#OpTV=#lCMh5Ubi=R_HDYy)C_>_SwFpn!n!<{!YEW~KI7HJrZxT`Q7 z%Y7WfT_@Z%_%xQg9>d)t+~=o}8b8ge7ipZIi-fz8fXfrkjxs&Si;4N|_U;ny3%Et% z8|`P2a9_e#gd3h@TzvNlw+OchH#ySJhlI;Po^Ye>EfMZc0&babUnk&J2zM90A%2cN zhew1fB(S$ixMBkCG2y;Vz&#<{cL}%>;qE2i)(Ce$0aqs6g9O|<;l5A6Z4mBZ01 z9};kzg!>VGB6T*pAD$9!DFIg{+;W0`e@3_!ctrdh_QPt-{CrNhCkf_Yop7ZD+!o>1 zpiKN6<}>bo*e2XMY{YVLb8v@n71$))aPG$0+a=u71oOHhgxiW eae3J%Ts?uk7pL49Y+(w=U>lnFJ&C_|VE2En2w#H$ literal 0 HcmV?d00001 diff --git a/app/android/bin/com/introlab/rtabmap/Renderer.class b/app/android/bin/com/introlab/rtabmap/Renderer.class new file mode 100644 index 0000000000000000000000000000000000000000..942113fb1c94c59d2605b1c6e09ce5cf06dff0e2 GIT binary patch literal 2130 zcmd5-S#Q%o5S~rbI&ssxl(TfpQ4)^2MSY@(mtI720nrkP$BnZ|wz7Av^}2_|f8l2! zfdr38{3yh@Nz(+GG$0{(@OqEgZ|0k8zkUDs82}!`ss$MW^PUPEA&pi(t2)}SYQRG0 zHJ3H6xwarnVDf++GRJ4K;k>LKaL*8!2wrK`&^(GZ1@lz{h1}kqbpj(#g%su)fuUll zLLj@LYTSZRynPP1rM1#I7$cdlZzSR=|B%-x+7t?(8KEQ%wW|6&h@Mb8zu)9k1fkM~ zGFj6~peU148CvD^?Dn6CB?cW|g!OBoauE8$6NW}{$TgZTBco%_C{0@)E0(B;XpBy+ z3yB2lc!F~l`=(oZneN7sNls7-DN=si2JV8i^E>Wdtn19<6~T{Etfod$M$Jjc?R;-k z{+aNOySnl}D<8nVtsQqm$+~Ft=V5~l69uqflE9^+d!FihMhn?kPfB6J#rVF6gDThi z46~cSw5vSkSC|&by1kh-O%V}TaQoIMR@zj=O>dwH%oKmF$WkQ-R|vctpwN23`V;=G z6eBRI|zqwQqNUs&P<|ewynzTB;%>)?hpw2>pX3r7c0$oaQnxcfW4<;?eT4qWbfG7 zd$F9PJLY{^CJjqOhD-BYaLu&WJh7mJ0q0xR#1=Jq2MY6=5E3b|GXtHY}tdIH?GW paH|U;@l%V%?G%Ky6oetTi+3`x1o!X{u!3I!JzRwQ@DP?^=m*efTCV^A literal 0 HcmV?d00001 diff --git a/app/android/bin/com/introlab/rtabmap/TangoInitializationHelper.class b/app/android/bin/com/introlab/rtabmap/TangoInitializationHelper.class new file mode 100644 index 0000000000000000000000000000000000000000..8b86103dbd87f7c2df1e78f0e8fc95a79771d946 GIT binary patch literal 3585 zcmeHK>rWFw5TB(;+XLjGAo#%bT~J#@LW~%b$|E)&l9WQ!FJyb&!s2ze?(GrqA2ZQJ zzxzjtv)4l|p`@7j3Hh>jzuDQ@nb~>l{`~dzI{?hUOd7ff$&>Ndd9_S{lnC@M zC>a$%HQQRie707KQf#)wW|8O5j>|(wzAEs*>7@c1>~&@@+}uok4dQvt#W!OBu`6* zihv4P6B@nb@8eXRYc4(9p{`W}@uv{yYBcZpEC^^__&jL4%bL+bZvFFJM5C#+p-k4a z5@_}@BU;Lw9k%G)glcPPsnPU>Ckzcj40VhFj9?vlMrqnQi1vX~);@8Qa0PNzmu7R8y9UE_LJcpRA}#t@qDFrVko&QUoWhUB(3HVBjfOT-7d9P=)=#@J1W?PWGfF>oUM95Yz>) zxk7%7sznoI!>;m} zzY*t3Ozbw>A|NpB{#}cn#|B-+o)h)(xOxz{nRDY*op+Cwl=;aI8Msf#&?!1#r-ZlN z@$6JB0;9fSu`jn7wrW>YHPidiouZV1DU!;b!fWTn@4Un&u7+o{UEno+O@-Rywnr0~ zIPn>gw{?LUo}^)#z|gVEgwKN&5d?VktzcaV;1)g(?ByPB@o)ip5T)&F05XWP z3G5*5P2fJn{Rw;#@jwC(A|6WMVZ@gb_%iN_1sz6lyn;K +#include namespace rtabmap { @@ -80,7 +81,7 @@ void onPoseAvailableRouter(void* context, const TangoPoseData* pose) if(pose->status_code == TANGO_POSE_VALID) { CameraTango* app = static_cast(context); - app->poseReceived(app->tangoPoseToTransform(pose, true)); + app->poseReceived(app->tangoPoseToTransform(pose)); } } @@ -90,18 +91,11 @@ void onTangoEventAvailableRouter(void* context, const TangoEvent* event) app->tangoEventReceived(event->type, event->event_key, event->event_value); } -// In OpenGL, axes are x->right, y->up and z->outScreen -// Image is x->right, y->down and z->inScreen -static rtabmap::Transform opticalRotation( - 1.0f, 0.0f, 0.0f, 0.0f, - 0.0f, -1.0f, 0.0f, 0.0f, - 0.0f, 0.0f, -1.0f, 0.0f); - ////////////////////////////// // CameraTango ////////////////////////////// CameraTango::CameraTango(int decimation, bool autoExposure) : - Camera(0, opticalRotation), + Camera(0), tango_config_(0), firstFrame_(true), decimation_(decimation), @@ -122,6 +116,8 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string { close(); + TangoSupport_initializeLibrary(); + // Connect to Tango LOGI("NativeRTABMap: Setup tango config"); tango_config_ = TangoService_getConfig(TANGO_CONFIG_DEFAULT); @@ -269,34 +265,16 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string // as well. We use timestamp 0.0 and the target frame pair to get the // extrinsics from the sensors. // - // Get device with respect to imu transformation matrix. - frame_pair.base = TANGO_COORDINATE_FRAME_IMU; - frame_pair.target = TANGO_COORDINATE_FRAME_DEVICE; - ret = TangoService_getPoseAtTime(0.0, frame_pair, &pose_data); - if (ret != TANGO_SUCCESS) - { - LOGE("NativeRTABMap: Failed to get transform between the IMU frame and device frames"); - return false; - } - imuTDevice_ = rtabmap::Transform( - pose_data.translation[0], - pose_data.translation[1], - pose_data.translation[2], - pose_data.orientation[0], - pose_data.orientation[1], - pose_data.orientation[2], - pose_data.orientation[3]); - - // Get color camera with respect to imu transformation matrix. - frame_pair.base = TANGO_COORDINATE_FRAME_IMU; - frame_pair.target = TANGO_COORDINATE_FRAME_CAMERA_DEPTH; + // Get color camera with respect to device transformation matrix. + frame_pair.base = TANGO_COORDINATE_FRAME_DEVICE; + frame_pair.target = TANGO_COORDINATE_FRAME_CAMERA_COLOR; ret = TangoService_getPoseAtTime(0.0, frame_pair, &pose_data); if (ret != TANGO_SUCCESS) { LOGE("NativeRTABMap: Failed to get transform between the color camera frame and device frames"); return false; } - imuTDepthCamera_ = rtabmap::Transform( + deviceTColorCamera_ = rtabmap::Transform( pose_data.translation[0], pose_data.translation[1], pose_data.translation[2], @@ -305,8 +283,6 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string pose_data.orientation[2], pose_data.orientation[3]); - deviceTDepth_ = imuTDevice_.inverse() * imuTDepthCamera_; - // camera intrinsic TangoCameraIntrinsics color_camera_intrinsics; ret = TangoService_getCameraIntrinsics(TANGO_CAMERA_COLOR, &color_camera_intrinsics); @@ -323,11 +299,11 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string this->getLocalTransform()); model_.setImageSize(cv::Size(color_camera_intrinsics.width, color_camera_intrinsics.height)); - // optical rotation - model_.setLocalTransform(Transform( - 0.0f, 0.0f, 1.0f, 0.0f, - -1.0f, 0.0f, 0.0f, 0.0f, - 0.0f, -1.0f, 0.0f, 0.0f)); + // device to camera optical rotation in rtabmap frame + model_.setLocalTransform(tango_device_T_rtabmap_device.inverse()*deviceTColorCamera_); + + LOGI("deviceTColorCameraTango =%s", deviceTColorCamera_.prettyPrint().c_str()); + LOGI("deviceTColorCameraRtabmap=%s", (tango_device_T_rtabmap_device.inverse()*deviceTColorCamera_).prettyPrint().c_str()); cameraStartedTime_.restart(); @@ -389,11 +365,16 @@ void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double times } } +static rtabmap::Transform opticalRotationTango( + 1.0f, 0.0f, 0.0f, 0.0f, + 0.0f, -1.0f, 0.0f, 0.0f, + 0.0f, 0.0f, -1.0f, 0.0f); void CameraTango::poseReceived(const Transform & pose) { if(!pose.isNull() && pose.getNormSquared() < 100000) { - this->post(new PoseEvent(pose)); + // send pose of the camera (without optical rotation), not the device + this->post(new PoseEvent(pose*deviceTColorCamera_*opticalRotationTango)); } } @@ -412,69 +393,52 @@ std::string CameraTango::getSerial() const return "Tango"; } -rtabmap::Transform CameraTango::tangoPoseToTransform(const TangoPoseData * tangoPose, bool inOpenGLFrame) const +rtabmap::Transform CameraTango::tangoPoseToTransform(const TangoPoseData * tangoPose) const { UASSERT(tangoPose); rtabmap::Transform pose; - if(!deviceTDepth_.isNull()) - { - pose = rtabmap::Transform( - tangoPose->translation[0], - tangoPose->translation[1], - tangoPose->translation[2], - tangoPose->orientation[0], - tangoPose->orientation[1], - tangoPose->orientation[2], - tangoPose->orientation[3]); - // transform in OpenGL + extrinsics - // opengl_world_T_opengl_camera = - // opengl_world_T_start_service * - // start_service_T_device * - // device_T_imu * - // imu_T_depth_camera * - // depth_camera_T_opengl_camera; - if(inOpenGLFrame) - { - pose = opengl_world_T_tango_world * pose * deviceTDepth_ * depth_camera_T_opengl_camera; - } - } + pose = rtabmap::Transform( + tangoPose->translation[0], + tangoPose->translation[1], + tangoPose->translation[2], + tangoPose->orientation[0], + tangoPose->orientation[1], + tangoPose->orientation[2], + tangoPose->orientation[3]); return pose; } -rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp, bool inOpenGLFrame) +rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp) { rtabmap::Transform pose; - if(!deviceTDepth_.isNull()) + + TangoPoseData pose_start_service_T_device; + TangoCoordinateFramePair frame_pair; + frame_pair.base = TANGO_COORDINATE_FRAME_START_OF_SERVICE; + frame_pair.target = TANGO_COORDINATE_FRAME_DEVICE; + TangoErrorType status = TangoService_getPoseAtTime(timestamp, frame_pair, &pose_start_service_T_device); + if (status != TANGO_SUCCESS) { - TangoPoseData pose_start_service_T_device; - TangoCoordinateFramePair frame_pair; - frame_pair.base = TANGO_COORDINATE_FRAME_START_OF_SERVICE; - frame_pair.target = TANGO_COORDINATE_FRAME_DEVICE; - TangoErrorType status = TangoService_getPoseAtTime(timestamp, frame_pair, &pose_start_service_T_device); - if (status != TANGO_SUCCESS) - { - LOGE( - "PoseData: Failed to get transform between the Start of service and " - "device frames at timestamp %lf", - timestamp); - } - if (pose_start_service_T_device.status_code != TANGO_POSE_VALID) - { - LOGW( - "PoseData: Failed to get transform between the Start of service and " - "device frames at timestamp %lf", - timestamp); - } - else - { - - pose = tangoPoseToTransform(&pose_start_service_T_device, inOpenGLFrame); - } - - + LOGE( + "PoseData: Failed to get transform between the Start of service and " + "device frames at timestamp %lf", + timestamp); } + if (pose_start_service_T_device.status_code != TANGO_POSE_VALID) + { + LOGW( + "PoseData: Failed to get transform between the Start of service and " + "device frames at timestamp %lf", + timestamp); + } + else + { + + pose = tangoPoseToTransform(&pose_start_service_T_device); + } + return pose; } @@ -558,26 +522,37 @@ SensorData CameraTango::captureImage(CameraInfo * info) // Querying the depth image's frame transformation based on the depth image's // timestamp. cv::Mat depth; - Transform poseDepth = getPoseAtTimestamp(cloudStamp, false); - Transform poseColor = getPoseAtTimestamp(rgbStamp, false); - if(poseColor.getNormSquared() > 100000) + // Calculate the relative pose from color camera frame at timestamp + // color_timestamp t1 and depth + // camera frame at depth_timestamp t0. + Transform colorToDepth; + TangoPoseData pose_color_image_t1_T_depth_image_t0; + if (TangoSupport_calculateRelativePose( + rgbStamp, TANGO_COORDINATE_FRAME_CAMERA_COLOR, cloudStamp, + TANGO_COORDINATE_FRAME_CAMERA_DEPTH, + &pose_color_image_t1_T_depth_image_t0) == TANGO_SUCCESS) { - LOGE("Very large odometry color pose detected (%s)! Ignoring this frame!", poseColor.prettyPrint().c_str()); - poseColor.setNull(); + colorToDepth = tangoPoseToTransform(&pose_color_image_t1_T_depth_image_t0); } - if(poseDepth.getNormSquared() > 100000) + else { - LOGE("Very large odometry depth pose detected (%s)! Ignoring this frame!", poseDepth.prettyPrint().c_str()); - poseDepth.setNull(); + LOGE( + "SynchronizationApplication: Could not find a valid relative pose at " + "time for color and " + " depth cameras."); } + if(colorToDepth.getNormSquared() > 100000) + { + LOGE("Very large color to depth error detected (%s)! Ignoring this frame!", colorToDepth.prettyPrint().c_str()); + colorToDepth.setNull(); + } cv::Mat scan; - if(!poseDepth.isNull() && !poseColor.isNull()) + if(!colorToDepth.isNull()) { // The Color Camera frame at timestamp t0 with respect to Depth // Camera frame at timestamp t1. - Transform colorToDepth = deviceTDepth_.inverse() * poseColor.inverse() * poseDepth * deviceTDepth_; LOGI("colorToDepth=%s", colorToDepth.prettyPrint().c_str()); int pixelsSet = 0; @@ -622,17 +597,24 @@ SensorData CameraTango::captureImage(CameraInfo * info) } else { - LOGE("Poses are null?!? color=%d (stamp=%f) depth=%d (stamp=%f)", poseColor.isNull()?0:1, rgbStamp, poseDepth.isNull()?0:1, cloudStamp); + LOGE("color to depth pose is null?!? (rgb stamp=%f) (depth stamp=%f)", rgbStamp, cloudStamp); } if(!rgb.empty() && !depth.empty()) { depth = rtabmap::util2d::fillDepthHoles(depth, holeSize, maxDepthError); - Transform poseColorOpenGL = getPoseAtTimestamp(rgbStamp, true); + Transform poseDevice = getPoseAtTimestamp(rgbStamp); + + LOGD("Local = %s", model.localTransform().prettyPrint().c_str()); + LOGD("tango = %s", poseDevice.prettyPrint().c_str()); + LOGD("opengl(t)= %s", (opengl_world_T_tango_world * poseDevice).prettyPrint().c_str()); //Rotate in RTAB-Map's coordinate - Transform odom = rtabmap_world_T_opengl_world * poseColorOpenGL * depth_camera_T_opengl_camera * model.localTransform().inverse(); + Transform odom = rtabmap_world_T_tango_world * poseDevice * tango_device_T_rtabmap_device; + + LOGD("rtabmap = %s", odom.prettyPrint().c_str()); + LOGD("opengl(r)= %s", (opengl_world_T_rtabmap_world * odom * rtabmap_device_T_opengl_device).prettyPrint().c_str()); data = SensorData(scan, LaserScanInfo(cloud.total()/scanDownsampling, 0, model.localTransform()), rgb, depth, model, this->getNextSeqID(), rgbStamp); data.setGroundTruth(odom); diff --git a/app/android/jni/CameraTango.h b/app/android/jni/CameraTango.h index 92b79715..e1ebf146 100644 --- a/app/android/jni/CameraTango.h +++ b/app/android/jni/CameraTango.h @@ -77,7 +77,7 @@ public: void close(); // close Tango connection virtual bool isCalibrated() const; virtual std::string getSerial() const; - rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose, bool inOpenGLFrame) const; + rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const; void setDecimation(int value) {decimation_ = value;} void setAutoExposure(bool enabled) {autoExposure_ = enabled;} @@ -90,7 +90,7 @@ protected: virtual SensorData captureImage(CameraInfo * info = 0); private: - rtabmap::Transform getPoseAtTimestamp(double timestamp, bool inOpenGLFrame); + rtabmap::Transform getPoseAtTimestamp(double timestamp); virtual void mainLoopBegin(); virtual void mainLoop(); @@ -108,10 +108,8 @@ private: double tangoColorStamp_; boost::mutex dataMutex_; USemaphore dataReady_; - rtabmap::Transform imuTDevice_; - rtabmap::Transform imuTDepthCamera_; - rtabmap::Transform deviceTDepth_; CameraModel model_; + Transform deviceTColorCamera_; }; } /* namespace rtabmap */ diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index de24e882..713e0670 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/app/android/jni/RTABMapApp.cpp @@ -89,7 +89,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters() parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), std::string("true"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), std::string("15"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisEstimationType(), std::string("0"))); // 0=3D-3D 1=PnP - parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeMaxError(), std::string("0.05"))); + parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeMaxError(), std::string("0.1"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityPathMaxNeighbors(), std::string("0"))); // disable scan matching to merged nodes parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityBySpace(), std::string("false"))); // just keep loop closure detection @@ -413,7 +413,7 @@ int RTABMapApp::Render() { cv::Mat compressed = iter->second.texture; iter->second.texture = rtabmap::uncompressImage(iter->second.texture); - main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world*iter->second.pose); + main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world*iter->second.pose*rtabmap_device_T_opengl_device); main_scene_.setCloudVisible(iter->first, iter->second.visible); iter->second.texture = compressed; } @@ -433,7 +433,7 @@ int RTABMapApp::Render() if(!pose.isNull()) { // update camera pose? - main_scene_.SetCameraPose(pose); + main_scene_.SetCameraPose(opengl_world_T_tango_world*pose); if(!camera_->isRunning() && cameraJustInitialized_) { notifyDataLoaded = true; diff --git a/app/android/jni/util.h b/app/android/jni/util.h index 8dad6ce1..055bf241 100644 --- a/app/android/jni/util.h +++ b/app/android/jni/util.h @@ -77,21 +77,26 @@ protected: }; static const rtabmap::Transform opengl_world_T_tango_world( - 1.0f, 0.0f, 0.0f, 0.0f, - 0.0f, 0.0f, 1.0f, 0.0f, + 1.0f, 0.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f); -static const rtabmap::Transform depth_camera_T_opengl_camera( - 1.0f, 0.0f, 0.0f, 0.0f, - 0.0f, -1.0f, 0.0f, 0.0f, - 0.0f, 0.0f, -1.0f, 0.0f); +static const rtabmap::Transform rtabmap_world_T_tango_world( + 0.0f, 1.0f, 0.0f, 0.0f, + -1.0f, 0.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 1.0f, 0.0f); + +static const rtabmap::Transform tango_device_T_rtabmap_device( + 0.0f, -1.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 1.0f, 0.0f, + -1.0f, 0.0f, 0.0f, 0.0f); static const rtabmap::Transform opengl_world_T_rtabmap_world( - 0.0f, -1.0f, 0.0f, 0.0f, - 0.0f, 0.0f, 1.0f, 0.0f, - -1.0f, 0.0f, 0.0f, 0.0f); + 0.0f, -1.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 1.0f, 0.0f, + -1.0f, 0.0f, 0.0f, 0.0f); -static const rtabmap::Transform rtabmap_world_T_opengl_world( +static const rtabmap::Transform rtabmap_device_T_opengl_device( 0.0f, 0.0f, -1.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f); diff --git a/app/android/res/menu/optionmenu.xml b/app/android/res/menu/optionmenu.xml index 8193274e..2a40a3c7 100644 --- a/app/android/res/menu/optionmenu.xml +++ b/app/android/res/menu/optionmenu.xml @@ -67,7 +67,7 @@ - + diff --git a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java index 01d30ac1..715b92ce 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java +++ b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java @@ -98,7 +98,7 @@ public class RTABMapActivity extends Activity implements OnClickListener { private int mParamTimeThrMsIndex = 4; private int mParamMaxFeaturesIndex = 2; private int mParamLoopThrMsIndex = 1; - private int mParamOptimizeErrorIndex = 3; + private int mParamOptimizeErrorIndex = 4; final String[] mUpdateRateValues = {"0.5", "1", "2", "Max"}; final String[] mTimeThrValues = {"400", "500", "600", "700", "800", "900", "1000", "1100", "1200", "1300", "1400", "1500", "No Limit"}; diff --git a/cmake_modules/FindTango.cmake b/cmake_modules/FindTango.cmake index 4a30ad24..2f6b211a 100644 --- a/cmake_modules/FindTango.cmake +++ b/cmake_modules/FindTango.cmake @@ -9,12 +9,13 @@ FIND_PATH(Tango_INCLUDE_DIR tango_client_api.h) FIND_LIBRARY(Tango_LIBRARY NAMES tango_client_api) +FIND_LIBRARY(Tango_support_LIBRARY NAMES tango_support_api) -IF (Tango_INCLUDE_DIR AND Tango_LIBRARY) +IF (Tango_INCLUDE_DIR AND Tango_LIBRARY AND Tango_support_LIBRARY) SET(Tango_FOUND TRUE) SET(Tango_INCLUDE_DIRS ${Tango_INCLUDE_DIR}) - SET(Tango_LIBRARIES ${Tango_LIBRARY}) -ENDIF (Tango_INCLUDE_DIR AND Tango_LIBRARY) + SET(Tango_LIBRARIES ${Tango_LIBRARY} ${Tango_support_LIBRARY}) +ENDIF (Tango_INCLUDE_DIR AND Tango_LIBRARY AND Tango_support_LIBRARY) IF (Tango_FOUND) # show which Tango was found only if not quiet @@ -24,7 +25,7 @@ IF (Tango_FOUND) ELSE (Tango_FOUND) # fatal error if Tango is required but not found IF (Tango_FIND_REQUIRED) - MESSAGE(FATAL_ERROR "Could not find Tango") + MESSAGE(FATAL_ERROR "Could not find Tango (client and/or support libraries)") ENDIF (Tango_FIND_REQUIRED) ENDIF (Tango_FOUND)