Added handling for cost maps; in progress. Added readCostMap,
writeCostMap, modified SAD/NCC/SNCC for cost maps, projected costs to
frames in projectPoints / projectForward.