Skip to content

Commit

Permalink
working icp cuda module. results look good. caputed a video with my data
Browse files Browse the repository at this point in the history
  • Loading branch information
Maurice Fallon committed Aug 22, 2015
1 parent 50e75e0 commit 92a3562
Show file tree
Hide file tree
Showing 3 changed files with 144 additions and 61 deletions.
4 changes: 2 additions & 2 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,8 +85,8 @@ pods_install_executables(icpcuda-app-original)


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})
pods_use_pkg_config_packages(se-icpcuda eigen3
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_install_executables(se-icpcuda)
7 changes: 4 additions & 3 deletions src/icpcuda-app/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@ add_definitions(-Wall )
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)
Expand Down Expand Up @@ -53,10 +54,10 @@ 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
/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 icpcuda
lcm
pods_use_pkg_config_packages(se-icpcuda-original
lcm
lcmtypes_bot2-core
)
pods_install_executables(se-icpcuda-original)
Expand Down
194 changes: 138 additions & 56 deletions src/icpcuda-app/icpcuda-app.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@ struct CommandLineConfig
bool verbose;
bool process_incoming;
bool fast_mode;
bool init_rotated;
};

class App{
Expand All @@ -46,63 +47,118 @@ class App{
std::string directory_;


cv::Mat1w firstRaw;
cv::Mat1w secondRaw;
cv::Mat1w prevImage;
cv::Mat1w currImage;
ICPOdometry* icpOdom;
ICPSlowdometry* icpSlowdom;
Eigen::Matrix4f currPose;
Eigen::Matrix4f currPose, prevPose;
int64_t prevUtime, currUtime;


void writeRawFile(cv::Mat1w & depth);
void prefilterData(cv::Mat1w & depth);
void writePngFile(cv::Mat1w & depth);

void writeRawFile(cv::Mat1w & depth);
void prefilterData(cv::Mat1w & depth);
void writePngFile(cv::Mat1w & depth);
int output_counter_;


int output_counter_;
};

App::App(boost::shared_ptr<lcm::LCM> &lcm_, const CommandLineConfig& cl_cfg_) :
lcm_(lcm_), cl_cfg_(cl_cfg_){
lcm_->subscribe("SCAN",&App::imagesHandler,this);
lcm_->subscribe("CAMERA",&App::imagesHandler,this);
lcm_->subscribe("KINECT_FRAME",&App::kinectHandler,this);

directory_ = "/home/mfallon/logs/kinect/rgbd_dataset_freiburg1_desk/";
init_ = false;
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());
}

firstRaw = cv::Mat1w(480, 640);
secondRaw = cv::Mat1w (480, 640);

icpOdom = new ICPOdometry(640, 480, 320, 240, 528, 528);
icpSlowdom = new ICPSlowdometry(640, 480, 320, 240, 528, 528);
icpOdom = new ICPOdometry(640, 480, 320, 240, 528, 528);
icpSlowdom = new ICPSlowdometry(640, 480, 320, 240, 528, 528);

assert(!asFile.eof() && asFile.is_open());
currImage = cv::Mat1w (480, 640);
currPose = Eigen::Matrix4f::Identity();
if (cl_cfg_.init_rotated){
currPose.topLeftCorner(3, 3) << 0, 0, 1, -1, 0, 0, 0, -1, 0;
}
currUtime = 0;

cudaDeviceProp prop;
cudaGetDeviceProperties(&prop, 0);
std::string dev(prop.name);
std::cout << dev << std::endl;
prevImage = cv::Mat1w(480, 640);
prevPose = currPose;
prevUtime = currUtime;

currPose = Eigen::Matrix4f::Identity();
init_ = false;
output_counter_=0;
}

bot_core::pose_t getPoseAsBotPose(Eigen::Isometry3f pose, int64_t utime){
void quat_to_euler(Eigen::Quaternionf q, float& roll, float& pitch, float& yaw) {
const float q0 = q.w();
const float q1 = q.x();
const float q2 = q.y();
const float 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::Matrix4f currPose, int64_t currUtime, Eigen::Matrix4f prevPose, int64_t prevUtime, Eigen::Vector3f &linRate, Eigen::Vector3f &rotRate){
double elapsed_time = (currUtime - prevUtime)*1E-6;
Eigen::Isometry3f currPoseIso(currPose);
Eigen::Isometry3f prevPoseIso(prevPose);
Eigen::Isometry3f deltaPoseIso = prevPoseIso.inverse()*currPoseIso;
linRate = deltaPoseIso.translation()/elapsed_time;
Eigen::Quaternionf deltaRotQuat = Eigen::Quaternionf(deltaPoseIso.rotation());
Eigen::Vector3f 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\n";
}
}

