bool FactorGraph::add_virtual_pose_point(virtual_pose3_point3_t virtual_pose_point){
int id = virtual_pose_point.id;
if (DEBUG) cout << "in FactorGraph trying to add virtual pose point at time: " << endl << "t = " << virtual_pose_point.t << endl ;
Pose3dTS_Node* pose = find_pose_from_time_and_id(virtual_pose_point.t, id);
if (pose == NULL){
cout << "[warning] {add_virtual_pose_point}: no pose for virtual_pose_point measurement at time " << virtual_pose_point.t << " for id " << id << "skipping.. " << endl;
return false;
}
// TODO there is also data association going on here (currently done with ids):
Point3dID_Node* point = find_point_from_feature_id(virtual_pose_point.feature_id);
if(point == NULL)
point = build_point(virtual_pose_point.feature_id);
MatrixXd mu,omega;
conversions::lcm2eigen(virtual_pose_point.point,mu,omega);
Point3d measure(mu);
Noise inf = Information(omega);
Virtual_Pose3d_Point3d_Factor* virtual_factor =
new Virtual_Pose3d_Point3d_Factor(dynamic_cast<Pose3d_Node*>(pose),
dynamic_cast<Point3d_Node*>(point),
measure,
inf);
if (DEBUG) cout << "pose point factors: " << *virtual_factor << endl;
_slam.add_factor(virtual_factor);
if(_lcm_viewer)
_virtual_collection->add(_virtual_link_count++,6,pose->unique_id(),3,point->unique_id());
return true;
}
void add_virtual_obstacles(){
int dis_Y = Y1 - Y0;
int dis_X = X1 - X0;
int interval_Y = dis_Y / (VIRTUAL_NUMBER - 1);
int interval_X = dis_X / (VIRTUAL_NUMBER - 1);
printf("-------------------print the virtual obstacles---------------\n");
int i=1;
for (; i<VIRTUAL_NUMBER-1 ; i++){
int p_x = X0 + i*interval_X;
point_t p_with_Y0 = build_point(p_x, Y0);
point_t p_with_Y1 = build_point(p_x, Y1);
print_point(&p_with_Y0);
print_point(&p_with_Y1);
push_point(p_with_Y0);
push_point(p_with_Y1);
}
i = 1;
for(; i<VIRTUAL_NUMBER-1; i++){
int p_y = Y0 + i*interval_Y;
point_t p_with_X0 = build_point(X0, p_y);
point_t p_with_X1 = build_point(X1, p_y);
print_point(&p_with_X0);
print_point(&p_with_X1);
push_point(p_with_X0);
push_point(p_with_X1);
}
point_t X0Y0 = build_point(X0, Y0);
point_t X0Y1 = build_point(X0, Y1);
point_t X1Y0 = build_point(X1, Y0);
point_t X1Y1 = build_point(X1, Y1);
push_point(X0Y0);
push_point(X0Y1);
push_point(X1Y0);
push_point(X1Y1);
print_point(&X0Y0);
print_point(&X0Y1);
print_point(&X1Y0);
print_point(&X1Y1);
printf("----------------virtual obstacles---------------\n");
}
bool FactorGraph::add_sidescan_detection(sidescan_detection_t detection){
if (_poses[detection.id].size()==0){
cout << "Rejecting odometry since no prior" << endl;
return false;
}
Pose3dTS_Node* pose = find_pose_from_time_and_id(detection.t,detection.id);
if (pose == NULL){ // times might not be exact pick the closest one.
cout << "No exact time match find closest" << endl;
pose = find_closest_pose_from_time_and_id(detection.t,detection.id);
}
if (pose == NULL) cout << "Error unable to find pose for time = " << detection.t << " and id " << detection.id << endl;
// TODO: decide if it's a detection of previously tracked target. EG do something like nearest neighbor or JCBB. Start with using feature id
Point3dID_Node* point = find_point_from_feature_id(detection.feature_id);
if (point == NULL)
point = build_point(detection.feature_id);
// here's the unique thing for a sidescan detection. It is always orthogonal to direction of motion. Therefore take the measurement and put it into "y" direction
MatrixXd omega;
conversions::lcm2eigen(detection,omega);
Point3d measure(0.0,detection.mu,0.0); // TODO deal with depth/z, this sidescan detection should be
// its own factor not use Pose3d_Point3d_Factor
Noise inf = Information(omega);
Pose3d_Point3d_Factor* detection_factor =
new Pose3d_Point3d_Factor(dynamic_cast<Pose3d_Node*>(pose),
dynamic_cast<Point3d_Node*>(point),
measure,
inf);
if (DEBUG){
cout << "Adding sidescan detection factor: " << *detection_factor << endl;
cout << "Between pose with id = " << detection.id << " and time t = " << detection.t << "and point with feature id " << detection.feature_id << endl;
}
_slam.add_factor(detection_factor);
if(_lcm_viewer)
_feature_collection->add(_feature_link_count++,2,pose->unique_id(),3,point->unique_id());
return true;
}
