From 6c07819f60637ea5c12543b6c58c5808cc9b81c4 Mon Sep 17 00:00:00 2001 From: Maurice Fallon Date: Mon, 24 Aug 2015 14:45:38 +0100 Subject: [PATCH] make app that uses bot frames --- CMakeLists.txt | 8 +- src/icpcuda-app/CMakeLists.txt | 66 ----- src/icpcuda-app/icpcuda-app-simple.cpp | 386 +++++++++++++++++++++++++ src/icpcuda-app/icpcuda-app.cpp | 147 +++++++--- 4 files changed, 494 insertions(+), 113 deletions(-) delete mode 100644 src/icpcuda-app/CMakeLists.txt create mode 100644 src/icpcuda-app/icpcuda-app-simple.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index f208efd..7416d0b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -83,10 +83,12 @@ add_executable(icpcuda-app-original src/icpcuda-app/icpcuda-app-original.cpp ${s target_link_libraries(icpcuda-app-original ${Boost_LIBRARIES} ${OpenCV_LIBS} ${Eigen_LIBRARIES} ${CUDA_LIBRARIES}) pods_install_executables(icpcuda-app-original) +add_executable(se-icpcuda-simple src/icpcuda-app/icpcuda-app-simple.cpp ${srcs} ${cuda} ${cuda_objs} ${containers}) +target_link_libraries(se-icpcuda-simple ${Boost_LIBRARIES} ${OpenCV_LIBS} ${Eigen_LIBRARIES} ${CUDA_LIBRARIES} z) +pods_use_pkg_config_packages(se-icpcuda-simple eigen3 lcm lcmtypes_kinect lcmtypes_bot2-core) +pods_install_executables(se-icpcuda-simple) add_executable(se-icpcuda src/icpcuda-app/icpcuda-app.cpp ${srcs} ${cuda} ${cuda_objs} ${containers}) target_link_libraries(se-icpcuda ${Boost_LIBRARIES} ${OpenCV_LIBS} ${Eigen_LIBRARIES} ${CUDA_LIBRARIES} z) -pods_use_pkg_config_packages(se-icpcuda eigen3 - lcm lcmtypes_kinect - lcmtypes_bot2-core) +pods_use_pkg_config_packages(se-icpcuda eigen3 lcm lcmtypes_kinect lcmtypes_bot2-core bot2-frames) pods_install_executables(se-icpcuda) diff --git a/src/icpcuda-app/CMakeLists.txt b/src/icpcuda-app/CMakeLists.txt deleted file mode 100644 index 9b6ef9b..0000000 --- a/src/icpcuda-app/CMakeLists.txt +++ /dev/null @@ -1,66 +0,0 @@ -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -Wno-deprecated-declarations") - - -add_definitions(-Wall ) -#add_definitions(-Wall -std=gnu99) # .... compiler warning - -############################################## -# Basic Test Program -#add_executable (se-icpcuda icpcuda-app.cpp) -#target_link_libraries(se-icpcuda boost_system) -#pods_use_pkg_config_packages(se-icpcuda icpcuda -# lcm -# lcmtypes_bot2-core) -#pods_install_executables(se-icpcuda) - -find_package(OpenCV REQUIRED) -find_package(Boost COMPONENTS thread filesystem system REQUIRED) -find_package(CUDA REQUIRED) -find_package(ZLIB REQUIRED) - -#remove this as soon as eigen is shipped with FindEigen.cmake - get_filename_component(EIGEN_ROOT "/usr/include/eigen3" PATH) - if(PKG_CONFIG_FOUND) - pkg_check_modules(PC_EIGEN eigen3) - endif(PKG_CONFIG_FOUND) - find_path(EIGEN_INCLUDE_DIRS Eigen/Core - HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} - "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" - PATHS "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" - "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" - PATH_SUFFIXES eigen3 include/eigen3 include) - find_package_handle_standard_args(eigen DEFAULT_MSG EIGEN_INCLUDE_DIRS) - set(EIGEN_DEFINITIONS ${EIGEN_DEFINITIONS} -DEIGEN_USE_NEW_STDVECTOR - -DEIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET) - -include_directories(${OpenCV_INCLUDE_DIRS}) -include_directories(${Boost_INCLUDE_DIR}) -include_directories(${CUDA_INCLUDE_DIRS}) -include_directories(${EIGEN_INCLUDE_DIRS}) - - -set(CUDA_ARCH_BIN "35" CACHE STRING "Specify 'real' GPU arch to build binaries for, BIN(PTX) format is supported. Example: 1.3 2.1(1.3) or 13 21(13)") -set(CUDA_ARCH_PTX "" CACHE STRING "Specify 'virtual' PTX arch to build PTX intermediate code for. Example: 1.0 1.2 or 10 12") - -SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}) -include(../../cmake/CudaComputeTargetFlags.cmake) -APPEND_TARGET_ARCH_FLAGS() - -set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "-Xcompiler;-fPIC;") -set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "--ftz=true;--prec-div=false;--prec-sqrt=false") - -CUDA_COMPILE(cuda_objs ${cuda}) - - -add_executable (se-icpcuda-original icpcuda-app-original.cpp) -target_link_libraries(se-icpcuda-original boost_system ${OpenCV_LIBS} ${CUDA_LIBRARIES} /usr/lib/x86_64-linux-gnu/libcuda.so - /usr/lib/x86_64-linux-gnu/libcudart.so /usr/lib/x86_64-linux-gnu/libicudata.so ${ZLIB_LIBRARIES} - /home/mfallon/otherprojects/pronto-distro/build/lib/libicpcuda.so) -pods_use_pkg_config_packages(se-icpcuda-original - lcm - lcmtypes_bot2-core - ) -pods_install_executables(se-icpcuda-original) - - - diff --git a/src/icpcuda-app/icpcuda-app-simple.cpp b/src/icpcuda-app/icpcuda-app-simple.cpp new file mode 100644 index 0000000..31b94bf --- /dev/null +++ b/src/icpcuda-app/icpcuda-app-simple.cpp @@ -0,0 +1,386 @@ +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +using namespace std; + +std::ifstream asFile; + +struct CommandLineConfig +{ + bool verbose; + bool process_incoming; + bool slow_mode; + bool init_rotated; +}; + +class App{ + public: + App(boost::shared_ptr &lcm_, const CommandLineConfig& cl_cfg_); + + ~App(){ + } + + private: + const CommandLineConfig cl_cfg_; + boost::shared_ptr lcm_; + //void imagesHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const bot_core::images_t* msg); + void kinectHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const kinect::frame_msg_t* msg); + + uint64_t loadDepth(cv::Mat1w & depth); + uint64_t loadDepthNew(cv::Mat1w & depth); + + bool init_; + std::string directory_; + + + cv::Mat1w prevImage; + cv::Mat1w currImage; + ICPOdometry* icpOdom; + ICPSlowdometry* icpSlowdom; + Eigen::Matrix4d currPose, prevPose; + int64_t prevUtime, currUtime; + + + void writeRawFile(cv::Mat1w & depth); + void prefilterData(cv::Mat1w & depth); + void writePngFile(cv::Mat1w & depth); + + int output_counter_; + + +}; + +App::App(boost::shared_ptr &lcm_, const CommandLineConfig& cl_cfg_) : + lcm_(lcm_), cl_cfg_(cl_cfg_){ + //lcm_->subscribe("CAMERA",&App::imagesHandler,this); + lcm_->subscribe("KINECT_FRAME",&App::kinectHandler,this); + + cudaDeviceProp prop; + cudaGetDeviceProperties(&prop, 0); + std::string dev(prop.name); + std::cout << dev << std::endl; + + if (!cl_cfg_.process_incoming){ + directory_ = "/home/mfallon/logs/kinect/rgbd_dataset_freiburg1_desk/"; + + std::string associationFile = directory_; + associationFile.append("association.txt"); + asFile.open(associationFile.c_str()); + assert(!asFile.eof() && asFile.is_open()); + } + + icpOdom = new ICPOdometry(640, 480, 320, 240, 528, 528); + icpSlowdom = new ICPSlowdometry(640, 480, 320, 240, 528, 528); + + currImage = cv::Mat1w (480, 640); + currPose = Eigen::Matrix4d::Identity(); + if (cl_cfg_.init_rotated){ + currPose.topLeftCorner(3, 3) << 0, 0, 1, -1, 0, 0, 0, -1, 0; + } + currUtime = 0; + + prevImage = cv::Mat1w(480, 640); + prevPose = currPose; + prevUtime = currUtime; + + init_ = false; + output_counter_=0; +} + +void quat_to_euler(Eigen::Quaterniond q, double& roll, double& pitch, double& yaw) { + const double q0 = q.w(); + const double q1 = q.x(); + const double q2 = q.y(); + const double q3 = q.z(); + roll = atan2(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2)); + pitch = asin(2*(q0*q2-q3*q1)); + yaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3)); +} + + +// Difference the transform and scale by elapsed time: +void getTransAsVelocityTrans(Eigen::Matrix4d currPose, int64_t currUtime, Eigen::Matrix4d prevPose, int64_t prevUtime, Eigen::Vector3d &linRate, Eigen::Vector3d &rotRate){ + double elapsed_time = (currUtime - prevUtime)*1E-6; + Eigen::Isometry3d currPoseIso(currPose); + Eigen::Isometry3d prevPoseIso(prevPose); + Eigen::Isometry3d deltaPoseIso = prevPoseIso.inverse()*currPoseIso; + linRate = deltaPoseIso.translation()/elapsed_time; + Eigen::Quaterniond deltaRotQuat = Eigen::Quaterniond(deltaPoseIso.rotation()); + Eigen::Vector3d deltaRot; + quat_to_euler(deltaRotQuat, deltaRot(0), deltaRot(1), deltaRot(2)); + rotRate = deltaRot/elapsed_time; + + if (1==0){ + std::stringstream ss; + std::cout << "Elapsed Time: " << elapsed_time << " sec\n"; + std::cout << "RPY: " << deltaRot.transpose() <<" rad\n"; + std::cout << "RPY: " << deltaRot.transpose()*180/M_PI <<" deg\n"; + std::cout << "RPY: " << rotRate.transpose() <<" rad/s | velocity scaled\n"; + std::cout << "RPY: " << rotRate.transpose()*180/M_PI <<" deg/s | velocity scaled\n"; + std::cout << "XYZ: " << linRate.transpose() << " m/s\n"; + } +} + +bot_core::pose_t getPoseAsBotPose(Eigen::Matrix4d currPose, int64_t currUtime, Eigen::Matrix4d prevPose, int64_t prevUtime){ + Eigen::Vector3d trans_out = currPose.topRightCorner(3, 1); + Eigen::Matrix3d rot_out = currPose.topLeftCorner(3, 3); + Eigen::Quaterniond r_x(rot_out); + + bot_core::pose_t pose_msg; + pose_msg.utime = currUtime; + pose_msg.pos[0] = trans_out[0]; + pose_msg.pos[1] = trans_out[1]; + pose_msg.pos[2] = trans_out[2]; + pose_msg.orientation[0] = r_x.w(); + pose_msg.orientation[1] = r_x.x(); + pose_msg.orientation[2] = r_x.y(); + pose_msg.orientation[3] = r_x.z(); + + Eigen::Vector3d linRate, rotRate; + getTransAsVelocityTrans(currPose, currUtime, prevPose, prevUtime, linRate, rotRate); + pose_msg.vel[0] = linRate[0]; + pose_msg.vel[1] = linRate[1]; + pose_msg.vel[2] = linRate[2]; + pose_msg.rotation_rate[0] = rotRate[0]; + pose_msg.rotation_rate[1] = rotRate[1]; + pose_msg.rotation_rate[2] = rotRate[2]; + + return pose_msg; +} + + +void tokenize(const std::string & str, std::vector & tokens, std::string delimiters = " "){ + tokens.clear(); + + std::string::size_type lastPos = str.find_first_not_of(delimiters, 0); + std::string::size_type pos = str.find_first_of(delimiters, lastPos); + + while (std::string::npos != pos || std::string::npos != lastPos){ + tokens.push_back(str.substr(lastPos, pos - lastPos)); + lastPos = str.find_first_not_of(delimiters, pos); + pos = str.find_first_of(delimiters, lastPos); + } +} + +// Freiburg Associations file: +uint64_t App::loadDepth(cv::Mat1w & depth){ + std::string currentLine; + std::vector tokens; + std::vector timeTokens; + + getline(asFile, currentLine); + tokenize(currentLine, tokens); + + std::cout << currentLine << "\n"; + + if(tokens.size() == 0) + return 0; + + std::string depthLoc = directory_; + depthLoc.append(tokens[3]); + depth = cv::imread(depthLoc, CV_LOAD_IMAGE_ANYDEPTH); + std::cout << depthLoc << "\n"; + tokenize(tokens[0], timeTokens, "."); + std::string timeString = timeTokens[0]; + timeString.append(timeTokens[1]); + uint64_t time; + std::istringstream(timeString) >> time; + + for(unsigned int i = 0; i < 480; i++){ + for(unsigned int j = 0; j < 640; j++){ + depth.at(i, j) /= 5; + } + } + + return time; +} + +// Sequence of incrementing png files +uint64_t App::loadDepthNew(cv::Mat1w & depth){ + std::stringstream depthLoc; + depthLoc << "./png/" << output_counter_ << ".png"; + depth = cv::imread(depthLoc.str(), CV_LOAD_IMAGE_ANYDEPTH); + std::cout << depthLoc.str() << "\n"; + uint64_t time = 0; + + for(unsigned int i = 0; i < 480; i++){ + for(unsigned int j = 0; j < 640; j++){ + depth.at(i, j) /= 5; + } + } + + return time; +} + + +void App::writePngFile(cv::Mat1w & depth){ + for(unsigned int i = 0; i < 480; i++){ + for(unsigned int j = 0; j < 640; j++){ + depth.at(i, j) *= 5; + } + } + + std::stringstream ss; + ss << output_counter_ << ".png"; + vector compression_params; + compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION); + compression_params.push_back(9); + + try { + imwrite(ss.str(), depth, compression_params); + } + catch (runtime_error& ex) { + fprintf(stderr, "Exception converting image to PNG format: %s\n", ex.what()); + return; + } +} + +void App::prefilterData(cv::Mat1w & depth){ + for(unsigned int i = 0; i < 480; i++){ + for(unsigned int j = 0; j < 640; j++){ + if (depth.at(i, j) > 4000){ + //std::cout << depth.at(i, j) << " " << i << " " << j << "\n"; + depth.at(i, j) = 0; + } + } + } +} + +void App::writeRawFile(cv::Mat1w & depth){ + std::stringstream ss; + ss << output_counter_ << ".txt"; + ofstream myfile (ss.str().c_str()); + if (myfile.is_open()){ + for(unsigned int i = 0; i < 480; i++){ + for(unsigned int j = 0; j < 640; j++){ + if (j>0) + myfile << ", "; + myfile << depth.at(i, j); + } + myfile << "\n"; + } + myfile.close(); + + } + else cout << "Unable to open file"; +} + +//void App::imagesHandler(const lcm::ReceiveBuffer* rbuf, +// const std::string& channel, const bot_core::images_t* msg){ +//} + + +void App::kinectHandler(const lcm::ReceiveBuffer* rbuf, + const std::string& channel, const kinect::frame_msg_t* msg){ + currUtime = msg->timestamp; + + // 1. Capture data (or read file) + if (cl_cfg_.process_incoming){ + + // 1.1 Decompress Data: + const uint8_t* depth_data =NULL; + uint8_t* uncompress_buffer = NULL; + int buffer_size = 0; + if(msg->depth.compression != kinect::depth_msg_t::COMPRESSION_NONE) { + if(msg->depth.uncompressed_size > buffer_size) { + buffer_size = msg->depth.uncompressed_size; + uncompress_buffer = (uint8_t*) realloc(uncompress_buffer, buffer_size); + } + unsigned long dlen = msg->depth.uncompressed_size; + int status = uncompress(uncompress_buffer, &dlen, + msg->depth.depth_data.data(), msg->depth.depth_data_nbytes); + if(status != Z_OK) { + return; + } + depth_data =(uint8_t*) uncompress_buffer; + }else{ + buffer_size = msg->depth.depth_data_nbytes; + depth_data = (uint8_t*) msg->depth.depth_data.data(); + } + + if (cl_cfg_.verbose) + std::cout <<"Got kinect: " << currUtime << ", " + <<"Compression: "<< (int) msg->depth.compression << ", Format: " << (int)msg->depth.depth_data_format << ", " + << msg->depth.depth_data_nbytes << "\n"; + + + memcpy(currImage.data, depth_data, buffer_size); + //memcpy(currImage.data, msg->depth.depth_data.data() , msg->depth.depth_data_nbytes); + }else{ + // currUtime = loadDepth(prevImage); + loadDepthNew(currImage); + } + + // 2. Write data back out + // writePngFile(currImage); + // prefilterData(currImage); + // writeRawFile(currImage); + + // 3. Estimate motion: + if (!init_){ + init_ = true; + }else{ + output_counter_++; + // these params should be optimized using test program + // originally i used 128 threads and 96 blocks + int threads = 128; + int blocks = 48; + + Eigen::Vector3f trans_f = currPose.topRightCorner(3, 1).cast (); + Eigen::Matrix rot_f = currPose.topLeftCorner(3, 3).cast (); + if (!cl_cfg_.slow_mode){ + icpOdom->initICPModel((unsigned short *)prevImage.data, 20.0f, currPose.cast () ); + icpOdom->initICP((unsigned short *)currImage.data, 20.0f); + icpOdom->getIncrementalTransformation(trans_f, rot_f, threads, blocks); + }else{ + icpSlowdom->initICPModel((unsigned short *)prevImage.data, 20.0f, currPose.cast () ); + icpSlowdom->initICP((unsigned short *)currImage.data, 20.0f); + icpSlowdom->getIncrementalTransformation(trans_f, rot_f); + } + currPose.topLeftCorner(3, 3) = rot_f.cast (); + currPose.topRightCorner(3, 1) = trans_f.cast (); + } + + // 4. Publish pose back + bot_core::pose_t pose_msg = getPoseAsBotPose( currPose , currUtime, prevPose , prevUtime); + lcm_->publish("POSE_BODY", &pose_msg ); + std::swap(prevImage, currImage); // second copied into first + prevPose = currPose; + prevUtime = currUtime; +} + +int main(int argc, char **argv){ + CommandLineConfig cl_cfg; + cl_cfg.verbose = false; + cl_cfg.process_incoming = true; + cl_cfg.slow_mode = false; // true is use fast mode, else use slow mode + cl_cfg.init_rotated = false; + + ConciseArgs parser(argc, argv, "icpcuda-app"); + parser.add(cl_cfg.verbose, "v", "verbose", "Verbose printf"); + parser.add(cl_cfg.process_incoming, "i", "process_incoming", "process_incoming"); + parser.add(cl_cfg.slow_mode, "s", "slow_mode", "slow_mode (default is fast)"); + parser.add(cl_cfg.init_rotated, "r", "init_rotated", "init_rotated with z forward, x right"); + parser.parse(); + + boost::shared_ptr lcm(new lcm::LCM); + if(!lcm->good()){ + std::cerr <<"ERROR: lcm is not good()" <handle()); +} diff --git a/src/icpcuda-app/icpcuda-app.cpp b/src/icpcuda-app/icpcuda-app.cpp index 0f341e9..59505ec 100644 --- a/src/icpcuda-app/icpcuda-app.cpp +++ b/src/icpcuda-app/icpcuda-app.cpp @@ -9,6 +9,9 @@ #include #include +#include +#include + #include #include @@ -37,6 +40,10 @@ class App{ private: const CommandLineConfig cl_cfg_; boost::shared_ptr lcm_; + + BotParam* botparam_; + BotFrames* botframes_; + //void imagesHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const bot_core::images_t* msg); void kinectHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const kinect::frame_msg_t* msg); @@ -47,13 +54,17 @@ class App{ std::string directory_; - cv::Mat1w prevImage; - cv::Mat1w currImage; + cv::Mat1w prevImage_; + cv::Mat1w currImage_; ICPOdometry* icpOdom; ICPSlowdometry* icpSlowdom; - Eigen::Matrix4d currPose, prevPose; - int64_t prevUtime, currUtime; + Eigen::Matrix4d currLocalToCamera_, prevLocalToCamera_; + int64_t prevUtime_, currUtime_; + Eigen::Isometry3d BodyToCamera_; // Fixed tf from the lidar to the robot's base link + Eigen::Isometry3d worldToBodyInit_; // Captures the position of the body frame in world at launch + Eigen::Isometry3d currWorldToBody_; // running position estimate + Eigen::Isometry3d prevWorldToBody_; void writeRawFile(cv::Mat1w & depth); void prefilterData(cv::Mat1w & depth); @@ -62,12 +73,26 @@ class App{ int output_counter_; + int get_trans_with_utime(BotFrames *bot_frames, + const char *from_frame, const char *to_frame, int64_t utime, + Eigen::Isometry3d & mat); + }; App::App(boost::shared_ptr &lcm_, const CommandLineConfig& cl_cfg_) : lcm_(lcm_), cl_cfg_(cl_cfg_){ + + // Set up frames and config: + botparam_ = bot_param_new_from_server(lcm_->getUnderlyingLCM(), 0); + botframes_= bot_frames_get_global(lcm_->getUnderlyingLCM(), botparam_); + //lcm_->subscribe("CAMERA",&App::imagesHandler,this); + lcm_->subscribe("KINECT_FRAME",&App::kinectHandler,this); + int status = get_trans_with_utime( botframes_ , "KINECT_RGB", "body" , 0, BodyToCamera_); + worldToBodyInit_ = Eigen::Isometry3d::Identity(); + currWorldToBody_ = Eigen::Isometry3d::Identity(); + prevWorldToBody_ = Eigen::Isometry3d::Identity(); cudaDeviceProp prop; cudaGetDeviceProperties(&prop, 0); @@ -86,16 +111,16 @@ App::App(boost::shared_ptr &lcm_, const CommandLineConfig& cl_cfg_) : icpOdom = new ICPOdometry(640, 480, 320, 240, 528, 528); icpSlowdom = new ICPSlowdometry(640, 480, 320, 240, 528, 528); - currImage = cv::Mat1w (480, 640); - currPose = Eigen::Matrix4d::Identity(); + currImage_ = cv::Mat1w (480, 640); + currLocalToCamera_ = Eigen::Matrix4d::Identity(); if (cl_cfg_.init_rotated){ - currPose.topLeftCorner(3, 3) << 0, 0, 1, -1, 0, 0, 0, -1, 0; + currLocalToCamera_.topLeftCorner(3, 3) << 0, 0, 1, -1, 0, 0, 0, -1, 0; } - currUtime = 0; + currUtime_ = 0; - prevImage = cv::Mat1w(480, 640); - prevPose = currPose; - prevUtime = currUtime; + prevImage_ = cv::Mat1w(480, 640); + prevLocalToCamera_ = currLocalToCamera_; + prevUtime_ = currUtime_; init_ = false; output_counter_=0; @@ -113,11 +138,11 @@ void quat_to_euler(Eigen::Quaterniond q, double& roll, double& pitch, double& ya // Difference the transform and scale by elapsed time: -void getTransAsVelocityTrans(Eigen::Matrix4d currPose, int64_t currUtime, Eigen::Matrix4d prevPose, int64_t prevUtime, Eigen::Vector3d &linRate, Eigen::Vector3d &rotRate){ - double elapsed_time = (currUtime - prevUtime)*1E-6; - Eigen::Isometry3d currPoseIso(currPose); - Eigen::Isometry3d prevPoseIso(prevPose); - Eigen::Isometry3d deltaPoseIso = prevPoseIso.inverse()*currPoseIso; +void getTransAsVelocityTrans(Eigen::Matrix4d secondPose, int64_t secondUtime, Eigen::Matrix4d firstPose, int64_t firstUtime, Eigen::Vector3d &linRate, Eigen::Vector3d &rotRate){ + double elapsed_time = (secondUtime - firstUtime)*1E-6; + Eigen::Isometry3d secondPoseIso(secondPose); + Eigen::Isometry3d firstPoseIso(firstPose); + Eigen::Isometry3d deltaPoseIso = firstPoseIso.inverse()*secondPoseIso; linRate = deltaPoseIso.translation()/elapsed_time; Eigen::Quaterniond deltaRotQuat = Eigen::Quaterniond(deltaPoseIso.rotation()); Eigen::Vector3d deltaRot; @@ -131,17 +156,17 @@ void getTransAsVelocityTrans(Eigen::Matrix4d currPose, int64_t currUtime, Eigen: std::cout << "RPY: " << deltaRot.transpose()*180/M_PI <<" deg\n"; std::cout << "RPY: " << rotRate.transpose() <<" rad/s | velocity scaled\n"; std::cout << "RPY: " << rotRate.transpose()*180/M_PI <<" deg/s | velocity scaled\n"; - std::cout << "XYZ " << linRate.transpose() << " m\n"; + std::cout << "XYZ: " << linRate.transpose() << " m/s\n"; } } -bot_core::pose_t getPoseAsBotPose(Eigen::Matrix4d currPose, int64_t currUtime, Eigen::Matrix4d prevPose, int64_t prevUtime){ - Eigen::Vector3d trans_out = currPose.topRightCorner(3, 1); - Eigen::Matrix3d rot_out = currPose.topLeftCorner(3, 3); +bot_core::pose_t getPoseAsBotPose(Eigen::Matrix4d secondPose, int64_t secondUtime, Eigen::Matrix4d firstPose, int64_t firstUtime){ + Eigen::Vector3d trans_out = secondPose.topRightCorner(3, 1); + Eigen::Matrix3d rot_out = secondPose.topLeftCorner(3, 3); Eigen::Quaterniond r_x(rot_out); bot_core::pose_t pose_msg; - pose_msg.utime = currUtime; + pose_msg.utime = secondUtime; pose_msg.pos[0] = trans_out[0]; pose_msg.pos[1] = trans_out[1]; pose_msg.pos[2] = trans_out[2]; @@ -151,7 +176,7 @@ bot_core::pose_t getPoseAsBotPose(Eigen::Matrix4d currPose, int64_t currUtime, E pose_msg.orientation[3] = r_x.z(); Eigen::Vector3d linRate, rotRate; - getTransAsVelocityTrans(currPose, currUtime, prevPose, prevUtime, linRate, rotRate); + getTransAsVelocityTrans(secondPose, secondUtime, firstPose, firstUtime, linRate, rotRate); pose_msg.vel[0] = linRate[0]; pose_msg.vel[1] = linRate[1]; pose_msg.vel[2] = linRate[2]; @@ -162,6 +187,11 @@ bot_core::pose_t getPoseAsBotPose(Eigen::Matrix4d currPose, int64_t currUtime, E return pose_msg; } +bot_core::pose_t getPoseAsBotPose(Eigen::Isometry3d secondPose, int64_t secondUtime, Eigen::Isometry3d firstPose, int64_t firstUtime){ + // TODO: I believe this conversion is correct, but should check: + return getPoseAsBotPose(secondPose.matrix(), secondUtime, firstPose.matrix(), firstUtime); +} + void tokenize(const std::string & str, std::vector & tokens, std::string delimiters = " "){ tokens.clear(); @@ -227,6 +257,22 @@ uint64_t App::loadDepthNew(cv::Mat1w & depth){ } +int App::get_trans_with_utime(BotFrames *bot_frames, + const char *from_frame, const char *to_frame, int64_t utime, + Eigen::Isometry3d & mat){ + int status; + double matx[16]; + status = bot_frames_get_trans_mat_4x4_with_utime( bot_frames, from_frame, to_frame, utime, matx); + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + mat(i,j) = matx[i*4+j]; + } + } + + return status; +} + + void App::writePngFile(cv::Mat1w & depth){ for(unsigned int i = 0; i < 480; i++){ for(unsigned int j = 0; j < 640; j++){ @@ -286,7 +332,7 @@ void App::writeRawFile(cv::Mat1w & depth){ void App::kinectHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const kinect::frame_msg_t* msg){ - currUtime = msg->timestamp; + currUtime_ = msg->timestamp; // 1. Capture data (or read file) if (cl_cfg_.process_incoming){ @@ -313,22 +359,22 @@ void App::kinectHandler(const lcm::ReceiveBuffer* rbuf, } if (cl_cfg_.verbose) - std::cout <<"Got kinect: " << currUtime << ", " + std::cout <<"Got kinect: " << currUtime_ << ", " <<"Compression: "<< (int) msg->depth.compression << ", Format: " << (int)msg->depth.depth_data_format << ", " << msg->depth.depth_data_nbytes << "\n"; - memcpy(currImage.data, depth_data, buffer_size); - //memcpy(currImage.data, msg->depth.depth_data.data() , msg->depth.depth_data_nbytes); + memcpy(currImage_.data, depth_data, buffer_size); + //memcpy(currImage_.data, msg->depth.depth_data.data() , msg->depth.depth_data_nbytes); }else{ - // currUtime = loadDepth(prevImage); - loadDepthNew(currImage); + // currUtime_ = loadDepth(prevImage_); + loadDepthNew(currImage_); } // 2. Write data back out - // writePngFile(currImage); - // prefilterData(currImage); - // writeRawFile(currImage); + // writePngFile(currImage_); + // prefilterData(currImage_); + // writeRawFile(currImage_); // 3. Estimate motion: if (!init_){ @@ -340,27 +386,40 @@ void App::kinectHandler(const lcm::ReceiveBuffer* rbuf, int threads = 128; int blocks = 48; - Eigen::Vector3f trans_f = currPose.topRightCorner(3, 1).cast (); - Eigen::Matrix rot_f = currPose.topLeftCorner(3, 3).cast (); + Eigen::Vector3f trans_f = currLocalToCamera_.topRightCorner(3, 1).cast (); + Eigen::Matrix rot_f = currLocalToCamera_.topLeftCorner(3, 3).cast (); if (!cl_cfg_.slow_mode){ - icpOdom->initICPModel((unsigned short *)prevImage.data, 20.0f, currPose.cast () ); - icpOdom->initICP((unsigned short *)currImage.data, 20.0f); + icpOdom->initICPModel((unsigned short *)prevImage_.data, 20.0f, currLocalToCamera_.cast () ); + icpOdom->initICP((unsigned short *)currImage_.data, 20.0f); icpOdom->getIncrementalTransformation(trans_f, rot_f, threads, blocks); }else{ - icpSlowdom->initICPModel((unsigned short *)prevImage.data, 20.0f, currPose.cast () ); - icpSlowdom->initICP((unsigned short *)currImage.data, 20.0f); + icpSlowdom->initICPModel((unsigned short *)prevImage_.data, 20.0f, currLocalToCamera_.cast () ); + icpSlowdom->initICP((unsigned short *)currImage_.data, 20.0f); icpSlowdom->getIncrementalTransformation(trans_f, rot_f); } - currPose.topLeftCorner(3, 3) = rot_f.cast (); - currPose.topRightCorner(3, 1) = trans_f.cast (); + currLocalToCamera_.topLeftCorner(3, 3) = rot_f.cast (); + currLocalToCamera_.topRightCorner(3, 1) = trans_f.cast (); } - // 4. Publish pose back - bot_core::pose_t pose_msg = getPoseAsBotPose( currPose , currUtime, prevPose , prevUtime); + + + // 2. Determine the body position using the camera position: + Eigen::Isometry3d currLocalToCameraIso = Eigen::Isometry3d( currLocalToCamera_); + Eigen::Isometry3d currWorldToCamera = worldToBodyInit_*BodyToCamera_*currLocalToCameraIso; + currWorldToBody_ = currWorldToCamera * BodyToCamera_.inverse(); + + // 4. Pose of camera in its local coordinate frame: + bot_core::pose_t pose_msg_alt = getPoseAsBotPose( currLocalToCamera_ , currUtime_, prevLocalToCamera_ , prevUtime_); + lcm_->publish("POSE_BODY_ALT", &pose_msg_alt ); + // Pose of the robot in its own frame + bot_core::pose_t pose_msg = getPoseAsBotPose( currWorldToBody_ , currUtime_, prevWorldToBody_ , prevUtime_); lcm_->publish("POSE_BODY", &pose_msg ); - std::swap(prevImage, currImage); // second copied into first - prevPose = currPose; - prevUtime = currUtime; + + // + std::swap(prevImage_, currImage_); // second copied into first + prevWorldToBody_ = currWorldToBody_; + prevLocalToCamera_ = currLocalToCamera_; + prevUtime_ = currUtime_; } int main(int argc, char **argv){