diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 2699104..a47c142 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -70,6 +70,11 @@ struct Feature { position(Eigen::Vector3d::Zero()), is_initialized(false), is_anchored(false) {} + +void Rhocost(const Eigen::Isometry3d& T_c0_ci, + const double x, const Eigen::Vector2d& z1, const Eigen::Vector2d& z2, + double& e) const; + /* * @brief cost Compute the cost of the camera observations * @param T_c0_c1 A rigid body transformation takes @@ -82,6 +87,13 @@ struct Feature { const Eigen::Vector3d& x, const Eigen::Vector2d& z, double& e) const; + bool initializeRho(const CamStateServer& cam_states); + + inline void RhoJacobian(const Eigen::Isometry3d& T_c0_ci, + const double x, const Eigen::Vector2d& z1, const Eigen::Vector2d& z2, + Eigen::Matrix& J, Eigen::Vector2d& r, + double& w) const; + /* * @brief jacobian Compute the Jacobian of the camera observation * @param T_c0_c1 A rigid body transformation takes @@ -97,6 +109,10 @@ struct Feature { Eigen::Matrix& J, Eigen::Vector2d& r, double& w) const; + inline double generateInitialDepth( + const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1, + const Eigen::Vector2d& z2) const; + /* * @brief generateInitialGuess Compute the initial guess of * the feature's 3d position using only two views. @@ -194,7 +210,7 @@ bool MarkerGeneration( const CAMState& cam_state, const StateIDType& cam_state_id, CameraCalibration& cam0, - const std::vector photo_r, + const Eigen::VectorXd& photo_r, std::stringstream& ss, cv::Point2f gradientVector, cv::Point2f residualVector) const; @@ -263,6 +279,26 @@ typedef std::map, Eigen::aligned_allocator< std::pair > > MapServer; +void Feature::Rhocost(const Eigen::Isometry3d& T_c0_ci, + const double x, const Eigen::Vector2d& z1, const Eigen::Vector2d& z2, + double& e) const +{ + // Compute hi1, hi2, and hi3 as Equation (37). + const double& rho = x; + + Eigen::Vector3d h = T_c0_ci.linear()* + Eigen::Vector3d(z1(0), z1(1), 1.0) + rho*T_c0_ci.translation(); + double& h1 = h(0); + double& h2 = h(1); + double& h3 = h(2); + + // Predict the feature observation in ci frame. + Eigen::Vector2d z_hat(h1/h3, h2/h3); + + // Compute the residual. + e = (z_hat-z2).squaredNorm(); + return; +} void Feature::cost(const Eigen::Isometry3d& T_c0_ci, const Eigen::Vector3d& x, const Eigen::Vector2d& z, @@ -275,9 +311,9 @@ void Feature::cost(const Eigen::Isometry3d& T_c0_ci, Eigen::Vector3d h = T_c0_ci.linear()* Eigen::Vector3d(alpha, beta, 1.0) + rho*T_c0_ci.translation(); - double& h1 = h(0); - double& h2 = h(1); - double& h3 = h(2); + double h1 = h(0); + double h2 = h(1); + double h3 = h(2); // Predict the feature observation in ci frame. Eigen::Vector2d z_hat(h1/h3, h2/h3); @@ -287,6 +323,43 @@ void Feature::cost(const Eigen::Isometry3d& T_c0_ci, return; } +void Feature::RhoJacobian(const Eigen::Isometry3d& T_c0_ci, + const double x, const Eigen::Vector2d& z1, const Eigen::Vector2d& z2, + Eigen::Matrix& J, Eigen::Vector2d& r, + double& w) const +{ + + // Compute hi1, hi2, and hi3 as Equation (37). + const double& rho = x; + + Eigen::Vector3d h = T_c0_ci.linear()* + Eigen::Vector3d(z1(0), z2(1), 1.0) + rho*T_c0_ci.translation(); + double& h1 = h(0); + double& h2 = h(1); + double& h3 = h(2); + + // Compute the Jacobian. + Eigen::Matrix3d W; + W.leftCols<2>() = T_c0_ci.linear().leftCols<2>(); + W.rightCols<1>() = T_c0_ci.translation(); + + J(0,0) = -h1/(h3*h3); + J(1,0) = -h2/(h3*h3); + + // Compute the residual. + Eigen::Vector2d z_hat(h1/h3, h2/h3); + r = z_hat - z2; + + // Compute the weight based on the residual. + double e = r.norm(); + if (e <= optimization_config.huber_epsilon) + w = 1.0; + else + w = optimization_config.huber_epsilon / (2*e); + + return; +} + void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci, const Eigen::Vector3d& x, const Eigen::Vector2d& z, Eigen::Matrix& J, Eigen::Vector2d& r, @@ -326,9 +399,9 @@ void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci, return; } -void Feature::generateInitialGuess( +double Feature::generateInitialDepth( const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1, - const Eigen::Vector2d& z2, Eigen::Vector3d& p) const + const Eigen::Vector2d& z2) const { // Construct a least square problem to solve the depth. Eigen::Vector3d m = T_c1_c2.linear() * Eigen::Vector3d(z1(0), z1(1), 1.0); @@ -343,6 +416,15 @@ void Feature::generateInitialGuess( // Solve for the depth. double depth = (A.transpose() * A).inverse() * A.transpose() * b; + return depth; +} + + +void Feature::generateInitialGuess( + const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1, + const Eigen::Vector2d& z2, Eigen::Vector3d& p) const +{ + double depth = generateInitialDepth(T_c1_c2, z1, z2); p(0) = z1(0) * depth; p(1) = z1(1) * depth; p(2) = depth; @@ -405,16 +487,11 @@ else if(type == "Sobel_y") double delta = 0; int offs = (int)(kernel.rows()-1)/2; -for(int i = 0; i < kernel.rows(); i++){ +for(int i = 0; i < kernel.rows(); i++) for(int j = 0; j < kernel.cols(); j++) - { - std::cout << "i: " << i << ":" << "j: " << j << ":" << kernel(i,j) << std::endl; - std::cout <<"pose: " << pose.y+i-offs << " : " << pose.x+j-offs << std::endl; - delta += ((float)frame.at(pose.y+i-offs , pose.x+j-offs))/255 * (float)kernel(i,j); - } -} -std::cout << "delta " << delta << std::endl; + delta += ((float)frame.at(pose.y+j-offs , pose.x+i-offs))/255 * (float)kernel(j,i); + return delta; } bool Feature::estimate_FrameIrradiance( @@ -572,7 +649,7 @@ bool Feature::VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, CameraCalibration& cam0, - const std::vector photo_r, + const Eigen::VectorXd& photo_r, std::stringstream& ss, cv::Point2f gradientVector, cv::Point2f residualVector) const @@ -685,17 +762,17 @@ bool Feature::VisualizePatch( for(int i = 0; i0) + if(photo_r(i*N+j)>0) cv::rectangle(irradianceFrame, cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)), - cv::Scalar(255 - photo_r[i*N+j]*255, 255 - photo_r[i*N+j]*255, 255), + cv::Scalar(255 - photo_r(i*N+j)*255, 255 - photo_r(i*N+j)*255, 255), CV_FILLED); else cv::rectangle(irradianceFrame, cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)), - cv::Scalar(255, 255 + photo_r[i*N+j]*255, 255 + photo_r[i*N+j]*255), + cv::Scalar(255, 255 + photo_r(i*N+j)*255, 255 + photo_r(i*N+j)*255), CV_FILLED); // gradient arrow @@ -911,9 +988,160 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) return true; } +bool Feature::initializeRho(const CamStateServer& cam_states) { + + // Organize camera poses and feature observations properly. + std::vector > cam_poses(0); + std::vector > measurements(0); + + for (auto& m : observations) { + auto cam_state_iter = cam_states.find(m.first); + if (cam_state_iter == cam_states.end()) continue; + + // Add the measurement. + measurements.push_back(m.second.head<2>()); + measurements.push_back(m.second.tail<2>()); + + // This camera pose will take a vector from this camera frame + // to the world frame. + Eigen::Isometry3d cam0_pose; + cam0_pose.linear() = quaternionToRotation( + cam_state_iter->second.orientation).transpose(); + cam0_pose.translation() = cam_state_iter->second.position; + + Eigen::Isometry3d cam1_pose; + cam1_pose = cam0_pose * CAMState::T_cam0_cam1.inverse(); + + cam_poses.push_back(cam0_pose); + cam_poses.push_back(cam1_pose); + } + + // All camera poses should be modified such that it takes a + // vector from the first camera frame in the buffer to this + // camera frame. + Eigen::Isometry3d T_c0_w = cam_poses[0]; + T_anchor_w = T_c0_w; + for (auto& pose : cam_poses) + pose = pose.inverse() * T_c0_w; + + // Generate initial guess + double initial_depth = 0; + initial_depth = generateInitialDepth(cam_poses[cam_poses.size()-1], measurements[0], + measurements[measurements.size()-1]); + + double solution = 1.0/initial_depth; + + // Apply Levenberg-Marquart method to solve for the 3d position. + double lambda = optimization_config.initial_damping; + int inner_loop_cntr = 0; + int outer_loop_cntr = 0; + bool is_cost_reduced = false; + double delta_norm = 0; + + // Compute the initial cost. + double total_cost = 0.0; + for (int i = 0; i < cam_poses.size(); ++i) { + double this_cost = 0.0; + Rhocost(cam_poses[i], solution, measurements[0], measurements[i], this_cost); + total_cost += this_cost; + } + + // Outer loop. + do { + Eigen::Matrix A = Eigen::Matrix::Zero(); + Eigen::Matrix b = Eigen::Matrix::Zero(); + + for (int i = 0; i < cam_poses.size(); ++i) { + Eigen::Matrix J; + Eigen::Vector2d r; + double w; + + RhoJacobian(cam_poses[i], solution, measurements[0], measurements[i], J, r, w); + + if (w == 1) { + A += J.transpose() * J; + b += J.transpose() * r; + } else { + double w_square = w * w; + A += w_square * J.transpose() * J; + b += w_square * J.transpose() * r; + } + } + + // Inner loop. + // Solve for the delta that can reduce the total cost. + do { + Eigen::Matrix damper = lambda*Eigen::Matrix::Identity(); + Eigen::Matrix delta = (A+damper).ldlt().solve(b); + double new_solution = solution - delta(0,0); + delta_norm = delta.norm(); + + double new_cost = 0.0; + for (int i = 0; i < cam_poses.size(); ++i) { + double this_cost = 0.0; + Rhocost(cam_poses[i], new_solution, measurements[0], measurements[i], this_cost); + new_cost += this_cost; + } + + if (new_cost < total_cost) { + is_cost_reduced = true; + solution = new_solution; + total_cost = new_cost; + lambda = lambda/10 > 1e-10 ? lambda/10 : 1e-10; + } else { + is_cost_reduced = false; + lambda = lambda*10 < 1e12 ? lambda*10 : 1e12; + } + + } while (inner_loop_cntr++ < + optimization_config.inner_loop_max_iteration && !is_cost_reduced); + + inner_loop_cntr = 0; + + } while (outer_loop_cntr++ < + optimization_config.outer_loop_max_iteration && + delta_norm > optimization_config.estimation_precision); + + // Covert the feature position from inverse depth + // representation to its 3d coordinate. + Eigen::Vector3d final_position(measurements[0](0)/solution, + measurements[0](1)/solution, 1.0/solution); + + // Check if the solution is valid. Make sure the feature + // is in front of every camera frame observing it. + bool is_valid_solution = true; + for (const auto& pose : cam_poses) { + Eigen::Vector3d position = + pose.linear()*final_position + pose.translation(); + if (position(2) <= 0) { + is_valid_solution = false; + break; + } + } + + //save inverse depth distance from camera + anchor_rho = solution; + + // Convert the feature position to the world frame. + position = T_c0_w.linear()*final_position + T_c0_w.translation(); + + if (is_valid_solution) + is_initialized = true; + + return is_valid_solution; +} + + bool Feature::initializePosition(const CamStateServer& cam_states) { // Organize camera poses and feature observations properly. + + +initializeRho(cam_states); +std::cout << "init rho is: " << anchor_rho << std::endl; + std::vector > cam_poses(0); std::vector - + - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 203603b..4a03598 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1240,9 +1240,7 @@ void MsckfVio::PhotometricMeasurementJacobian( //photometric observation std::vector photo_z; - - std::vector photo_r; - + VectorXd r_photo = VectorXd::Zero(N*N); // individual Jacobians Matrix dI_dhj = Matrix::Zero(); Matrix dh_dCpij = Matrix::Zero(); @@ -1303,8 +1301,7 @@ void MsckfVio::PhotometricMeasurementJacobian( photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); //calculate photom. residual - photo_r.push_back((photo_z[count] - estimate_photo_z[count])); - + r_photo(count) = photo_z[count] - estimate_photo_z[count]; //cout << "residual: " << photo_r.back() << endl; // add jacobians @@ -1325,9 +1322,10 @@ void MsckfVio::PhotometricMeasurementJacobian( gradientVector.x += dx; gradientVector.y += dy; - residualVector.x += dx * photo_r[count]; - residualVector.y += dy * photo_r[count]; - res_sum += photo_r[count]; + + residualVector.x += dx * r_photo(count); + residualVector.y += dy * r_photo(count); + res_sum += r_photo(count); //dh / d{}^Cp_{ij} dh_dCpij(0, 0) = 1 / p_c0(2); @@ -1348,7 +1346,6 @@ void MsckfVio::PhotometricMeasurementJacobian( // d{}^Gp_P{ij} / \rho_i double rho = feature.anchor_rho; - // Isometry T_anchor_w takes a vector in anchor frame to world frame dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho*rho), feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho)); @@ -1363,6 +1360,12 @@ void MsckfVio::PhotometricMeasurementJacobian( H_plj = dI_dhj * dh_dXplj; // 1 x 6 H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 + /* + cout << endl; + std::cout << H_plj << endl; + cout << r_photo.segment(count, 1) << endl; + std::cout << H_plj.colPivHouseholderQr().solve(r_photo.segment(count, 1)) << std::endl; + */ H_rho.block<1, 1>(count, 0) = H_rhoj; H_pl.block<1, 6>(count, 0) = H_plj; H_pA.block<1, 6>(count, 0) = H_pAj; @@ -1375,6 +1378,14 @@ void MsckfVio::PhotometricMeasurementJacobian( count++; } + /* + std::cout << "\n\n frame change through patch" << std::endl; + std::cout << H_pl << std::endl; + std::cout << r_photo << std::endl; + std::cout << endl; + std::cout << H_pl.colPivHouseholderQr().solve(r_photo) << std::endl; + */ + MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1); @@ -1408,16 +1419,14 @@ void MsckfVio::PhotometricMeasurementJacobian( H_y = H_yl; //TODO make this more fluent as well - count = 0; - for(auto data : photo_r) - r[count++] = data; + r = r_photo; std::stringstream ss; ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; if(PRINTIMAGES) { feature.MarkerGeneration(marker_pub, state_server.cam_states); - feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss, gradientVector, residualVector); + //feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector); } return; @@ -1463,6 +1472,8 @@ void MsckfVio::PhotometricFeatureJacobian( if (feature.observations.find(cam_id) == feature.observations.end()) continue; + if (feature.observations.find(cam_id) == feature.observations.begin()) + continue; valid_cam_state_ids.push_back(cam_id); } @@ -1519,6 +1530,7 @@ void MsckfVio::PhotometricFeatureJacobian( { ofstream myfile; myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); + myfile << "Hxi\n" << H_xi << "ri\n" << r_i << "Hyi\n" << H_yi << endl; myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; myfile.close(); cout << "---------- LOGGED -------- " << endl; @@ -1601,6 +1613,13 @@ void MsckfVio::featureJacobian( const std::vector& cam_state_ids, MatrixXd& H_x, VectorXd& r) { + // stop playing bagfile if printing images + if(PRINTIMAGES) + { + std::cout << "stopped playpack" << std::endl; + nh.setParam("/play_bag", false); + } + const auto& feature = map_server[feature_id]; @@ -1662,12 +1681,20 @@ void MsckfVio::featureJacobian( H_x = A.transpose() * H_xj; r = A.transpose() * r_j; - - ofstream myfile; - myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); - myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; - myfile.close(); - cout << "---------- LOGGED -------- " << endl; + + // stop playing bagfile if printing images + if(PRINTIMAGES) + { + + ofstream myfile; + myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); + myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; + myfile.close(); + cout << "---------- LOGGED -------- " << endl; + std::cout << "stopped playpack" << std::endl; + nh.setParam("/play_bag", true); + } + return; }