bot_core::pose_t getPoseAsBotPose(Eigen::Matrix4f currPose, int64_t currUtime, Eigen::Matrix4f prevPose, int64_t prevUtime){
Eigen::Vector3f trans_out = currPose.topRightCorner(3, 1);
Eigen::Matrix3f rot_out = currPose.topLeftCorner(3, 3);
Eigen::Quaternionf r_x(rot_out);

bot_core::pose_t pose_msg;
pose_msg.utime = utime;
pose_msg.pos[0] = pose.translation().x();
pose_msg.pos[1] = pose.translation().y();
pose_msg.pos[2] = pose.translation().z();
Eigen::Quaternionf r_x(pose.rotation());
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::Vector3f 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;
}

Expand All @@ -120,6 +176,7 @@ void tokenize(const std::string & str, std::vector<std::string> & tokens, std::s
}
}

// Freiburg Associations file:
uint64_t App::loadDepth(cv::Mat1w & depth){
std::string currentLine;
std::vector<std::string> tokens;
Expand Down Expand Up @@ -152,7 +209,7 @@ uint64_t App::loadDepth(cv::Mat1w & depth){
return time;
}


// Sequence of incrementing png files
uint64_t App::loadDepthNew(cv::Mat1w & depth){
std::stringstream depthLoc;
depthLoc << "./png/" << output_counter_ << ".png";
Expand Down Expand Up @@ -226,71 +283,96 @@ 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){
int64_t timestamp = msg->timestamp;
std::cout <<"Got kinect: " << timestamp << ", "
<< (int) msg->depth.depth_data_format << ", "
<< msg->depth.depth_data_nbytes << "\n";
currUtime = msg->timestamp;

// 1. Capture data (or read file)
if (cl_cfg_.process_incoming){
memcpy(secondRaw.data, msg->depth.depth_data.data() , msg->depth.depth_data_nbytes);

// 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{
// timestamp = loadDepth(firstRaw);
loadDepthNew(secondRaw);
// currUtime = loadDepth(prevImage);
loadDepthNew(currImage);
}

// writePngFile(secondRaw);
// prefilterData(secondRaw);
// writeRawFile(secondRaw);
// 2. Write data back out
// writePngFile(currImage);
// prefilterData(currImage);
// writeRawFile(currImage);

// 3. Estimate motion:
if (!init_){
init_ = true;
}else{
output_counter_++;
int threads = 128;
int threads = 128; // these params should be optimized using test program
int blocks = 96;

Eigen::Vector3f trans = currPose.topRightCorner(3, 1);
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> rot = currPose.topLeftCorner(3, 3);
if (cl_cfg_.fast_mode==1){
icpOdom->initICPModel((unsigned short *)firstRaw.data, 20.0f, currPose);
icpOdom->initICP((unsigned short *)secondRaw.data, 20.0f);
icpOdom->initICPModel((unsigned short *)prevImage.data, 20.0f, currPose);
icpOdom->initICP((unsigned short *)currImage.data, 20.0f);
icpOdom->getIncrementalTransformation(trans, rot, threads, blocks);
}else{
icpSlowdom->initICPModel((unsigned short *)firstRaw.data, 20.0f, currPose);
icpSlowdom->initICP((unsigned short *)secondRaw.data, 20.0f);
icpSlowdom->initICPModel((unsigned short *)prevImage.data, 20.0f, currPose);
icpSlowdom->initICP((unsigned short *)currImage.data, 20.0f);
icpSlowdom->getIncrementalTransformation(trans, rot);
}
currPose.topLeftCorner(3, 3) = rot;
currPose.topRightCorner(3, 1) = trans;
}

std::swap(firstRaw, secondRaw); // second copied into first

Eigen::Vector3f trans_out = currPose.topRightCorner(3, 1);
Eigen::Matrix3f rot_out = currPose.topLeftCorner(3, 3);
Eigen::Quaternionf currentCameraRotation(rot_out);

Eigen::Isometry3f tf_out;
tf_out.setIdentity();
tf_out.translation() << trans_out;
tf_out.rotate(currentCameraRotation);

bot_core::pose_t pose_msg = getPoseAsBotPose( tf_out , timestamp);
// 4. Pu
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.fast_mode = true; // 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.fast_mode, "f", "fast_mode", "fast_mode (else slow)");
parser.add(cl_cfg.init_rotated, "r", "init_rotated", "init_rotated with z forward, x right");
parser.parse();

boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
Expand Down

0 comments on commit 92a3562

Please sign in to comment.